Welcome to the third lab session of CoTeSys-ROS Fall School on Cognition-enabled Mobile Manipulation on grasping, manipulation, collision-free arm navigation, and plan-based control.

The task for the day is to use ROS arm navigation packages and CRAM, along with the code you have already developed for navigation and perception, to develop a pick and place application.

You can find the cram presentation slides here.

Preparation

  1. SVN Update tum-ros-pkg to make sure you have the package cram_emacs_repl

  2. Make sure that you have roslisp_common trunk checked out. Re-rosinstalling https://svn.code.sf.net/p/tum-ros-pkg/code/rosinstall/fall_school2010.rosinstall should suffice there. Alternatively, check out https://code.ros.org/svn/ros-pkg/stacks/roslisp_common/trunk into your sandbox

  3. Make sure you have run rosmake in cotesys_ros_grasping cram_emacs_repl

  4. Download cram_tutorials.tar.bz2 and extract it into your sandbox

  5. rosmake cram_tutorials

The Actions

You will be employing a set of actions that utilize different components of the arm navigation stacks.

MoveArmToPosition

This action moves the arm to a given position in joint space using a particular named grasp. The set of named grasps is defined in cotesys_ros_grasping/config/pr2_grasps.yaml (or rosie_grasps.yaml). This will employ collision-free arm_navigation. 

# an action which moves the end_effector to the specified x,y,z position relative to the base_link of the robot

# string name associated with the arm to which the end effector is attached
string arm_name

# which named grasp to use
string grasp_name

# x, y, z point of the grasp
geometry_msgs/Point point

---
motion_planning_msgs/ArmNavigationErrorCodes error_code
---

What can go wrong

There are a number of failures that will be reflected in the error code, and a suggestion of what you might do in response:

NO_IK_SOLUTION - when the goal point is unreachable - try a different approach point

IK_LINK_IN_COLLISION - point to be reached puts the end effector into collision. Try a different approach point that's further away from the object

START_STATE_IN_COLLISION - The robot is currently in collision and we can't plan out of it. You can potentially try to clean out collision or attached objects.

GOAL_STATE_IN_COLLISION - Try a different approach pose

TRAJECTORY_CONTROLLER_FAILED - whenever the controller has trouble tracking a trajectory. Just try again.

Defining new grasps

For the pr2 we currently are defining the following grasps in cotesys_ros_grasping/config/pr2_grasps.yaml: 

grasps:
  - name: front_level
    end_effector_x_rot: 0.0
    end_effector_y_rot: 0.0
    end_effector_z_rot: 0.0
    end_effector_w_rot: 1.0
    ik_to_gripper_x_diff: .12
    ik_to_gripper_y_diff: 0.0
    ik_to_gripper_z_diff: 0.0
  - name: top
    end_effector_x_rot: 0.0
    end_effector_y_rot: .7071
    end_effector_z_rot: 0.0
    end_effector_w_rot: .7071
    ik_to_gripper_x_diff: 0.0
    ik_to_gripper_y_diff: 0.0
    ik_to_gripper_z_diff: .12
  - name: default
    end_effector_x_rot: 0.0
    end_effector_y_rot: 0.0
    end_effector_z_rot: 0.0
    end_effector_w_rot: 1.0

To define a new grasp give it a unique name and determine a new end-effector orientation in the base_link frame - make sure that whatever you specify is a valid quaternion or it won't work. The ik_to_gripper are to transform (in cm) from the origin of the ik_link (on the PR2 in the wrist) to the center of the gripper in the base link frame. This is so you can pass into the action the point in base_link where you want to grasp rather than having to reason about where the desired grasp point is in relation to the wrist before calling the action. Whatever values you set for these parameters will be subtracted from the target point to determine the ik link pose for ik. In the top grasp, for instance, if the point to be grasped is (1.0, 0.0, .5) the point passed in for the ik link will be 12 cm above that in the grasp orientation, as ik_to_gripper_z_diff has been set to .12. Once you've added a grasp make sure to relaunch.

MoveArmRelativeCartesianPointGoal

This action moves the arm using the interpolated_ik_motion_planner relative to the current position of the arm in the base_link frame. It is currently set up to disable collision checking. 

# This action moves the arm in the current orientation to the point relative to the current position
# using non-collision aware interpolated ik

# which arm to use
string arm_name

