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:05] – [Expanding Failure Management Capabilities] Added information about grasps 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 42: 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-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 a keyword which is an identifier 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 a keyword which is an identifier of the grasp pose to pick the object with.
Line 64: 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 73: 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 111: 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 119: 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 125: 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 144: 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 160: 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 174: 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 222: 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 229: 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 239: 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 262: 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 272: 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 287: 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 302: 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 310: 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 384: 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 395: 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 425: 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 436: 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 444: 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 455: 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 461: 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 475: 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|}}