Simulation Setup Tutorial 4: Grasping, Placing, Real-Time Gripper Control, and Planned Moves

Overview

This tutorial covers asking the robot to grasp (using segmentation/recognition, or by manually specifying the grasp pose), as well as how to place objects or use the motion planner to move to a specified pose.

Moving arms to the side/above/front

For starters, for most manipulation tasks we want the arms out of the way to start, so that we can have a clear view of what's going on. To get the arms to go off to the side, in rviz interact mode, right-click on the bicep of the arm you wish to move, and select either 'Safe move to ...' or 'Forced move to...', and then 'side', as shown below:

Safe move refers to trying to use the motion planners to move the arm; this is useful when the arm might hit something on its way to the side. If there is clearly nothing in the way of the arm getting to the side, selecting Forced move instead will send it to the side faster, but with no consideration of possible obstacles. (Forced move is also useful in times when the motion planners for whatever reason refuse to move the arm.)

You can also ask the arms to move to the 'above' or 'front' positions; experiment with those when the robot is in free-space to see what those positions look like. 'Front' is often useful for driving around while holding objects, although because the arms are not within the footprint of the robot, you should not use autonomous navigation while doing so.

Taking snapshots and positioning the base for manipulation

If you are trying to manipulate objects on a table, you typically want the base to be fairly close to the table, and in many cases, ideally, with the front edge of the base lined up with the edge of the table. To drive the base up to the table, it is usually good to take a snapshot with the stereo camera first, by right-clicking on the head and selecting 'Take Snapshot', as shown below:

The above picture also shows approximately what the scene looks like after you've used the manual/incremental base control arrows to drive up to the edge of the table, using your taken snapshot as a guide.

Moving the torso up/down

In real life, most tables are at a height where we need the torso to be all the way up in order to reach most objects. To move the torso up or down, use the arrows on the back of the PR2:

If you right-click either arrow, you can select 'All the way', which will cause the torso to continue all the way to the top or bottom position without you continuing to hold down the button. While it is moving all the way up or down, click the opposite arrow briefly to stop it in place.

In this simulation, however, the table happens to be fairly low, and so we can manipulate objects even with the torso down.

Grasping segmented objects

Now that we've driven up to the table and adjusted our torso height appropriately, we should take a new snapshot with the head (by either right-clicking on the head and selecting 'Take Snapshot' again, or by right-clicking on the point cloud and selecting 'Refresh'. This is because the robot's driving odometry is only so accurate, and so the old point cloud is likely to be not quite exactly in the right place relative to the robot.

Once we have a new snapshot, if we are dealing with well-separated objects on a tabletop, we can use our segmentation algorithms to find object clusters to pick up autonomously. Click 'Segment' in the Interactive Object Detection window. If segments are found, it will report how many in the status bar of that window, and the segments will appear as green clusters. To pick up a green cluster, right-click on the desired cluster and select 'Pick Up' and the appropriate arm to use:

If the cluster planner successfully finds a non-colliding/feasible grasp from the top or side of the object, the robot will go ahead and execute that grasp.

Grasping recognized objects

If the object happens to be one in the object database (in this simulation, the soda cans are recognizable objects; the mug is not), after the Segment step, instead of grasping a cluster, you can additionally hit the 'Recognize' button. If the robot recognizes one of the segmented clusters as an object in its object database, it will replace the green cluster with a green object mesh for that object. You can also right-click on recognized objects and ask the robot to pick them up:

The advantage of doing so is that the robot has more varied grasps for recognized objects stored in its database, so it may be able to pick up some objects that the cluster planner may fail on.

Manually specifying a grasp pose

For objects in clutter, or for which the autonomous grasp planners fail, you can manually specify a grasp to execute. To do so, in the Interactive Manipulation window, select the arm you wish to use in the bottom right, and then click the 'Grasp' button. A disembodied representation of the gripper (that will hopefully turn green in a moment) should show up at the current gripper pose for that arm, surrounded by rings and arrows:

This disembodied gripper control can be used to specify a desired final grasp pose for the arm. Move the controls around by left-clicking and dragging the arrows to move back and forth along each axis, and rotate them around the three axes by left-click-dragging the three rings.

