Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
tutorials:intermediate:simple_mobile_manipulation_plan [2019/08/02 14:31] – [Some Useful Designators] Added more information about the object designator amartutorials:intermediate:simple_mobile_manipulation_plan [2022/04/25 09:20] (current) – schimpf
Line 1: Line 1:
-**//Tested with Cram v0.7.0, ROS version: Kinetic, Ubuntu 16.04//**+**//Tested with Cram v0.8.0, ROS version: Noetic, Ubuntu 20.04//**
  
 ====== Simple Mobile Manipulation Plan ====== ====== Simple Mobile Manipulation Plan ======
Line 88: Line 88:
   (unless (assoc :bottle btr::*mesh-files*)   (unless (assoc :bottle btr::*mesh-files*)
     (add-objects-to-mesh-list))     (add-objects-to-mesh-list))
-  (btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-1.6 -0.9 0.82) (0 0 0 1)))+  (btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-1.6 -1 0.82) (0 0 0 1)))
   (btr:simulate btr:*current-bullet-world* 10))   (btr:simulate btr:*current-bullet-world* 10))
 BTW-TUT> (spawn-bottle) BTW-TUT> (spawn-bottle)
Line 97: Line 97:
 <code lisp> <code lisp>
 BTW-TUT> BTW-TUT>