# the point relative to the current position to move to
geometry_msgs/Point rel_point
---
motion_planning_msgs/ArmNavigationErrorCodes error_code
---

What can go wrong

NO_IK_SOLUTION - this can happen when you try to interpolate too far, or when a consistent interpolated IK path can not be found. Try a reduced distance or an alternate approach.

TRAJECTORY_CONTROLLER_FAILED - whenever the controller has trouble. Redo the previous MoveArmToPosition action and try to interpolate again.

TakeStaticCollisionMap

This action takes a static laser collision map

There are no arguments for this action - the laser parameters can be changed in the action server launch description.

AttachBoundingBox

This action attaches the indicated collision object to the indicated arm. If 'remove' is set to true, the indicated object will be removed. Setting the 'arm_name' to 'all' and 'remove' to true will remove all collision objects attached to the indicated arm. 

#this attaches a bounding box of the given object detection to the indicated end effector

string arm_name
bool remove

mapping_msgs/CollisionObject object
---
---

Compiling and launching the interactive CRAM lisp environment

  1. Launch a prepared emacs with the lisp repl with 

rosrun cram_emacs_repl repl
  1. In the repl, load the cram environment 

(ros-load:load-system "cram_tutorials" :cram-pick-and-place)

By executing the previous command, you can compile and reload your changes to the system at any point in time without requiring to restart emacs.

Launching the simulator environment

Since you might need to restart the grasping actions and perception several times, for instance to replace or add new grasps, you need to launch the grasping stuff separate from gazebo and the tools from day 1.

First launch the simulation: 

export ROBOT_INITIAL_POSE="-x 3 -y -0.8 -Y 2.9"

roslaunch cram_tutorials cram_tutorials_gazebo.launch

Then launch in a different terminal the grasping actions plus perception: 

roslaunch cram_tutorials manipulation.launch

This is the terminal that you may want to shut down and restart later.

First steps in the REPL

Be aware of the information on roslisp and cram that you can find here:

As a first, very simple exercise, try moving the base and the arm from the command line. After loading the system in the REPL, try executing the nav_pcontroller move base action (same interface as move base but just a simple p-controller without a planner but with collision avoidance). First switch to the cram-pick-and-place-tutorial package (:ct) and call the init method to connect to all action servers and initialize the system: 

(in-package :ct)
(init)

The call of (init) sets up several variable for convenience, such as a transform listener *tf* to be used for transforms and *move-base-client* to be used with actionlib functions.

Now call the move base action to move the base one meter forward: 

(actionlib:call-goal *move-base-client*
  (make-msg "move_base_msgs/MoveBaseGoal"
    (frame_id header target_pose) "base_link"
    (x position pose target_pose) 1.0
    (w orientation pose target_pose) 1.0))

First you need to move the arm to a sane position. Gazebo starts it up in a weird configuration where it doesn't seem to be possible to make relative movements: 

(park-arm :left)

Now try out the move-arm-relative method. It is just a simple wrapper around the /move_arm_relative_cartesian_point action: 

(move-arm-relative :left (cl-transforms:make-3d-vector 0.0 0.1 0.0))

if the action fails, maybe pr2 cannot move the gripper to that spot, so choose reasonable coordinates.

Play around a little bit with the REPL.

Tutorial

Check if the robot sees the checkerboard 

rosrun image_view image_view image:=/wide_stereo/left/image_rect

If not move the head with 

rosrun teleop_head teleop_head_keyboard

Download and repace the cram_tutorials/tutorial/plans.lisp file with the following plans.lisp

You can execute the tutorial plan with 

(pick-and-place "cluster_0" :right)

from the repl. It parks the arms, approaches the table, tries to grasp an object named cluster_0 with the right arm and puts it right next to the other object on the table.

Open the file cram_tutorials/tutorial/plans.lisp (with an editor of your choice) and follow the TODO instructions in the file. In plans.lisp you will have to implement code to decide on grasps, calculate the pre-grasp point and the approach vector. Also, try to make the plan as robust against failures as possible.

At the end of the tutorial, you will have a plan for picking up an object, moving it to a different table and putting it down relative to another object. We will transform the simple action script into a plan that is transparent for a prolog engine which means that it can be reasoned about. We will record an execution trace and make simple inferences on what happened during the plan. To do so, after calling init in the repl, execute 