If you have taken a snapshot of the scene, you can quickly warp the gripper control over to a point on that snapshot by right-clicking on any point in the point cloud and selecting 'Broadcast click position'. When you do so, the robot estimates the normal direction at that point in the point cloud, and uses that to initialize the direction from which the gripper will try to grasp that point. If you wish to grasp the object from the side, select a point on the side; if you wish to grasp from the top, select a point on top. Normals can be noisy, so if the gripper does not appear to come from the correct direction, you can try again with a different point. Below is an example of the initialized pose (coming from the top) for a point selected on the top of a can:

When you warp the gripper to a broadcast point or let go of the gripper controls, the robot will check that pose to see if it is feasible for grasping. What that means is that it will back up that pose by 10 cm along the gripper's approach direction (which is always gripper-x, along the fingertips), to what we call the 'pregrasp' pose. The pregrasp pose needs to be entirely collision-free, both in terms of the gripper and in terms of where the arm thinks it needs to be to place the gripper there (the inverse kinematics solution for that pose). At the actual grasp pose, the gripper itself is allowed to collide with the environment, because you may want to shove other objects aside when moving along the approach from the pregrasp to the grasp; however, the forearm and the rest of the arm still needs to have a feasible, collision-free set of joint angles that will place the gripper there. It will further check that the gripper can lift along the desired lift direction, which defaults to up/vertical (see the 'Advanced Options' section below to change the lift direction).

If the grasp and pregrasp checks are all fine, the gripper will turn green. If there is a problem (either pose is out of reach or there would be a collision), it will turn red. For instance, the pose below:

is out of reach because the arm would have to come from all the way opposite the robot, which is not possible.

When the gripper is in an acceptable pose for grasping, and turns green to indicate that the robot thinks the pose is fine, you can right-click on the green gripper and select 'Accept' to ask the robot to execute the grasp, as shown below:

In simulation, after the robot closes the gripper around an object, it is often the case that the gripper controllers will continue to jitter because the simulation contacts are somewhat unstable; if this happens, the robot will not recognize that the grasp is complete. If the gripper closes on an object but does not lift it, and instead just sits there, you will have to lift the object manually using real-time gripper control, as described below. (If this happens, you would see a message that looks like "[ERROR] [1319857818.414735764, 239.056000000]: Hand posture controller timed out on goal" in the terminal window containing the robot manipulation launch.)

Real-time gripper control

Sometimes you want to do things besides just grasping; to move the gripper in real-time, left-click on the gripper's palm to activate real-time Cartesian control, as shown below:

Just as with the Grasp control, you can move the gripper around by left-click-dragging the rings and arrows. The difference here is that the gripper will move in real-time as you drag it around. Try to move smoothly and not too quickly; the controllers are set to allow fast movement, but when you move quickly the arm will tend to oscillate, and also Cartesian controllers are somewhat unpredictable, so the arm may not go where you expect. Left-click again on the gripper palm to make the Cartesian controls disappear.

Posture control

If the elbow is in a posture that you do not like (for instance, if the elbow is down and the forearm is hitting the table), you can adjust the elbow posture using the posture ring. Left-click on the bicep of the robot to pull up the posture control, as shown below:

SLOWLY rotate the red ring to attempt to move the elbow up or down; too-fast movement can sometimes result in the robot flailing wildly. Because posture control right now is actually only trying to control one joint angle (the shoulder roll joint) while simultaneously attempting to maintain the current pose of the gripper, there are some poses/arm angle configurations for which the shoulder roll joint cannot actually rotate. If that happens, move the gripper to a different pose and try again.

Opening and closing the gripper

At any point, if you want to open or close a gripper, right-click on the gripper palm and select 'Open Gripper' or 'Close Gripper', as shown below:

If you are trying to grasp an object, and you can't see what's going on through the robot's camera, you can often tell whether you successfully grasped by whether the gripper is closed all the way. For instance, this attempt at grasping a can failed, which I can tell because the gripper is closed:

If the grasp had succeeded, the gripper would still be partially open, since once you close, the gripper is attempting to close all the way; only the object prevents it from doing so.

