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/07/08 18:00] – [Installing the projection project] gkazhoyatutorials: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 34: Line 34:
 You'll have your bullet world launched with the PR2 in the kitchen. Now you're good to go. You'll have your bullet world launched with the PR2 in the kitchen. Now you're good to go.
  
-==== Some Useful Designators ====+===== Some Useful Designators ====
 Before we proceed further let's look at some of the designators that are supported by the robot in the bullet world (and this works in the real world robot as well). To refresh your memory on designators, visit the tutorial [[tutorials:beginner:motion_designators|Creating motion designators for the TurtleSim]].  Before we proceed further let's look at some of the designators that are supported by the robot in the bullet world (and this works in the real world robot as well). To refresh your memory on designators, visit the tutorial [[tutorials:beginner:motion_designators|Creating motion designators for the TurtleSim]]. 
 Here are some of the supported motion and action designators: Here are some of the supported motion and action designators:
Line 41: Line 42:
       * <code lisp> (desig:a motion (type going) (target ?loc-desig)) </code> is a motion that moves the body of the robot and expects a key ''target'', whose value should be of type ''location-designator''.       * <code lisp> (desig:a motion (type going) (target ?loc-desig)) </code> is a motion that moves the body of the robot and expects a key ''target'', whose value should be of type ''location-designator''.
       * <code lisp> (desig:a motion (type looking) (target ?loc-desig))</code> is a motion that moves the head of the robot to look at the specfied location. It expects a key ''target'' with value of type ''location'' designator.        * <code lisp> (desig:a motion (type looking) (target ?loc-desig))</code> is a motion that moves the head of the robot to look at the specfied location. It expects a key ''target'' with value of type ''location'' designator. 