(cet:setup-auto-tracing (make-pathname :directory "/tmp/fall_school_traces/") :ensure-directory t)

This enables auto tracing and execution traces are generated for every top-level plan. If you could not generate a trace, take that one from pick_and_place.et. Now load one of the traces in the REPL: 

(setf cet:*episode-knowledge* (cet:load-episode-knowledge "/path/to/file.et"))

Now make some simple queries: 

(cut:force-ll (crs:prolog '(task ?tsk)))
(cut:force-ll (crs:prolog '(and (task ?tsk)
                                (task-goal ?tsk (perceive ?obj)))))

Cheat sheet

This section contains a quick reference for the most important lisp expressions, functions and methods to solve this tutorial.

Action wrappers

The package cram_tutorials contains wrapper functions for all actions explained above.

  • (park-arm <side> [<grasp>])

    • Moves the corresponding arm to a parking position on the side of the robot. The orientation of the gripper can be configured with the optional grasp parameter. The side is either :left or :right.

  • (move-arm-to-point <side> <point> <grasp>)

    • Moves the arm indicated by side to a point-stamped, using the specified grasp. This function calls the action MoveArmToPosition.

  • (move-arm-relative <side> <vector>)

  • (take-collision-map)

  • (find-objects)

    • Searches for all visible objects and returns a vector of all objects. This function also adds the objects to the collision environment.

  • (find-object <name>)

    • Searches the object named name. name is the id as returned in the CollisionObjects generated by the table_objects detector node.

  • (attach-bounding-box <side> <obj>)

  • (detach-bounding-box <side> <obj>)

    • Detaches an object from the gripper indicated by side. The parameters are equal to attach-bounding-box.

Conditions

The common lisp program has wrappers for the most important motion planning errors. These are:

  • no-ik-solution

  • ik-link-in-collision

  • start-state-in-collision

  • goal-state-in-collision

  • trajectory-controller-failed

Transformations, poses and vectors

The roslisp_common stack contains a native implementation of tf. That implies a rich set of functions to work with transformations and stamped poses.

  • - Create a 3d vector: 

(cl-transforms:make-3d-vector <x> <y> <z>)
  • - Create a point stamped: 

(tf:make-point-stamped <frame-id> <time-stamp> <3d-vector>)

When in doubt, use 0 as time-stamp

  • - Create a quaternion: 

(cl-transforms:make-quaternion <x> <y> <z> <w>)
  • - Create a pose stamped: 

(tf:make-pose-stamped <frame-id> <time-stamp> <3d-vector> <quaternion>)
  • - Add two vectors: 

(cl-transforms:v+ <vector-1> <vector-2>)
  • - Multiply two quaternions: 

(cl-transforms:q* <quaternion-1> <quaternion-2>)
  • - Transform a pose stamped: 

(tf:transform-pose *tf* :pose <pose> :target-frame <frame-id>)

Failure handling

Apart from special forms for parallel plan execution and synchronization, CRAM has a powerful failure handling mechanism that also works across threads. Due to the hierarchy implied by nested par and pursue expressions, it is possible to propagate an exception from a child thread up to its parent. Other threads running in parallel and belonging to the same (par...) or (pursue ...) expression are evaporated. The central failure handing construct is with-failure-handling. In contrast to common exception handling, it supports retry. A with-failure-handling block is defined as follows: 

(with-failure-handling
    ((condition-type-1 (var)
       <code for hanlding this exception>
       (retry))
     (condition-type-2 (var)
       <code for handling the exception>))
  <actual code>)

Conditions in Common Lisp are the equivalent to exceptions in C++ or Python. Conditions are similar some kind of classes that are defined with define-condition. For examples, look into the file cram_tutorials/perception.lisp where the condition no-objects-found is defined as a subclass of the condition plan-error.

To just ignore the exception without retrying, the handler code block can call (return <return-value>). Rethrowing is achieved by neither calling retry nor return.

PR2 Pick and Place Demo by John Hsu

http://www.ros.org/wiki/pr2_simulator/Tutorials/SimpleGraspingDemo

Our solution

You can find our solution here: cram_tutorials_solution.tar.bz2

Wiki: Events/CoTeSys-ROS-School/Day4 (last edited 2012-12-07 13:19:04 by Lorenz Mösenlechner)

Except where otherwise noted, the ROS wiki is licensed under the
Creative Commons Attribution 3.0