After grasping, either successfully or not successfully, it is often good to move the arm back out of the way by asking it to perform a safe move to side, as shown below:

Once the arm is out of the way, you can take a new snapshot and see what the world looks like now.

Placing

Placing is much like Grasping; if you press the 'Place' button, you can manually specify a place pose to drop an object. If the robot thinks it has successfully grasped an object (which will not happen if the gripper controllers' jitter prevented a successful grasp+lift), the place control will additionally show either the recognized object mesh or the segmented object point cloud that the robot picked up, so that you can use it to visualize where it will go when placed.

The gripper will also turn red if the place pose is not feasible/in collision, and green if it is acceptable. Accepting a place pose will cause the robot to move to the pre-place location (which by default is above the place pose), then lower the object to the place location (and past, if reactive place is on; see Advanced Options below to change this), open the gripper, and retreat along the gripper-approach direction (in the direction of the fingertips/gripper-forward).

Planned Moves

Planned Moves are like Grasping and Placing, except that the robot does not do any pre-grasp, pre-place, lift, or retreat. A pose selected using the Planned Move button needs to be out of collision entirely for the pose to turn green. Upon accepting a planned move pose, the robot will use the motion planners to plan a path to that pose.

Resetting collision representations

When the robot attempts to motion plan (to a grasp, a place, a planned move, or to one of the fixed sets of angles like the side/front/above), it uses a collision representation taken by the tilting laser rangefinder. You can look at the current, built-up collision map by clicking on the PointCloud2 checkbox (of topic /octomap_point_cloud) or the Marker checkbox (of topic /occupied_cells) in the Displays window.

If people have been near/around the robot, or if objects have moved significantly in the scene, this collision map may have become crowded with points where there isn't actually anything. You can often tell when this happens because the robot will refuse to move to poses that it would normally be able to get to. When this happens, you need to reset the collision map, by clicking on the 'Collision' tab in the Interactive Manipulation panel. Change the drop-down to 'collision map' and press the Reset button:

If you picked up a recognized/segmented object and it fell out of the hand, the robot may not realize that the object is gone; when you picked it up, if the robot thought that a successful grasp happened, it will have attached a collision representation to the gripper to avoid hitting the environment with that object. To tell the robot explicitly that there is nothing in the hand, you can change the drop-down to 'all attached objects' (which gets rid of things in both grippers), or 'arm attached objects', which will only get rid of an object attached to the arm chosen in the bottom right of the Interactive Manipulation window.

Selecting 'All collision objects' and hitting Reset will remove explicit representations for any objects in the environment that the robot has segmented, recognized, or placed.

Advanced Options

You can change the desired behavior when executing grasps by clicking on the 'Options' button in the 'Grasp and place' tab of the Interactive Manipulation window.

'Reactive grasping' and 'Reactive transport' refers to using the tactile sensors to adjust a grasp once executed; this does not yet work in simulation.

'Reactive place' is also something that wants the tactile sensors; if you are not using the tactile sensors, the robot during a place action will move the object past the desired place pose, which may or may not be desired behavior. (When placing on a table, this can result in destroying place-actions that try to balance objects on other objects; uncheck this box to make the robot let go exactly where you asked it to place.)

After grasping an object, the robot performs a 'lift' move to get the object away from its supporting surface. The lift move defaults to always lifting up (vertically), but can be changed to lift 'along approach direction' by changing the drop-down menu. This is very useful when grasping out of shelves, since you usually want to back out the way you came in rather than always lifting objects up. Likewise, when placing, the robot approaches the place pose from either above (vertically) or along the gripper-forward direction (along approach direction).

You can also change the distance that the robot tries to lift an object after grasping ('Try to lift after grasp') or the distance that it tries to retreat the gripper after placing an object ('Try to retreat after place'), as well as the desired/minimum approach distances for a grasp. These parameters are useful to change in highly-constrained situations, or for grasping handles and the like, when you want the robot to close but not to move when it gets there.

Movie

Here is a movie showing grasping and placing in a simulated world. The movie can also be found on the laptop's desktop.