-      * <code lisp> (desig:a motion (type detecting) (object ?obj-desig)) </code> is a a motion that perceives and detects the specified object. It expects a key ''object'' with a value of type ''object-designator''+      * <code lisp> (desig:a motion (type detecting) (object ?obj-desig)) </code> is a a motion that perceives and detects the specified object. It expects a key ''object'' with a value of type ''object-designator''. An example is <code lisp>  
 +      (desig:a motion (type detecting) (desig:an object (type bowl))) </code>. This motion perceives and tries to find an object of type bowl in the field of view where the camera of the robot is pointed to. The resultant of this is also an object designator, but it has much more information. A sample is given below <code lisp> 
 +    #<A OBJECT 
 +    (TYPE BOWL) 
 +    (NAME BOWL-1) 
 +    (POSE ((:POSE 
 +            #<CL-TRANSFORMS-STAMPED:POSE-STAMPED  
 +   FRAME-ID: "base_footprint", STAMP: 1.564755796743144d9 
 +   #<3D-VECTOR (0.7672472770564228d0 -0.35741216776250767d0 0.888642116546631d0)> 
 +   #<QUATERNION (-0.005783746257358676d0 -0.0013544535828327665d0 -0.26923602469246516d0 0.9630558655492675d0)>>
 +           (:TRANSFORM 
 +            #<CL-TRANSFORMS-STAMPED:TRANSFORM-STAMPED  
 +   FRAME-ID: "base_footprint", CHILD-FRAME-ID: "bowl_1", STAMP: 1.564755796743144d9 
 +   #<3D-VECTOR (0.7672472770564228d0 -0.35741216776250767d0 0.888642116546631d0)> 
 +   #<QUATERNION (-0.005783746257358676d0 -0.0013544535828327665d0 -0.26923602469246516d0 0.9630558655492675d0)>>
 +           (:POSE-IN-MAP 
 +            #<CL-TRANSFORMS-STAMPED:POSE-STAMPED  
 +   FRAME-ID: "map", STAMP: 1.564755796743144d9 
 +   #<3D-VECTOR (1.5255411783854167d0 0.761638069152832d0 0.8886421203613282d0)> 
 +   #<QUATERNION (-0.005410938989371061d0 -0.0024511234369128942d0 -0.07722260057926178d0 0.9969961643218994d0)>>
 +           (:TRANSFORM-IN-MAP 
 +            #<CL-TRANSFORMS-STAMPED:TRANSFORM-STAMPED  
 +   FRAME-ID: "map", CHILD-FRAME-ID: "bowl_1", STAMP: 1.564755796743144d9 
 +   #<3D-VECTOR (1.5255411783854167d0 0.761638069152832d0 0.8886421203613282d0)> 
 +   #<QUATERNION (-0.005410938989371061d0 -0.0024511234369128942d0 -0.07722260057926178d0 0.9969961643218994d0)>>)))> </code> Note that this resulting object designator has the name of the object along with the coordinate of the object w.r.t the robot coordinate frame and other useful coordinate transformations which can later be used to calculate the pose of the end effector for grasping.
     * **Action Designators** - These describe the high-level actions which may consist of multiple calls to different low-level motion to carry out a small plan.     * **Action Designators** - These describe the high-level actions which may consist of multiple calls to different low-level motion to carry out a small plan.
-        * <code lisp> (desig:an action (type picking-up) (arm ?grasp-arm) (grasp ?grasp-pose) (object ?obj-desig)) </code> is an action that picks an object with the specified arm and grasp pose. It expects an ''object'' key with value of type ''object-designator'', an ''arm'' with the value of type keyword for the arm to choose, and a ''grasp'' key whose value specifies the keyword of the grasp pose to pick the object with.+        * <code lisp> (desig:an action (type picking-up) (arm ?grasp-arm) (grasp ?grasp-pose-identifier) (object ?obj-desig)) </code> is an action that picks an object with the specified arm and grasp pose. It expects an ''object'' key with value of type ''object-designator'', an ''arm'' with the value of type keyword for the arm to choose, and a ''grasp'' key whose value specifies keyword which is an identifier of the grasp pose to pick the object with.
         * <code lisp> (desig:an action (type placing) (arm ?grasp-arm) (object ?obj-desig) (target ?loc-desig)) </code> is an action that places the object specified to the target. It expects an optional key ''arm'' with the keyword of the arm to use, an ''object'' of type ''object-designator'' and a ''target'' key with the value of type ''location-designator''.         * <code lisp> (desig:an action (type placing) (arm ?grasp-arm) (object ?obj-desig) (target ?loc-desig)) </code> is an action that places the object specified to the target. It expects an optional key ''arm'' with the keyword of the arm to use, an ''object'' of type ''object-designator'' and a ''target'' key with the value of type ''location-designator''.
  
 We will use these designators throughout this tutorial. We will use these designators throughout this tutorial.
  
-==== Constructing plans ====+===== Constructing plans =====
  
 The goal of this chapter is to write a plan for a simple task: pick and place an object from one position to another position in the world. Let's go ahead and dive into the code: The goal of this chapter is to write a plan for a simple task: pick and place an object from one position to another position in the world. Let's go ahead and dive into the code:
Line 63: 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 -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 72: 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 110: 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 118: 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 124: 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 143: 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 159: 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 173: 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 -0.82) (0 0 0 1)))
   (btr:simulate btr:*current-bullet-world* 10))   (btr:simulate btr:*current-bullet-world* 10))
 </code> </code>
Line 221: 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 228: 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 238: 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 261: 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 271: 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 286: 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 301: Line 326:
                                                 (pose ?drop-pose))))))                                                 (pose ?drop-pose))))))
       (pp-plans::park-arms :arm ?grasping-arm))))       (pp-plans::park-arms :arm ?grasping-arm))))
 +
 </code> </code>
  
-Clean up and run ''(move-bottle)'' again, and this time, you'll find that the robot succeeds in transporting the bottle.+Run ''(move-bottle)'' again, and this time, you'll find that the robot succeeds in transporting the bottle.
 {{:tutorials:intermediate:btw-tut-found-bottle-again.png?800|}} {{:tutorials:intermediate:btw-tut-found-bottle-again.png?800|}}
  
