| Note: This tutorial assumes that you have completed the previous tutorials: Running the arm_navigation stack on the Barrett WAM (Fuerte). |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Running the object_manipulation stack on the Barrett WAM (Fuerte)
Description: A tutorial teaching how to set up and run the object_manipulation stack on the Barrett WAM.Keywords: manipulation, WAM
Tutorial Level: INTERMEDIATE
Contents
Introduction
This tutorial goes through the steps required to have the object_manipulation stack run on the Barrett WAM. All the packages created in this tutorial are included in the CSIR manipulation repository in the _wam_tutorials directory. Their names have been pre-appended with an underscore.
Prerequisites
Grasp Database
The object_manipulation stack can retrieve pre-planned grasps from a database after recognising an object. This section describes how to set up the database for the Barrett WAM and Hand.
Install Graspit for ROS.
$ sudo apt-get install ros-fuerte-graspit-simulator
Shadow the object_manipulation stack and apply a patch from the CSIR manipulation project to add debug statements and relax the joint accuracy constraints.
$ roscd $ cd sandbox $ rosws set object_manipulation --svn https://code.ros.org/svn/wg-ros-pkg/stacks/object_manipulation/branches/0.6-branch $ rosws update object_manipulation $ source ~/fuerte_workspace/setup.bash $ patch -p0 < ~/fuerte_workspace/csir_manipulation/csir_intelligent_manipulator/object_manipulation.patch $ rosmake object_manipulation
Set up a grasp database following these tutorials:
PostgreSQL installation tutorial replacing the user name willow with your user name (type whoami in the terminal to find your user name; wam-ctrl is the user name for this tutorial) and enabling localhost connections. Log in to the PostgreSQL using sudo access when following the tutorial.
Use the following command to add a new database user:
CREATE ROLE "wam-ctrl" LOGIN CREATEDB CREATEROLE PASSWORD 'wam';
Add the following line to /etc/postgresql/9.1/main/pg_hba.conf to enable local host connections:
local all wam-ctrl md5
Test that a new user has been successfully added using the command below:
$ psql --username wam-ctrl --dbname postgres
Use PGAdmin3 to to add a database server on the computer configured as in the screenshot below:
The newly added server should appear in the object browser. You may have to right click on the newly created wam_db_server and click Connect to see the databases on it.
household_objects database installation tutorial again using your user name. Make sure to download the latest database backup (household_objects-0.6_fuerte_prerelease_1.backup).
Create a new database called household_objects-0.6.
Restore the database from the downloaded backup file.
You can use LibreOffice Base to edit the database and add entries for the Barrett Hand.
$ sudo apt-get install libreoffice-base libreoffice-sdbc-postgresql
Start LibreOffice Base ([Alt+F2] > Base) and choose to connect to an existing database.
Enter the database url as host = localhost dbname=houdehold_objects-0.6
Test the connection before clicking Finish.
Edit the hand table to add the Barrett Hand.
hand name |
Barrett |
hand_relative_path |
/models/robots/Barrett/Barrett.xml |
Add grasps for the Barrett Hand to the grasp table. The entries already present in the household objects database are supposed to have been generated by GraspIt for the PR2. The entries in the table below, however, will just direct the manipulator to a point above a recognised object and have not been produced by a grasp planner.
Object recognition is not perfectly accurate. The Coke can used to test the operation of the stack is sometimes identified as one of several models listed below:
scaled_model_id |
18744 |
- or 18746 or 18766 or 18798 or 18765 or 18685 or 18802
The household objects database is documented on its page in the ROS wiki.
For each model, the rest of the grasp data is identical for the purposes of this tutorial:
grasp_pregrasp_joints |
{0,0,0,0,0,0,0,0} |
grasp_grasp_joints |
{0,0,0,0,0,0,0,0} |
grasp_energy |
1 |
grasp_pregrasp_position |
{0,0,0.4,0,0,-0.70711,0.70711} |
grasp_grasp_position |
{0,0,0.3,0,0,-0.70711,0.70711} |
grasp_source_id |
|
grasp_pregrasp_clearance |
0 |
grasp_cluster_rep |
1 |
hand_name |
Barrett |
grasp_table_clearance |
-1 |
grasp_compliant_copy |
0 |
grasp_compliant_original_id |
-1 |
grasp_scaled_quality |
1 |
fingertip_object_collision |
1 |
ROS Setup
A number of configuration files are required for object manipulation in addition to those automatically generated by the arm navigation configuration wizard. Copy the configuration files from the CSIR manipulation project.
$ roscd wam_arm_navigation/config/ $ roscp csir_wam_arm_navigation collision_map_sources_kinect.yaml . $ roscp csir_wam_arm_navigation self_filter.yaml .
Patch wam_planning_description.yaml to add allowed collisions between robot links besides those generated by the planning wizard.
$ roscd $ patch -p0 < ~/fuerte_workspace/csir_manipulation/csir_intelligent_manipulator/wam_planning_description.patch
A depth camera (Asus Xtion Pro or Kinect) is used to capture the environment. OpenNI converts data from the camera into depth images and point clouds.
$ sudo apt-get install ros-fuerte-openni-launch
You may have to restart your computer after installing OpenNI.
Launch and configuration files
Create package for launch files.
$ roscd $ roscreate-pkg wam_object_manipulation_tutorial $ rosws set wam_object_manipulation_tutorial $ source ~/fuerte_workspace/setup.bash
Fist, top-level tabletop launch file.
$ roscd wam_object_manipulation_tutorial $ mkdir launch && cd launch $ roscp wam_tabletop_manipulation_launch wam_tabletop_manipulation.launch .
Edit it as below.
1 <launch>
2 <!-- Only one arm used, left by default -->
3 <arg name="flatten_table" default="false"/>
4 <arg name="kinect_camera_name" default="wam_base_kinect"/>
5 <!-- database server running on local machine -->
6 <rosparam command="load" file="$(find wam_object_manipulation_tutorial)/config/db_server.yaml"/>
7 <node pkg="household_objects_database" name="objects_database_node" type=
8 "objects_database_node" respawn="true" output="screen"/>
9 <!-- manipulation prerequisites -->
10 <include file="$(find wam_object_manipulation_tutorial)/launch/wam_manipulation_prerequisites.launch">
11 <arg name="kinect_camera_name" value="$(arg kinect_camera_name)"/>
12 </include>
13 <!-- manipulation -->
14 <include file="$(find wam_object_manipulation_tutorial)/launch/wam_manipulation.launch"/>
15 <!-- tabletop collision map processing -->
16 <node pkg="tabletop_collision_map_processing" name="tabletop_collision_map_processing"
17 type="tabletop_collision_map_processing_node" respawn="false" output="screen">
18 <param name="get_model_mesh_srv" value="/objects_database_node/get_model_mesh"/>
19 <param name="static_map_cloud_name" value="full_cloud_filtered"/>
20 <param name="table_thickness" value="0.01"/>
21 </node>
22 <!-- tabletop segmentation and object recognition -->
23 <!-- Use custom launch file to disable transform from /optical_frame to /base_link -->
24 <include file="$(find wam_tabletop_manipulation_launch)/launch/tabletop_complete.launch">
25 <arg name="tabletop_segmentation_points_input" value="$(arg kinect_camera_name)/depth_registered/points"/>
26 <arg name="flatten_table" value="$(arg flatten_table)"/>
27 </include>
28 </launch>
Manipulation pipeline prereq launch file
$ roscd wam_object_manipulation_tutorial/launch $ roscp wam_object_manipulation_launch wam_manipulation_prerequisites.launch .
Edit it as below.
1 <launch>
2 <arg name="kinect_camera_name" default="wam_base_kinect" />
3 <!-- load perception -->
4 <include file="$(find wam_object_manipulation_tutorial)/launch/kinect-perception.launch">
5 <arg name="kinect_camera_name" value="$(arg kinect_camera_name)"/>
6 </include>
7 <!-- load planning -->
8 <include file="$(find wam_arm_navigation)/launch/ompl_planning.launch"/>
9 <!-- warehouse database node -->
10 <param name="warehouse_host" value="localhost"/>
11 <param name="warehouse_port" value="27021"/>
12 <node name="mongo" type="wrapper.py" pkg="mongodb">
13 <param name="overwrite" value="false"/>
14 <param name="database_path" value="arm_navigation_dbs/wam"/>
15 </node>
16 <!-- move arm -->
17 <include file="$(find wam_arm_navigation)/launch/move_arm.launch"/>
18 <!-- monitor -->
19 <!-- ? add substitute for move_arm_head_monitor -->
20 <!-- but head_monitor does not seem to publish a collision map -->
21 <!-- ADD FJTAS node here (+ joint state agglomerator) -->
22 <node pkg="wam_controllers" type="wam_joint_pub.py" name="wam_joint_pub" output="screen"/>
23 <node pkg="wam_controllers" type="wam_follow_joint_trajectory_controller.py" name="wam_follow_joint_controller" output="screen"/>
24 <!-- Publish robot transforms -->
25 <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher"/>
26 <!-- load planning environment - environment_server and planning_scene_validity_server -->
27 <include file="$(find planning_environment)/launch/environment_server.launch">
28 <arg name="use_monitor" value="true"/>
29 <arg name="use_collision_map" value="true"/>
30 </include>
31 <!-- trajectory filter -->
32 <include file="$(find wam_arm_navigation)/launch/trajectory_filter_server.launch"/>
33 <param name="trajectory_filter/object_padd" value="0.04"/>
34 <!-- load IK -->
35 <include file="$(find wam_arm_navigation)/launch/constraint_aware_kinematics.launch"/>
36 <param name="wam_arm_kinematics/object_padd" value="0.04"/>
37 <!-- interpolated IK -->
38 <node name="interpolated_ik_node" pkg="interpolated_ik_motion_planner"
39 type="interpolated_ik_motion_planner.py" respawn="false" output="screen">
40 <!--launch-prefix="xterm -e python -m pdb"-->
41 <param name="robot_prefix" type="string" value="csir_wam"/>
42 <param name="consistent_angle" type="double" value="1.05"/>
43 </node>
44 <!-- joint trajectory normalization -->
45 <include file="$(find trajectory_filter_server)/launch/trajectory_unnormalize.launch"/>
46 </launch>
Manipulation launch file
$ roscd wam_object_manipulation_tutorial/launch $ roscp wam_object_manipulation_launch wam_manipulation.launch .
Edit it as below.
1 <launch>
2 <!-- hand descriptions -->
3 <rosparam command="load" file="$(find wam_object_manipulation_launch)/config/bhand_description.yaml"/>
4 <!-- hand posture controller -->
5 <include file="$(find bhand_grasp_controller)/launch/bhand_grasp_posture_action.launch"/>
6 <!--object manipulator launch-prefix="xterm -e gdb -tui -args">-->
7 <node name="object_manipulator" pkg="object_manipulator"
8 type="object_manipulator_node" respawn="false" output="screen">
9 <!-- left only, but what should happen with unused topics -->
10 <remap from="arm/constraint_aware_ik" to="/wam_arm_kinematics/get_constraint_aware_ik"/>
11 <remap from="right_arm/constraint_aware_ik" to="/wam_arm_kinematics/get_constraint_aware_ik"/>
12 <remap from="arm/get_fk" to="/wam_arm_kinematics/get_fk"/>
13 <remap from="right_arm/get_fk" to="/wam_arm_kinematics/get_fk"/>
14 <remap from="arm/interpolated_ik" to="/interpolated_ik_motion_plan"/>
15 <remap from="right_arm/interpolated_ik" to="/r_interpolated_ik_motion_plan"/>
16 <remap from="arm/interpolated_ik_set_params" to="/interpolated_ik_motion_plan_set_params"/>
17 <remap from="right_arm/interpolated_ik_set_params" to="/r_interpolated_ik_motion_plan_set_params"/>
18 <remap from="arm/get_ik_solver_info" to="/wam_arm_kinematics/get_ik_solver_info"/>
19 <remap from="right_arm/get_ik_solver_info" to="/wam_arm_kinematics/get_ik_solver_info"/>
20 <remap from="arm/get_state_validity" to="/environment_server/get_state_validity"/>
21 <remap from="right_arm/get_state_validity" to="/environment_server/get_state_validity"/>
22 <remap from="arm/move_arm" to="/move_arm"/>
23 <!--
24 <remap from="left_arm/joint_trajectory" to="/l_arm_controller/joint_trajectory_action"/>
25 <remap from="right_arm/joint_trajectory" to="/r_arm_controller/joint_trajectory_action"/>
26 -->
27 <remap from="arm/hand_posture_execution" to="/bhand_grasp_posture_controller"/>
28 <remap from="arm/grasp_status" to="/bhand_grasp_status"/>
29 <!-- This node does not do anything except return a positive response so
30 this node does not halt before moving the arm. -->
31 <remap from="switch_controller" to="/wam_switch_controller"/>
32 <remap from="list_controllers" to="/wam_list_controllers"/>
33 <param name="arm_joint_controller" value="wam_joint_controller"/>
34 <param name="arm_cartesian_controller" value="wam_cart_controller"/>
35 <param name="default_cluster_planner" value="/wam_cluster_planner"/>
36 <param name="default_database_planner" value="/database_grasp_planning"/>
37 <param name="use_probabilistic_grasp_planner" value="false"/>
38 <param name="randomize_grasps" value="false"/>
39 <!-- Use generic kinematics plugin -->
40 <param name="kinematics_plugin_name" value="arm_kinematics_constraint_aware/KDLArmKinematicsPlugin"/>
41 </node>
42 <!-- This node does not do anything except satisfy ROS API requirements for
43 the object_manipulator node. -->
44 <node name="wam_controller_manager" pkg="wam_controllers"
45 type="wam_controller_manager.py" output="screen"/>
46 <node name="wam_joint_controller" pkg="wam_controllers" type="wam_joint_controller.py" output="screen"/>
47 <!-- cluster bounding box finder -->
48 <node name="cluster_bounding_box_finder" pkg="object_manipulator" cwd="node"
49 type="cluster_bounding_box_finder_server.py" output="screen" respawn="false">
50 <param name="z_up_frame" value="/base_link"/>
51 </node>
52 <!-- pad objects when filtering from collision map -->
53 <rosparam param="/robot_description_planning/default_object_padding">0.0001</rosparam>
54 <!-- Extremely simple grasp planner for segmented clusters -->
55 <node name="wam_cluster_planner_node" pkg="wam_grasp_segmented_cluster_planner"
56 type="segmented_cluster_planner.py" output="screen"/>
57 </launch>
Depth camera (Kinect) launch file
$ roscd wam_object_manipulation_tutorial/launch $ roscp wam_object_manipulation_launch kinect-perception.launch .
Edit it as below.
1 <launch>
2 <arg name="kinect_camera_name" default="wam_base_kinect"/>
3 <!-- additional description parameters -->
4 <include file="$(find wam_arm_navigation)/launch/wam_planning_environment.launch"/>
5 <!-- Kinect processing -->
6 <include file="$(find openni_launch)/launch/openni.launch">
7 <arg name="camera" value="$(arg kinect_camera_name)"/>
8 <arg name="depth_registration" value="true"/>
9 <arg name="publish_tf" value="false"/>
10 </include>
11 <!-- self filter Kinect points -->
12 <node pkg="robot_self_filter" type="self_filter" respawn="true" name="kinect_self_filter"
13 output="screen">
14 <remap from="cloud_in" to="/$(arg kinect_camera_name)/depth_registered/points"/>
15 <remap from="cloud_out" to="/$(arg kinect_camera_name)/rgb/points_filtered"/>
16 <param name="sensor_frame" type="string" value="/$(arg kinect_camera_name)_rgb_optical_frame" />
17 <param name="subsample_value" type="double" value="0.1"/>
18 <rosparam command="load" file="$(find wam_arm_navigation)/config/self_filter.yaml" />
19 </node>
20 <node pkg="collider" type="collider_node" name="collider_node" respawn="true" output="screen">
21 <param name="fixed_frame" type="string" value="base_link" />
22 <param name="resolution" type="double" value="0.025" />
23 <param name="max_range" type="double" value="2.5" />
24 <param name="publish_static_over_dynamic_map" value="true" />
25 <param name="sensor_model_hit" value= "0.8" />
26 <param name="sensor_model_miss" value= "0.31" />
27 <param name="sensor_model_min" value= "0.12" />
28 <param name="sensor_model_max" value= "0.95" />
29 <remap from="collision_map_out" to="collision_map_occ" />
30 <remap from="point_cloud_out" to="octomap_point_cloud" />
31 <rosparam command="load"
32 file="$(find wam_arm_navigation)/config/collision_map_sources_kinect.yaml"/>
33 <!-- self filtering links -->
34 <rosparam command="load" file="$(find wam_arm_navigation)/config/self_filter.yaml" />
35 </node>
36 </launch>
Database configuration file.
$ roscd wam_object_manipulation_tutorial $ mkdir config && cd config $ roscp wam_tabletop_manipulation_launch db_server.yaml .
Configuration already correct if following tutorial.
Using the manipulation stack
- Connect the depth camera to the external PC that will act as the ROS master.
- Ensure that the ROS environment variables are correctly set in ~/.bashrc ($ vi ~/.bashrc) on both the external and WAM computers. If the IP of the external PC is 192.168.44.102, .bashrc on the external PC should contain the following:
export ROS_MASTER_URI="http://192.168.44.102:11311" export ROS_HOST_NAME=192.168.44.102 export ROS_IP=192.168.44.102
- If the IP of the WAM PC is 192.168.44.101, then .bashrc on the WAM PC should contain:
export ROS_MASTER_URI="http://192.168.44.102:11311" export ROS_HOST_NAME=192.168.44.101 export ROS_IP=192.168.44.101
- Update the environment variables on both computers.
$ source ~/.bashrc
wam_manipulation is a simple application based on the PR2 pick and place tutorial which should pick an object off a table and place it 10 cm to the left.
$ rosmake --pre-clean wam_pick_and_place
- Launch the WAM tabletop manipulation pipeline on the external PC:
$ roslaunch wam_object_manipulation_tutorial wam_tabletop_manipulation.launch
- Launch the WAM node on the WAM PC:
$ roslaunch wam_node wam_node.launch
- In a separate terminal window on the external PC, run the script included in the wam_pick_and_place package to set the logger levels to ease debugging.
$ rosrun wam_pick_and_place set_loggers.sh
- Run RViz in another terminal window:
$ roscd wam_tabletop_manipulation_launch/config/ $ rosrun rviz rviz -d rviz.vcg
- Run the pick and place app. It may take several attempts to successfully plan the operation. Consult the debug messages in the terminal where you launched the tabletop processing pipeline for more information to correct any problems.
$ rosservice call /wam/hold_joint_pos 0 $ rosrun wam_pick_and_place wam_manipulation
Initially RViz displays the point cloud and robot model.
The pick and place application segments the point cloud into a table and a graspable object, then attempts to identify the graspable object from its database and place the mesh of the identified object in the planning scene.
The planning scene consists of graspable objects and obstacles.
The arm approaches the graspable object. 
The arm grasps the object. The success rate for grasping is only about 1 in 5 owing to various factors including inaccuracies in the WAM joint encoders and the depth camera.
This is where things currently break down. The arm is incorrectly identified as being in collision. The application eventually aborts the place attempt and releases the grasped object.
Future Work
Rather than correct the defects in the WAM adaptation of this deprecated stack, it would be more worthwhile to migrate to the moveit package which is in active development.
