no way to compare when less than two revisions
Differences
This shows you the differences between two versions of the page.
Previous revisionNext revision | |||
— | tutorials:intermediate:simple_mobile_manipulation_plan [2019/04/05 09:14] – amar | ||
---|---|---|---|
Line 1: | Line 1: | ||
+ | **//Tested with Cram v0.7.0, ROS version: Kinetic, Ubuntu 16.04//** | ||
+ | ====== Simple Mobile Manipulation Plan ====== | ||
+ | |||
+ | This tutorial demonstrates how to write a simple plan for the mobile manipulation using the robot, how to write simple error handling and recovery behavior using a lightweight simulation. | ||
+ | |||
+ | This tutorial assumes that you have a basic understanding of plans, and have undergone the [[tutorials: | ||
+ | |||
+ | ==== Installing the projection project ==== | ||
+ | |||
+ | As the system we will be working with will get quite complex things might not go as expected. If you get into trouble while following this tutorial feel free to consult the [[: | ||
+ | |||
+ | If you followed the installation instructions for the full CRAM on the Installation page, you should be ready to go. | ||
+ | |||
+ | ==== Environment Setup ==== | ||
+ | |||
+ | In order to be able to demonstrate many different features of the belief state representation we will set up the environment of the robot similar to how it is done for the real system: we will need an occupancy map for navigation, an IK solver for manipulation, | ||
+ | |||
+ | First, let's set up the environment in our terminal by calling the launch file. It already brings up a '' | ||
+ | <code bash> | ||
+ | $ roslaunch cram_bullet_world_tutorial world.launch | ||
+ | </ | ||
+ | |||
+ | ==== REPL Setup ==== | ||
+ | |||
+ | Now, let's load the package in the REPL (start the REPL with '' | ||
+ | <code lisp> | ||
+ | CL-USER> (ros-load: | ||
+ | CL-USER> (in-package : | ||
+ | BTW-TUT> (roslisp-utilities: | ||
+ | </ | ||
+ | You'll have your bullet world launched with the PR2 in the kitchen. Now you're good to go. | ||
+ | |||
+ | ==== 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, | ||
+ | Here are some of the supported motion and action designators: | ||
+ | * **Motion Designators** - They describe a low-level motion that the robot would take. | ||
+ | * <code lisp> (desig:a motion (type moving-torso) (joint-angle ? | ||
+ | * <code lisp> (desig:a motion (type going) (target ? | ||
+ | * <code lisp> (desig:a motion (type looking) (target ? | ||
+ | * <code lisp> (desig:a motion (type detecting) (object ? | ||
+ | * **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 ? | ||
+ | * <code lisp> (desig:an action (type placing) (arm ?grasp-arm) (object ?obj-desig) (target ? | ||
+ | |||
+ | We will use these designators throughout this tutorial. | ||
+ | |||
+ | ==== 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 - something you'll already be familiar with from the previous tutorial. Let's go ahead and dive into the code: | ||
+ | |||
+ | Any time you want a clean environment, | ||
+ | <code lisp> | ||
+ | BTW-TUT> (init-projection) | ||
+ | </ | ||
+ | |||
+ | Now, Let's define a method to spawn a bottle into the world and apply the physics using the Bullet physics engine and simulate the world for 10 seconds, and run it: | ||
+ | <code lisp> | ||
+ | BTW-TUT> | ||
+ | (defun spawn-bottle () | ||
+ | (unless (assoc :bottle btr:: | ||
+ | (add-objects-to-mesh-list)) | ||
+ | (prolog: | ||
+ | `(and (btr: | ||
+ | | ||
+ | | ||
+ | :mass 0.2 :color (1 0 0) :mesh :bottle)) | ||
+ | | ||
+ | BTW-TUT> (spawn-bottle) | ||
+ | </ | ||
+ | |||
+ | You should now see a red bottle on the table. To make our lives easier, let's define some parameters. The *final-object-destination* is the place where the pr2 has to place the red bottle in the end. The '' | ||
+ | |||
+ | <code lisp> | ||
+ | BTW-TUT> | ||
+ | (defparameter *final-object-destination* | ||
+ | (cl-transforms-stamped: | ||
+ | " | ||
+ | | ||
+ | | ||
+ | |||
+ | (defparameter *base-pose-near-table* | ||
+ | (cl-transforms-stamped: | ||
+ | " | ||
+ | | ||
+ | | ||
+ | |||
+ | (defparameter *downward-look-coordinate* | ||
+ | (cl-transforms-stamped: | ||
+ | " | ||
+ | | ||
+ | | ||
+ | |||
+ | (defparameter *base-pose-near-counter* | ||
+ | (cl-transforms-stamped: | ||
+ | " | ||
+ | | ||
+ | | ||
+ | </ | ||
+ | |||
+ | Let's note down the steps that would involve picking the bottle from the table and placing it on the counter. | ||
+ | * Move the robot near the table. | ||
+ | * Move the arms out of the perception range, so that the robot can see the bottle without obstruction. | ||
+ | * Look towards the object area. | ||
+ | * Detect the object that has to be picked. | ||
+ | * Pick up the object and once again, keep the arms away from the front. | ||
+ | * Move the robot near the counter. | ||
+ | * Place the bottle on the destination. | ||
+ | |||
+ | Implementing all the above steps in code, will look something like as shown below: | ||
+ | <code lisp> | ||
+ | (defun move-bottle () | ||
+ | (spawn-bottle) | ||
+ | (pr2-proj: | ||
+ | (let ((? | ||
+ | (cpl:par | ||
+ | (exe: | ||
+ | (type moving-torso) | ||
+ | (joint-angle 0.3))) | ||
+ | (pp-plans:: | ||
+ | ;; Moving the robot near the table. | ||
+ | (exe: | ||
+ | (type going) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | ;; Looking towards the bottle before perceiving. | ||
+ | (let ((? | ||
+ | (exe: | ||
+ | (type looking) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | ;; Detect the bottle on the table. | ||
+ | (let ((? | ||
+ | (? | ||
+ | (type detecting) | ||
+ | | ||
+ | (type : | ||
+ | ;; Pick up the bottle | ||
+ | (exe: | ||
+ | (type picking-up) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | (pp-plans:: | ||
+ | ;; Moving the robot near the counter. | ||
+ | (let ((?nav-goal *base-pose-near-counter*)) | ||
+ | (exe: | ||
+ | (type going) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | (coe: | ||
+ | ;; Setting the bottle down on the counter | ||
+ | (let ((? | ||
+ | (exe: | ||
+ | (type placing) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | (pose ? | ||
+ | (pp-plans:: | ||
+ | </ | ||
+ | |||
+ | Note that the plan is nested under '' | ||
+ | |||
+ | Now run '' | ||
+ | |||
+ | // | ||
+ | Note:- Every time you run '' | ||
+ | ===== Increasing the Effectiveness by Improving the plan ===== | ||
+ | The previous example worked perfectly because, we knew the exact coordinates to look for the bottle. This is hardly true for the real life scenario, especially since we are dealing with a kitchen environment, | ||
+ | |||
+ | <code lisp> | ||
+ | BTW-TUT> | ||
+ | (defun spawn-bottle () | ||
+ | (unless (assoc :bottle btr:: | ||
+ | (add-objects-to-mesh-list)) | ||
+ | (prolog: | ||
+ | `(and (btr: | ||
+ | | ||
+ | ((-2 -0.9 0.860) (0 0 0 1)) | ||
+ | :mass 0.2 :color (1 0 0) :mesh :bottle)) | ||
+ | | ||
+ | </ | ||
+ | Now try running '' | ||
+ | And the output will look like this. | ||
+ | <code lisp> | ||
+ | BTW-TUT> (move-bottle) | ||
+ | ; Evaluation aborted on #< | ||
+ | </ | ||
+ | |||
+ | {{: | ||
+ | |||
+ | Clearly the robot cannot find the object anymore because even though the robot is near the bottle, it is out of the field of vision of the robot due to the predefined base and object pose in our code. Let's try fixing this issue. | ||
+ | |||
+ | === Recovering from Failures === | ||
+ | To understand the syntax and get a refresher on failure handling, you can refer to [[tutorials: | ||
+ | We're going to add some code into fixing perception in our case, so that the robot would still be able to find the bottle. Let's list down a plan of action for this. | ||
+ | - Tilt the head of the robot downwards | ||
+ | - Try to detect the bottle | ||
+ | - If, | ||
+ | * successful in finding the bottle - continue with the rest of the code. | ||
+ | * failed to find the bottle - turn the head to a different configuration (eg., left/right) and try detecting again. | ||
+ | - When all possible directions fail, error out. | ||
+ | |||
+ | Let's define some additional parameters to aid us: | ||
+ | <code lisp> | ||
+ | BTW-TUT> | ||
+ | (defparameter *left-downward-look-coordinate* | ||
+ | (cl-transforms-stamped: | ||
+ | " | ||
+ | | ||
+ | | ||
+ | |||
+ | (defparameter *right-downward-look-coordinate* | ||
+ | (cl-transforms-stamped: | ||
+ | " | ||
+ | | ||
+ | | ||
+ | |||
+ | </ | ||
+ | We defined two coordinates '' | ||
+ | |||
+ | Now we define a method '' | ||
+ | <code lisp> | ||
+ | (defun get-preferred-arm-for-direction (direction-looked) | ||
+ | (let ((preferred-arm :RIGHT)) | ||
+ | (when (eq direction-looked *left-downward-look-coordinate*) | ||
+ | (setf preferred-arm :LEFT)) | ||
+ | preferred-arm)) | ||
+ | | ||
+ | (defun find-object (? | ||
+ | (let* ((possible-look-directions `(, | ||
+ | , | ||
+ | , | ||
+ | | ||
+ | (setf possible-look-directions (cdr possible-look-directions)) | ||
+ | (exe: | ||
+ | (type looking) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | |||
+ | (cpl: | ||
+ | ((cram-common-failures: | ||
+ | ;; Try different look directions until there is none left. | ||
+ | (when possible-look-directions | ||
+ | | ||
+ | | ||
+ | (type looking) | ||
+ | | ||
+ | (setf ? | ||
+ | (setf possible-look-directions (cdr possible-look-directions)) | ||
+ | | ||
+ | (type looking) | ||
+ | | ||
+ | (pose ? | ||
+ | | ||
+ | | ||
+ | |||
+ | (let ((? | ||
+ | (exe: | ||
+ | (type detecting) | ||
+ | (object (desig:an object | ||
+ | (type ? | ||
+ | (values ? | ||
+ | </ | ||
+ | Let's see what this method does. The '' | ||
+ | Also note that the '' | ||
+ | Let us also update our '' | ||
+ | <code lisp> | ||
+ | (defun move-bottle () | ||
+ | (spawn-bottle) | ||
+ | (pr2-proj: | ||
+ | (let ((? | ||
+ | (cpl:par | ||
+ | (exe: | ||
+ | (type moving-torso) | ||
+ | (joint-angle 0.3))) | ||
+ | (pp-plans:: | ||
+ | ;; Moving the robot near the table. | ||
+ | (exe: | ||
+ | (type going) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | ;; Find and detect the bottle on the table. | ||
+ | (multiple-value-bind (? | ||
+ | (find-object :bottle) | ||
+ | (exe: | ||
+ | (type picking-up) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | (pp-plans:: | ||
+ | ;; Moving the robot near the counter. | ||
+ | (let ((?nav-goal *base-pose-near-counter*)) | ||
+ | (exe: | ||
+ | (type going) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | |||
+ | (coe: | ||
+ | ;; Setting the object down on the counter | ||
+ | (let ((? | ||
+ | (exe: | ||
+ | (type placing) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | (pose ? | ||
+ | (pp-plans:: | ||
+ | </ | ||
+ | |||
+ | Clean up and run '' | ||
+ | {{: | ||
+ | |||
+ | === Expanding Failure Management Capabilities === | ||
+ | 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' | ||
+ | |||
+ | Let's try to visualize this issue, by spawning the bottle in yet another position: | ||
+ | <code lisp> | ||
+ | (defun spawn-bottle () | ||
+ | (unless (assoc :bottle btr:: | ||
+ | (add-objects-to-mesh-list)) | ||
+ | (prolog: | ||
+ | `(and (btr: | ||
+ | | ||
+ | | ||
+ | :mass 0.2 :color (1 0 0) :mesh :bottle)) | ||
+ | | ||
+ | </ | ||
+ | Now run '' | ||
+ | <code lisp> | ||
+ | BTW-TUT> (move-bottle) | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550502686.470: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550502686.470: | ||
+ | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) WARN] 1550502686.797: | ||
+ | | ||
+ | #< | ||
+ | #< | ||
+ | Ignoring. | ||
+ | [(PICK-PLACE MOVE-ARMS-IN-SEQUENCE) ERROR] 1550502687.092: | ||
+ | | ||
+ | #< | ||
+ | #< | ||
+ | Failing. | ||
+ | [(PP-PLANS PICK-UP) WARN] 1550502687.092: | ||
+ | | ||
+ | #< | ||
+ | #< | ||
+ | Ignoring. | ||
+ | ; Evaluation aborted on #< | ||
+ | </ | ||
+ | |||
+ | The robot has failed to grasp again, even though the bottle is well within perception and grasping range. | ||
+ | |||
+ | So what went wrong? If you go back to definition of '' | ||
+ | <code lisp> | ||
+ | (defun get-preferred-arm-for-direction (direction-looked) | ||
+ | (let ((preferred-arm :RIGHT)) | ||
+ | ;; Always prefers right arm unless looking at the left side | ||
+ | (when (eq direction-looked *left-downward-look-coordinate*) | ||
+ | (setf preferred-arm :LEFT)) | ||
+ | preferred-arm)) | ||
+ | </ | ||
+ | And the part where we pick up the object: | ||
+ | <code lisp> | ||
+ | (exe: | ||
+ | (type picking-up) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | </ | ||
+ | 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). | ||
+ | |||
+ | {{: | ||
+ | |||
+ | Once again, let's formulate a strategy like the previous case here. The plan we are going for will look something like this: | ||
+ | - Choose the favored arm. | ||
+ | - Get all possible grasp poses for the given type of the object and the arm. | ||
+ | - Choose one grasp from the possible grasp list. | ||
+ | - Try to grasp the object. | ||
+ | - If, | ||
+ | * Grasp succeeds - Continue with the rest of the code | ||
+ | * Grasp Fails - Choose a different grasp pose from the list. | ||
+ | - When no more possible grasp poses, try changing the arm used to grasp **Once** and try resuming from Step (2) | ||
+ | - When attempted with all arms and grasps, error out. | ||
+ | |||
+ | Let's encapsulate all this in a method called '' | ||
+ | <code lisp> | ||
+ | (defun pick-up-object (? | ||
+ | (let ((? | ||
+ | ;;Retry by changing the arm | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | ((common-fail: | ||
+ | | ||
+ | | ||
+ | (setf ? | ||
+ | | ||
+ | | ||
+ | |||
+ | ;; Retry by changing the grasp | ||
+ | (let* ((? | ||
+ | | ||
+ | | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | (((or cram-common-failures: | ||
+ | cram-common-failures: | ||
+ | | ||
+ | " | ||
+ | e ?grasp ? | ||
+ | | ||
+ | (when (cut: | ||
+ | | ||
+ | " | ||
+ | ? | ||
+ | (setf ? | ||
+ | | ||
+ | (setf ?grasp (cut: | ||
+ | | ||
+ | | ||
+ | | ||
+ | ;; Perform the grasp | ||
+ | (exe: | ||
+ | (type picking-up) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | With this, the '' | ||
+ | |||
+ | There are two nested failure-handling clauses here. The inner failure-handling part will take care of '' | ||
+ | |||
+ | And only upon the failure of all these will the error be bubbled up. The method also returns the grasping arm it used to pick the object up so that the rest of the code is aware of the arm in which the bottle rests. Also, note that the arm we decided on earlier in '' | ||
+ | |||
+ | Also let's redefine '' | ||
+ | <code lisp> | ||
+ | (defun move-bottle () | ||
+ | (spawn-bottle) | ||
+ | (pr2-proj: | ||
+ | (let ((? | ||
+ | (cpl:par | ||
+ | (exe: | ||
+ | (type moving-torso) | ||
+ | (joint-angle 0.3))) | ||
+ | (pp-plans:: | ||
+ | ;; Moving the robot near the table. | ||
+ | (exe: | ||
+ | (type going) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | | ||
+ | (multiple-value-bind (? | ||
+ | (find-object :bottle) | ||
+ | (setf ? | ||
+ | (pp-plans:: | ||
+ | ;; Moving the robot near the counter. | ||
+ | (let ((?nav-goal *base-pose-near-counter*)) | ||
+ | (exe: | ||
+ | (type going) | ||
+ | (target (desig:a location | ||
+ | (pose ? | ||
+ | |||
+ | (coe: | ||
+ | ;; Setting the object down on the counter | ||
+ | (let ((? | ||
+ | (exe: | ||
+ | (type placing) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | (pose ? | ||
+ | (pp-plans:: | ||
+ | </ | ||
+ | You should see a result that looks like the one below. //[Some messages that would come up are suppressed here for readability.]// | ||
+ | <code lisp> | ||
+ | BTW-TUT> (init-projection) | ||
+ | BTW-TUT> (move-bottle) | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550504321.279: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550504321.279: | ||
+ | [(GRASP-FAILURE) WARN] Failed to grasp from LEFT-SIDE using RIGHT arm | ||
+ | [(TRYING-NEW-GRASP) INFO] 1550504800.749: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550504800.789: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550504800.789: | ||
+ | [(GRASP-FAILURE) WARN] Failed to grasp from RIGHT-SIDE using RIGHT arm | ||
+ | [(TRYING-NEW-GRASP) INFO] 1550504801.577: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550504801.601: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550504801.602: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550504801.939: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550504801.973: | ||
+ | [(PICK-PLACE PICK-UP) INFO] 1550504801.974: | ||
+ | [(PICK-PLACE PLACE) INFO] 1550504802.356: | ||
+ | [(PICK-PLACE PLACE) INFO] 1550504802.508: | ||
+ | [(PICK-PLACE PLACE) INFO] 1550504802.619: | ||
+ | [(PICK-PLACE PLACE) INFO] 1550504802.655: | ||
+ | [(PICK-PLACE PLACE) INFO] 1550504802.660: | ||
+ | </ | ||
+ | {{: | ||
+ | |||
+ | The robot has once again succeeded in grasping the object. | ||
+ | |||
+ | 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, | ||
+ | |||
+ | 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. | ||
+ | |||
+ | === Useful Macros === | ||
+ | Many a time, you will find yourselves writing very similar failure handling strategies, because the solution in most cases is to iterate safely between the solutions of a designator or the elements of a list. CRAM provides a couple of useful macros to deal with this to avoid the cumbersome process of rewriting the same logic again and again. | ||
+ | You will find these macros under '' | ||
+ | * '' | ||
+ | * '' | ||
+ | * '' | ||
+ | |||
+ | To demonstrate this, let's take a look at our existing '' | ||
+ | <code lisp> | ||
+ | defun pick-up-object (? | ||
+ | (let ((? | ||
+ | ;;Retry by changing the arm | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | ((common-fail: | ||
+ | | ||
+ | | ||
+ | (setf ? | ||
+ | | ||
+ | | ||
+ | |||
+ | ;; Retry by changing the grasp | ||
+ | (let* ((? | ||
+ | | ||
+ | | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | (((or cram-common-failures: | ||
+ | cram-common-failures: | ||
+ | | ||
+ | " | ||
+ | e ?grasp ? | ||
+ | | ||
+ | (when (cut: | ||
+ | | ||
+ | " | ||
+ | ? | ||
+ | (setf ? | ||
+ | | ||
+ | (setf ?grasp (cut: | ||
+ | | ||
+ | | ||
+ | | ||
+ | ;; Perform the grasp | ||
+ | (exe: | ||
+ | (type picking-up) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | |||
+ | We can see that we do iterate through different grasp poses in a list in the inner failure handling clause. Let us rewrite this using the macro: | ||
+ | |||
+ | <code lisp> | ||
+ | (defun pick-up-object (? | ||
+ | (let ((? | ||
+ | ;;Retry by changing the arm | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | ((common-fail: | ||
+ | | ||
+ | | ||
+ | (setf ? | ||
+ | | ||
+ | | ||
+ | |||
+ | ;; Retry by changing the grasp | ||
+ | (let* ((? | ||
+ | | ||
+ | | ||
+ | (cpl: | ||
+ | (cpl: | ||
+ | (((or cram-common-failures: | ||
+ | cram-common-failures: | ||
+ | | ||
+ | ? | ||
+ | | ||
+ | | ||
+ | " | ||
+ | e ?grasp ? | ||
+ | : | ||
+ | : | ||
+ | (setf ?grasp (cut: | ||
+ | | ||
+ | (exe: | ||
+ | (type picking-up) | ||
+ | (arm ? | ||
+ | | ||
+ | | ||
+ | ? | ||
+ | </ | ||
+ | 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 '' |