Line 309: 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))
-  (prolog:prolog +  (btr-utils:spawn-object 'bottle-1 :bottle :color '(1 0 0) :pose '((-1.8 -1 0.860) (0 0 0 1))) 
-   `(and (btr:bullet-world ?world) +  (btr:simulate btr:*current-bullet-world* 10))
-         (assert (btr:object ?world :mesh bottle-1 +
-                             ((-1.1 -0.75 0.860) (0 0 0 1)+
-                             :mass 0.2 :color (1 0 0) :mesh :bottle)) +
-         (btr:simulate ?world 100))))+
 </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.470Opening gripper + 
-[(PICK-PLACE PICK-UP) INFO] 1550502686.470Reaching +[(PICK-PLACE PICK-UP) INFO] 1649925566.714Looking 
-[(PICK-PLACE MOVE-ARMS-IN-SEQUENCEWARN1550502686.797: #<POSE-STAMPED +[(PICK-PLACE PICK-UP) INFO] 1649925566.758Opening gripper and reaching 
 +[(URDF-PROJ MOVE-TCPERROR1649925567.115: #<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. 
 +Failing. 
 +[(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) WARN] 1649925567.119: #<POSE-STAMPED  
 +   FRAME-ID: "torso_lift_link", STAMP: 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.
 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-UPWARN1550502687.092: Manipulation messed up: #<POSE-STAMPED +[(PICK-PLACE MOVE-ARMS-IN-SEQUENCEERROR1649925567.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.
 +   #<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 363: Line 417:
                                (object ?perceived-bottle)))                                (object ?perceived-bottle)))
 </code> </code>
-We see that the robot defaults the right arm when the object is in the center and will always try to grasp the left side of the bottle, even when the object is slightly favoring the left side under it (Refer the figure below).+We see that the robot defaults the right arm when the object is in the center and will always try to grasp the left side of the bottle, even when the object is slightly favoring the left side under it (Refer the figure below).  
 + 
 +Note: ''We have been using left-side here a lot. In the context of (grasp left-side), it refers to the left-side of the bottle. But what defines the "left-side"?'' 
 + 
 +''Before we understand it, we have to acknowledge that every object spawned in the bullet world has its own coordinate axes - even rotationally symmetric objects like the bottle here. And the definition for left-side is the side of the -ve Y-axis in the bottle coordinate frame. Similarly right-side is +ve Y-axis, front is denoted by +ve X-axis and the back by -ve X-axis. '' 
 +''With this the "left-side" grasp can change based on how the object is oriented in the world. But it remains the w.r.t the object coordinate frame.'' 
 + 
 +''The file at <your workspace>/cram/cram_common/cram_object_knowledge/src/household.lisp holds some definitions for grasps supported by the household objects supporte in CRAM.''
  
 {{:tutorials:intermediate:btw-tut-grasp-config-fail.png?800|}} {{:tutorials:intermediate:btw-tut-grasp-config-fail.png?800|}}
Line 380: 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 391: 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 421: 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 432: 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 440: 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 451: 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 457: 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 471: Line 536:
 BTW-TUT> (init-projection) BTW-TUT> (init-projection)
 BTW-TUT> (move-bottle) BTW-TUT> (move-bottle)
-[(PICK-PLACE PICK-UP) INFO] 1550504321.279Opening gripper +[(PERFORM ACTION) INFO] 1649927731.920Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved. 
-[(PICK-PLACE PICK-UP) INFO] 1550504321.279Reaching +[(PICK-PLACE PICK-UP) INFO] 1649927732.109Looking 
-[(GRASP-FAILURE) WARN] Failed to grasp from LEFT-SIDE using RIGHT arm  +[(PICK-PLACE PICK-UP) INFO] 1649927732.148Opening gripper and reaching 
-[(TRYING-NEW-GRASP) INFO] 1550504800.749Trying 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.055Gripping 
-[(PICK-PLACE PICK-UP) INFO] 1550504800.789Reaching +[(PICK-PLACE GRIP) INFO] 1649927733.089Assert grasp into knowledge base 
-[(GRASP-FAILURE) WARN] Failed to grasp from RIGHT-SIDE using RIGHT arm  +[(PICK-PLACE PICK-UP) INFO] 1649927733.097Lifting 
-[(TRYING-NEW-GRASP) INFO] 1550504801.577Trying to grasp from BACK using RIGHT arm +[(PICK-PLACE PLACE) INFO] 1649927733.610Parking 
-[(PICK-PLACE PICK-UP) INFO] 1550504801.601Opening gripper +[(PERFORM ACTION) INFO] 1649927733.834Action goal `((ARMS-POSITIONED-AT NIL NIL))' already achieved
-[(PICK-PLACE PICK-UP) INFO] 1550504801.602Reaching +[(PICK-PLACE PLACE) INFO] 1649927733.945Looking 
-[(PICK-PLACE PICK-UP) INFO] 1550504801.939Gripping +[(PICK-PLACE PLACE) INFO] 1649927734.003: Reaching 
-[(PICK-PLACE PICK-UPINFO] 1550504801.973: Assert grasp into knowledge base +[(PICK-PLACE PLACE) INFO] 1649927734.430: Putting 
-[(PICK-PLACE PICK-UP) INFO] 1550504801.974Lifting +[(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|}}
Line 496: Line 561:
 You can now try spawning the bottle in different points on the table and observing how the robot resolves the arm and grasp for it. Subsequently, you will notice that our plans will work as long as the bottle is within reach of the robot. You could try implementing the adjustment of the robot's position to facilitate the fetch. You can now try spawning the bottle in different points on the table and observing how the robot resolves the arm and grasp for it. Subsequently, you will notice that our plans will work as long as the bottle is within reach of the robot. You could try implementing the adjustment of the robot's position to facilitate the fetch.
  
-Since this is a simple tutorial in formulating and understanding mobile plans using CRAM, developing advanced plans and recovery behaviorsis left up to you.+Instead of a hardcoded pose for the base of the robot, you now would now need to obtain a list of poses where the robot will be able to grasp the bottle from. This is exactly where [[tutorials:intermediate:costmaps|location costmaps]] come into play. CRAM provides an easy way to handle these where the user does not have to bother with hardcoded poses. A way to obtain this would be through the following designator 
 +<code lisp> 
 +(a location  
 +   (reachable-for pr2) 
 +   (object (an object 
 +               (type bottle)))) 
 + 
 +</code> 
 +Referencing this will give you locations where our robot PR2 can successfully reach an object of type bottle. Note that the object designator used here is pretty generic and will apply to all the bottles in the robot's belief state. 
 +To make it more specific we can provide the name and the location designator where the bottle is expected to be in. 
 +Eg: 
 +<code lisp> 
 +;; This is the location designator used to reference locations where the pr2 should be to reach an object  
 +;; named bottle-1 of type bottle which is situated on the counter-top named sink-area-surface 
 +(a location 
 +   (reachable-for pr2) 
 +   (object (an object 
 +               (type bottle) 
 +               (name bottle-1) 
 +               (location (a location  
 +                            (on (an object  
 +                                    (type counter-top) 
 +                                    (urdf-name sink-area-surface) 
 +                                    (part-of kitchen)))))))) 
 +</code> 
 +You can see that the designator is not that intimidating as it looks, but provides a structured way to reference objects and locations. You can use these to write plans without having to worry about the actual coordinates. 
 + 
 +Since this is a simple tutorial in formulating and understanding mobile plans using CRAM, developing advanced plans and recovery behaviors is left up to you. 
 + 
 + 
 +<html><!-- 
 +THIS IS ONLY IN CRAM v0.8.0 
  
 === Useful Macros === === Useful Macros ===
Line 591: Line 688:
 </code> </code>
 This code will behave exactly like before but all the repetitive logic has been moved to this macro. The first two arguments are always the iterating list (in this case, the list containing possible grasp pose) and the number of retries. The keyword ''error-object-or-string'' can accept a warning string or just the plain error object and it will be used for logging. ''warning-namespace'' will accept the name in which the logging information is classified into. Since our method rethrows ''common-fail:object-unreachable'' we pass that along into ''rethrow-failure'', and if none is passed, it will rethrow the same error it received. This code will behave exactly like before but all the repetitive logic has been moved to this macro. The first two arguments are always the iterating list (in this case, the list containing possible grasp pose) and the number of retries. The keyword ''error-object-or-string'' can accept a warning string or just the plain error object and it will be used for logging. ''warning-namespace'' will accept the name in which the logging information is classified into. Since our method rethrows ''common-fail:object-unreachable'' we pass that along into ''rethrow-failure'', and if none is passed, it will rethrow the same error it received.
 +
 +
 +-->
 +
 +</html>