-(defparameter *final-object-destination*+ (defparameter *final-object-destination*
   (cl-transforms-stamped:make-pose-stamped   (cl-transforms-stamped:make-pose-stamped
    "map" 0.0    "map" 0.0
    (cl-transforms:make-3d-vector -0.8 2 0.9)    (cl-transforms:make-3d-vector -0.8 2 0.9)
-   (cl-transforms:make-identity-rotation)))+   (cl-transforms:make-quaternion 0.0d0 0.0d0 -1.0d0 0.0d0)))
  
 (defparameter *base-pose-near-table* (defparameter *base-pose-near-table*
Line 135: Line 135:
 (defun move-bottle () (defun move-bottle ()
   (spawn-bottle)   (spawn-bottle)
-  (pr2-proj:with-simulated-robot+  (urdf-proj:with-simulated-robot
     (let ((?navigation-goal *base-pose-near-table*))     (let ((?navigation-goal *base-pose-near-table*))
       (cpl:par       (cpl:par
Line 143: Line 143:
         (pp-plans::park-arms)         (pp-plans::park-arms)
         ;; Moving the robot near the table.         ;; Moving the robot near the table.
-        (exe:perform (desig:a motion+        (exe:perform (desig:an action
                               (type going)                               (type going)
                               (target (desig:a location                                (target (desig:a location 
Line 149: Line 149:
     ;; Looking towards the bottle before perceiving.     ;; Looking towards the bottle before perceiving.
     (let ((?looking-direction *downward-look-coordinate*))     (let ((?looking-direction *downward-look-coordinate*))
-      (exe:perform (desig:a motion +      (exe:perform (desig:an action 
                             (type looking)                             (type looking)
                             (target (desig:a location                              (target (desig:a location 
Line 168: Line 168:
       ;; Moving the robot near the counter.       ;; Moving the robot near the counter.
       (let ((?nav-goal *base-pose-near-counter*))       (let ((?nav-goal *base-pose-near-counter*))
-        (exe:perform (desig:a motion+        (exe:perform (desig:an action
                               (type going)                               (type going)
                               (target (desig:a location                                (target (desig:a location 
Line 184: Line 184:
 </code> </code>
  
-Note that the plan is nested under ''pr2-proj:with-simulated-robot'' indicating that all the methods resolved by the designators is being called for the projection environment (You can see these low level methods being called under the ''cram_pr2_projections'' package under ''low-level.lisp''). If ''pr2-pms:with-real-robot'' is used to replace this, the functions from the real robot ROS interfaces will be called. Also note that it is possible to execute actions which are independent in parallel (In this case, moving the robot torso up and moving the robot base are independent actions done in parallel)+Note that the plan is nested under ''urdf-proj:with-simulated-robot'' indicating that all the methods resolved by the designators is being called for the projection environment (You can see these low level methods being called under the ''cram_urdf_projections'' package under ''low-level.lisp''). If ''pr2-pms:with-real-robot'' is used to replace this, the functions from the real robot ROS interfaces will be called. Also note that it is possible to execute actions which are independent in parallel (In this case, moving the robot torso up and moving the robot base are independent actions done in parallel)
  
 Now run ''(move-bottle)'' and you will see that the robot successfully picks the bottle and places it on the counter. Now run ''(move-bottle)'' and you will see that the robot successfully picks the bottle and places it on the counter.
Line 198: Line 198:
   (unless (assoc :bottle btr::*mesh-files*)   (unless (assoc :bottle btr::*mesh-files*)
     (add-objects-to-mesh-list))     (add-objects-to-mesh-list))
-  (btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-2 -0.9 0.860) (0 0 0 1)))+  (btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-2 -1 0.82) (0 0 0 1)))
   (btr:simulate btr:*current-bullet-world* 10))   (btr:simulate btr:*current-bullet-world* 10))
 </code> </code>
Line 246: Line 246:
       (setf preferred-arm :LEFT))       (setf preferred-arm :LEFT))
     preferred-arm))     preferred-arm))
-    + 
 (defun find-object (?object-type) (defun find-object (?object-type)
   (let* ((possible-look-directions `(,*downward-look-coordinate*   (let* ((possible-look-directions `(,*downward-look-coordinate*
Line 253: Line 253:
          (?looking-direction (first possible-look-directions)))          (?looking-direction (first possible-look-directions)))
     (setf possible-look-directions (cdr possible-look-directions))     (setf possible-look-directions (cdr possible-look-directions))
-    (exe:perform (desig:a motion +    (exe:perform (desig:an action
                           (type looking)                           (type looking)
                           (target (desig:a location                            (target (desig:a location 
Line 263: Line 263:
            (when possible-look-directions            (when possible-look-directions
              (roslisp:ros-warn (perception-failure) "~a~%Turning head." e)              (roslisp:ros-warn (perception-failure) "~a~%Turning head." e)
-             (exe:perform (desig:a motion +             (exe:perform (desig:an action
                                    (type looking)                                     (type looking) 
                                    (direction forward)))                                    (direction forward)))
              (setf ?looking-direction (first possible-look-directions))              (setf ?looking-direction (first possible-look-directions))
              (setf possible-look-directions (cdr possible-look-directions))              (setf possible-look-directions (cdr possible-look-directions))
-             (exe:perform (desig:a motion +             (exe:perform (desig:an action 
                                    (type looking)                                    (type looking)
                                    (target (desig:a location                                    (target (desig:a location
Line 286: Line 286:
 Let us also update our ''move-bottle'' to use this method. Let us also update our ''move-bottle'' to use this method.
 <code lisp> <code lisp>
-(defun move-bottle ()+ (defun move-bottle ()
   (spawn-bottle)   (spawn-bottle)
-  (pr2-proj:with-simulated-robot+  (urdf-proj:with-simulated-robot
     (let ((?navigation-goal *base-pose-near-table*))     (let ((?navigation-goal *base-pose-near-table*))
       (cpl:par       (cpl:par
Line 296: Line 296:
         (pp-plans::park-arms)         (pp-plans::park-arms)
         ;; Moving the robot near the table.         ;; Moving the robot near the table.
-        (exe:perform (desig:a motion+        (exe:perform (desig:an action
                               (type going)                               (type going)
                               (target (desig:a location                                (target (desig:a location 
Line 311: Line 311:
       ;; Moving the robot near the counter.       ;; Moving the robot near the counter.
       (let ((?nav-goal *base-pose-near-counter*))       (let ((?nav-goal *base-pose-near-counter*))
-        (exe:perform (desig:a motion+        (exe:perform (desig:an action
                               (type going)                               (type going)
                               (target (desig:a location                                (target (desig:a location 
                                                (pose ?nav-goal))))))                                                (pose ?nav-goal))))))
 + 
       (coe:on-event (make-instance 'cpoe:robot-state-changed))       (coe:on-event (make-instance 'cpoe:robot-state-changed))
       ;; Setting the object down on the counter       ;; Setting the object down on the counter
Line 326: Line 326:
                                                 (pose ?drop-pose))))))                                                 (pose ?drop-pose))))))
       (pp-plans::park-arms :arm ?grasping-arm))))       (pp-plans::park-arms :arm ?grasping-arm))))
 +
 </code> </code>
  
Line 334: Line 335:
 Everything is good so far, but let's call this a lucky coincidence. For the robot, knowing which arm to use to pick up the bottle is not always enough. There are many positions with which we can grasp objects - from the object's front, back, left, right, etc. One might think that since the bottle is a rotationally symmetric object, it doesn't matter which side you approach from. But consider the bottle as an object model, which has a specific front, back and sides according to its orientation with respect to the bullet world. In which case, the side with which the arm approaches the bottle greatly matters, accounting for the configuration of the joints the robot arm will make while trying to grasp - some poses may be unachievable, while others may result in the collision of the arm with the table. Everything is good so far, but let's call this a lucky coincidence. For the robot, knowing which arm to use to pick up the bottle is not always enough. There are many positions with which we can grasp objects - from the object's front, back, left, right, etc. One might think that since the bottle is a rotationally symmetric object, it doesn't matter which side you approach from. But consider the bottle as an object model, which has a specific front, back and sides according to its orientation with respect to the bullet world. In which case, the side with which the arm approaches the bottle greatly matters, accounting for the configuration of the joints the robot arm will make while trying to grasp - some poses may be unachievable, while others may result in the collision of the arm with the table.
  
-Let's try to visualize this issue, by spawning the bottle in yet another position:+Let's try to visualize this issue, by spawning the bottle in yet another position and changing the grasp position slightly:
 <code lisp> <code lisp>
-(defun spawn-bottle ()+(defparameter *base-pose-near-table* 
 +  (cl-transforms-stamped:make-pose-stamped 
 +   "map" 0.0 
 +   (cl-transforms:make-3d-vector -2.447d0 -0.150d0 0.0d0) 
 +   (cl-transforms:axis-angle->quaternion (cl-transforms:make-3d-vector 0 0 1) (/ pi -2)))) 
 + 
 + (defun spawn-bottle ()
   (unless (assoc :bottle btr::*mesh-files*)   (unless (assoc :bottle btr::*mesh-files*)
     (add-objects-to-mesh-list))     (add-objects-to-mesh-list))
-  (btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-1.1 -0.75 0.860) (0 0 0 1)))+  (btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-1.8 -1 0.860) (0 0 0 1)))
   (btr:simulate btr:*current-bullet-world* 10))   (btr:simulate btr:*current-bullet-world* 10))
 </code> </code>
-Now run ''move-bottle'' once more, and the output should be something similar to as below:+Now run ''move-bottle'' once more, and and the output should look like this [Some messages that would come up are suppressed here for readability.] 
 <code lisp> <code lisp>
 BTW-TUT> (move-bottle) BTW-TUT> (move-bottle)
-[(PICK-PLACE PICK-UP) INFO] 1550502686.470: Opening gripper + 
-[(PICK-PLACE PICK-UP) INFO] 1550502686.470: Reaching +[(PICK-PLACE PICK-UP) INFO] 1649925566.714: Looking 
-[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) WARN] 1550502686.797: #<POSE-STAMPED +[(PICK-PLACE PICK-UP) INFO] 1649925566.758: Opening gripper and reaching 
 +[(URDF-PROJ MOVE-TCP) ERROR] 1649925567.115: #<POSE-STAMPED  
 +   FRAME-ID: "torso_lift_link", STAMP: 0.0 
 +   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.1556749380156398d0)> 
 +   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision. 
 +Failing. 
 +[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) WARN] 1649925567.119: #<POSE-STAMPED 
    FRAME-ID: "torso_lift_link", STAMP: 0.0    FRAME-ID: "torso_lift_link", STAMP: 0.0
-   #<3D-VECTOR (0.6819813024319948d0 0.4206671881870251d0 -0.11278482277792945d0)> +   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.1556749380156398d0)> 
-   #<QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>> is unreachable for EE.+   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision.
 Ignoring. Ignoring.
-[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1550502687.092: #<POSE-STAMPED +[(PERFORM MOTION) INFO] 1649925567.125:  
 +#<A MOTION 
 +    (TYPE MOVING-TCP) 
 +    (RIGHT-POSE #<POSE-STAMPED  
 +   FRAME-ID: "map", STAMP: 0.0 
 +   #<3D-VECTOR (-1.7999999507109004d0 -0.8699999493007575d0 0.8650000342205167d0)> 
 +   #<QUATERNION (0.0d0 0.0d0 -0.7071067690849304d0 0.7071067690849304d0)>>) 
 +    (COLLISION-MODE ALLOW-ALL) 
 +    (MOVE-BASE T)> 
 +[(URDF-PROJ MOVE-TCP) ERROR] 1649925567.483: #<POSE-STAMPED 
    FRAME-ID: "torso_lift_link", STAMP: 0.0    FRAME-ID: "torso_lift_link", STAMP: 0.0
-   #<3D-VECTOR (0.6797228574484719d0 0.4210222500057509d0 -0.2627674055849005d0)> +   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.22567493831366303d0)> 
-   #<QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>> is unreachable for EE.+   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision.
 Failing. Failing.
-[(PP-PLANS PICK-UP) WARN] 1550502687.092: Manipulation messed up: #<POSE-STAMPED +[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1649925567.484: #<POSE-STAMPED 
    FRAME-ID: "torso_lift_link", STAMP: 0.0    FRAME-ID: "torso_lift_link", STAMP: 0.0
-   #<3D-VECTOR (0.6797228574484719d0 0.4210222500057509d0 -0.2627674055849005d0)> +   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.22567493831366303d0)> 
-   #<QUATERNION (-0.005172943672216379d0 0.0055962335340426494d0 -0.7845776913102387d0 0.6199836767360266d0)>> is unreachable for EE.+   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision. 
 +Failing. 
 +[(PP-PLANS PICK-UP) WARN] 1649925567.484: Manipulation messed up: #<POSE-STAMPED  
 +   FRAME-ID: "torso_lift_link", STAMP: 0.0 
 +   #<3D-VECTOR (0.5899999463583037d0 0.6470002075389858d0 -0.22567493831366303d0)> 
 +   #<QUATERNION (0.0d0 0.0d0 0.0d0 1.0d0)>> is unreachable for EE or is in collision.
 Ignoring. Ignoring.
-; Evaluation aborted on #<CRAM-COMMON-FAILURES:MANIPULATION-POSE-UNREACHABLE {10100E64C3}>.+[(PICK-PLACE PICK-UP) INFO] 1649925567.484: Grasping 
 +[(PICK-PLACE PICK-UP) INFO] 1649925567.845: Gripping 
 +[(PICK-AND-PLACE GRIP) WARN] 1649925567.875: There was no object to grip 
 +Retrying 
 +[(PICK-AND-PLACE GRIP) WARN] 1649925567.888: No retries left. Propagating up. 
 +; Evaluation aborted on #<CRAM-COMMON-FAILURES:GRIPPER-CLOSED-COMPLETELY {10058EA443}>. 
 </code> </code>
  
Line 408: Line 441:
 Let's encapsulate all this in a method called ''pick-up-object'': Let's encapsulate all this in a method called ''pick-up-object'':
 <code lisp> <code lisp>
-(defun pick-up-object (?perceived-object ?object-type ?grasping-arm)+ (defun pick-up-object (?perceived-object ?object-type ?grasping-arm)
   (let ((?possible-arms '(:right :left)))   (let ((?possible-arms '(:right :left)))
     ;;Retry by changing the arm     ;;Retry by changing the arm
Line 419: Line 452:
                  (cpl:retry))                  (cpl:retry))
                (roslisp:ros-warn (arm-failures) "No more retries left")))                (roslisp:ros-warn (arm-failures) "No more retries left")))
-             + 
           ;; Retry by changing the grasp           ;; Retry by changing the grasp
           (let* ((?possible-grasp           (let* ((?possible-grasp
-                   (cram-object-interfaces:get-object-type-grasps ?object-type nil nil nil ?grasping-arm))+                   (cram-manipulation-interfaces:get-action-grasps ?object-type ?grasping-arm ?perceived-object))
                  (?grasp (cut:lazy-car ?possible-grasp)))                  (?grasp (cut:lazy-car ?possible-grasp)))
             (cpl:with-retry-counters ((grasp-retries 3))             (cpl:with-retry-counters ((grasp-retries 3))
Line 449: Line 482:
                                        (object ?perceived-object)))))))))                                        (object ?perceived-object)))))))))
   ?grasping-arm)   ?grasping-arm)
 +
 </code> </code>
 With this, the ''pick-up-object'' can now iterate through all possible grasp configurations stored in ''?possible-grasp''. The ''cram-object-interaces:get-object-type-grasps'' is a useful method which will give these values as a lazy list, provided the grasping arm and the object type. With this, the ''pick-up-object'' can now iterate through all possible grasp configurations stored in ''?possible-grasp''. The ''cram-object-interaces:get-object-type-grasps'' is a useful method which will give these values as a lazy list, provided the grasping arm and the object type.
Line 460: Line 494:
 (defun move-bottle () (defun move-bottle ()
   (spawn-bottle)   (spawn-bottle)
-  (pr2-proj:with-simulated-robot+  (urdf-proj:with-simulated-robot
     (let ((?navigation-goal *base-pose-near-table*))     (let ((?navigation-goal *base-pose-near-table*))
       (cpl:par       (cpl:par
Line 468: Line 502:
         (pp-plans::park-arms)         (pp-plans::park-arms)
         ;; Moving the robot near the table.         ;; Moving the robot near the table.
-        (exe:perform (desig:a motion+        (exe:perform (desig:an action
                               (type going)                               (type going)
                               (target (desig:a location                                (target (desig:a location 
                                                (pose ?navigation-goal)))))))                                                (pose ?navigation-goal)))))))
-      + 
     (multiple-value-bind (?perceived-bottle ?grasping-arm)      (multiple-value-bind (?perceived-bottle ?grasping-arm) 
         (find-object :bottle)         (find-object :bottle)
Line 479: Line 513:
       ;; Moving the robot near the counter.       ;; Moving the robot near the counter.
       (let ((?nav-goal *base-pose-near-counter*))       (let ((?nav-goal *base-pose-near-counter*))
-        (exe:perform (desig:a motion+        (exe:perform (desig:an action
                               (type going)                               (type going)
                               (target (desig:a location                                (target (desig:a location 
Line 485: Line 519:
    
       (coe:on-event (make-instance 'cpoe:robot-state-changed))       (coe:on-event (make-instance 'cpoe:robot-state-changed))
 +      (exe:perform (desig:a motion 
 +                              (type moving-torso)
 +                              (joint-angle 0.3)))
       ;; Setting the object down on the counter       ;; Setting the object down on the counter
       (let ((?drop-pose *final-object-destination*))       (let ((?drop-pose *final-object-destination*))
Line 499: Line 536:
 BTW-TUT> (init-projection) BTW-TUT> (init-projection)
 BTW-TUT> (move-bottle) BTW-TUT> (move-bottle)
-[(PICK-PLACE PICK-UP) INFO] 1550504321.279: Opening gripper +[(PERFORM ACTION) INFO] 1649927731.920: Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved. 
-[(PICK-PLACE PICK-UP) INFO] 1550504321.279: Reaching +[(PICK-PLACE PICK-UP) INFO] 1649927732.109: Looking 
-[(GRASP-FAILURE) WARN] Failed to grasp from LEFT-SIDE using RIGHT arm  +[(PICK-PLACE PICK-UP) INFO] 1649927732.148: Opening gripper and reaching 
-[(TRYING-NEW-GRASP) INFO] 1550504800.749: Trying to grasp from RIGHT-SIDE using RIGHT arm +[(PICK-PLACE PICK-UP) INFO] 1649927732.789: Grasping 
-[(PICK-PLACE PICK-UP) INFO] 1550504800.789: Opening gripper +[(PICK-PLACE PICK-UP) INFO] 1649927733.055: Gripping 
-[(PICK-PLACE PICK-UP) INFO] 1550504800.789: Reaching +[(PICK-PLACE GRIP) INFO] 1649927733.089: Assert grasp into knowledge base 
-[(GRASP-FAILURE) WARN] Failed to grasp from RIGHT-SIDE using RIGHT arm  +[(PICK-PLACE PICK-UP) INFO] 1649927733.097: Lifting 
-[(TRYING-NEW-GRASP) INFO] 1550504801.577: Trying to grasp from BACK using RIGHT arm +[(PICK-PLACE PLACE) INFO] 1649927733.610: Parking 
-[(PICK-PLACE PICK-UP) INFO] 1550504801.601: Opening gripper +[(PERFORM ACTION) INFO] 1649927733.834: Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved. 
-[(PICK-PLACE PICK-UP) INFO] 1550504801.602: Reaching +[(PICK-PLACE PLACE) INFO] 1649927733.945: Looking 
-[(PICK-PLACE PICK-UP) INFO] 1550504801.939: Gripping +[(PICK-PLACE PLACE) INFO] 1649927734.003: Reaching 
-[(PICK-PLACE PICK-UP) INFO] 1550504801.973: Assert grasp into knowledge base +[(PICK-PLACE PLACE) INFO] 1649927734.430: Putting 
-[(PICK-PLACE PICK-UP) INFO] 1550504801.974: Lifting +[(PICK-PLACE PLACE) INFO] 1649927734.677: Opening gripper 
-[(PICK-PLACE PLACE) INFO] 1550504802.356: Reaching +[(PICK-PLACE PLACE) INFO] 1649927734.718: Retract grasp in knowledge base 
-[(PICK-PLACE PLACE) INFO] 1550504802.508: Putting +[(PICK-PLACE PLACE) INFO] 1649927734.807: Updating object location in knowledge base 
-[(PICK-PLACE PLACE) INFO] 1550504802.619: Opening gripper +[(PICK-PLACE PLACE) INFO] 1649927734.807: Retracting 
-[(PICK-PLACE PLACE) INFO] 1550504802.655: Retract grasp in knowledge base +[(PICK-PLACE PLACE) INFO] 1649927735.214: Parking 
-[(PICK-PLACE PLACE) INFO] 1550504802.660: Retracting+[(PERFORM ACTION) INFO] 1649927735.398: Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved.
 </code> </code>
 {{:tutorials:intermediate:btw-tut-grasp-again.png?800|}} {{:tutorials:intermediate:btw-tut-grasp-again.png?800|}}