Author: Alessandro Di Fava < email@example.com >
Support: < firstname.lastname@example.org >
|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.|
Planning with Octomap demoDescription: Use Octomap in MoveIt! to compute the collision checking with the environment around the robot during the motion planning of poses given in cartesian space
Keywords: Octomap, MoveIt!, motion planning, cartesian space, inverse kinematics
Tutorial Level: INTERMEDIATE
Next Tutorial: Pick and place
The purpose of this tutorials is to provide an example of using Octomap in MoveIt! to plan with TIAGo. The Occupancy map monitor of MoveIt! uses an Octomap to maintain the 3D representation of the environment around the robot. A simulation environment comprising a table with some objects is defined. The robot locates the environment in its camera and builds the Octomap. Then, MoveIt! is used to plan a joint trajectory in order to reach a given pose in cartesian space. MoveIt! uses the Octomap into the collision checking library.
First, make sure that the tutorials are properly installed along with the TIAGo simulation, as shown in the Tutorials Installation Section.
Open three consoles and source the catkin workspace of the public simulation
cd /tiago_public_ws/ source ./devel/setup.bash
Launching the simulation
In the first console launch the following simulation which spawns TIAGo in front of a table with some objects on its top
roslaunch tiago_gazebo tiago_gazebo.launch public_sim:=true robot:=steel world:=tutorial_office gzpose:="-x 1.40 -y -2.79 -z -0.003 -R 0.0 -P 0.0 -Y 0.0" use_moveit_camera:=true
Gazebo will show up with TIAGo. When the simulation is launched, the MoveIt! is automatically launched. The camera:=true parameter will enable MoveIt! to use input from the depth sensor. The Octomap configuration, defined in tiago_moveit_config/launch/tiago_moveit_sensor_manager.launch.xml, specifies the coordinate frame in which this representation will be stored. The coordinate frame in this demo is odom (see http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/perception_configuration.html for details about Moveit! Octomap configuration).
Wait until TIAGo has tucked its arm. Then you may proceed with the next steps.
Launching the nodes
On the second console run the keypoint extractor node
roslaunch tiago_moveit_tutorial octomap_tiago.launch
which will launch Rviz in order to help the user visualize the different steps involved in the demo.
On a third console, launch a topic throttle to reduce the point cloud publish frequency to 2, and name it to the topic moveit is expecting.
rosrun topic_tools throttle messages /xtion/depth_registered/points 2 /throttle_filtering_points/filtered_points
Starting the demo
In the third console the head_look_around motion will be run in order to start the robot to locates the environment around it. When the simulation is launched, the play_motion action server is automatically launched (see http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/play_motion for details). Before to run the head_look_around motion the tiago_moveit_tutorial/config/tiago_octomap_motions.yaml needs to be loaded in the ros parameter server.
rosparam load `rospack find tiago_moveit_tutorial`/config/tiago_octomap_motions.yaml
Then, in order to execute this motion the following node will be used
rosrun play_motion run_motion head_look_around
This motion will move the TIAGo head to locates the table and the objects on its top.
Instead of the head_look_around motion, you can also choose to move the TIAGo head by yourself using the following command (see http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/head_action for details)
rosrun look_to_point look_to_point
When the head_look_around motion node is finished, Rviz will show the built Octomap of the environment.
Then, in the same console (the third console), the following motion will be run (see http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/play_motion for details), in order to move TIAGo to a suitable pose to start the motion planning.
rosrun play_motion run_motion unfold_arm
Wait until TIAGo has unfolded its arm. Then you may proceed with the next steps.
When the unfold_arm motion node is finished, we are going to run an example that will bring TIAGo's end-effector frame, i.e. arm_tool_link, to the three cartesian space configurations with respect to /base_footprint (see http://wiki.ros.org/Robots/TIAGo/Tutorials/MoveIt/Planning_cartesian_space for details).
The three cartesian space configurations with respect to /base_footprint are:
x: 0.508 y: -0.408 z: 0.832 Roll: 0.005 Pitch: -0.052 Yaw: 0.039
x: 0.706 y: 0.357 z: 0.839 Roll: 0.007 Pitch: -0.041 Yaw: 0.040
x: 0.5 y: 0.35 z: 0.38 Roll: 0.003 Pitch: -0.030 Yaw: 0.039
In order to safely reach such cartesian goals the node plan_arm_torso_ik, included in tiago_moveit_tutorial package, has to be called as follows. Please wait each node to be finished before running the next node.
rosrun tiago_moveit_tutorial plan_arm_torso_ik 0.508 -0.408 0.832 0.005, -0.052, 0.039
rosrun tiago_moveit_tutorial plan_arm_torso_ik 0.706 0.357 0.839 0.007, -0.041, 0.040
rosrun tiago_moveit_tutorial plan_arm_torso_ik 0.5 0.35 0.38 0.003, -0.030, 0.039
Sometimes MoveIt! raises internal errors, it can come with plan not found or impossibility to execute the planned trajectory, in this case please run again the single command that has generated the error.
The following images depict the final pose of /arm_tool_link after the execution of each node:
An example of execution of the three nodes is shown in the following video:
MoveIt! RViz Plugin
MoveIt! comes with a plugin for the ROS Visualizer (RViz) (see http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/ros_visualization/visualization_tutorial.html for details). This is a graphical way to define new goal. The plugin allows you to setup scenes in which the robot will work, generate plans, visualize the output and interact directly with a visualized robot.
The usage of the MoveIt! RViz plugin with Octomap in our simulation is as follows
roslaunch tiago_moveit_config demo_tiago.launch
Rviz will show up with TIAGo and the interactive marker. The image shows the MoveIt! RViz plugin with Octomap after the head_look_around and unfold_arm motions.