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 [2022/04/13 09:41] – [Recovering from Failures] schimpftutorials: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 335: 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 409: 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 420: 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 450: 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 461: 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 469: 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 480: 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 486: 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 500: 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|}}