Robonaut/Rviz Plugin/Quick Start

MoveIt! Configuration
You should have completed the previous tutorial and you should now have a MoveIt! configuration for the Robonaut that you can use.

In order to run a Demo in Rviz that let you test the robot configuration that you generated in the previous tutorial, it is necessary to write some ROS launch files.

upload_r2
First of all we will generate the upload_r2.launch. To do so you have to create a new blank file, and add each of these lines:
 * Launch upload file for Robonaut


 * Then you must save this file in the launch directory of the generated MoveIt configuration package.

r2_demo

 * Launch the robot description
 * Publish a transform for the virtual joint between world and reference_frame that we created
 * Publish default joint values on the /joint_states topic
 * Publish transforms corresponding to all links in the robot (using the values in /joint_states)
 * Launch the planning components for MoveIt!
 * Launch the visualization component of MoveIt!


 * This file also needs to be saved in the launch directory of the generated MoveIt configuration package.

Configure the environment
Given that you are working on your own workspace because the configuration package is created out of the /opt/ros/groovy environment, you need to export the ROS_PACKAGE_PATH environment variable. To do so, execute:

* export ROS_PACKAGE_PATH=~/your_path_to_robonaut_config_package:$ROS_PACKAGE_PATH


 * The above assumes you exported the Groovy environment

Launch the demo and Configure the Plugin

 * Launch the demo


 * If you are doing this for the first time, you will have to add the Motion Planning Plugin.
 * In the Rviz Displays Tab, press Add
 * From the moveit_ros_visualization folder, choose "MotionPlanning" as the DisplayType. Press "Ok".


 * Now, you can start configuring the Plugin for your robot (the Robonaut in this case).
 * Make sure the robot_description field is set to "robot_description"
 * Make sure the planning_scene field is set to "planning_scene".
 * In Planning Request, change the Planning Group to "right_arm".
 * Set the "Trajectory Topic" in the Planned Path tab to "move_group_display/planned_path".



Visualized Robots
There are four different visualizations active here currently:


 * 1) The start state for motion planning (the planned group is represented in green).
 * 2) The goal state for motion planning (the planned group is represented in orange).
 * 3) The robot's configuration in the planning scene/ planning environment
 * 4) The planned path for the robot,

The display states for each of these visualizations can be toggled on and off using checkboxes.


 * 1) The start state using the "Query Start State" checkbox in the "Planning Request" tab.
 * 2) The goal state using the "Query Goal State" checkbox in the "Planning Request" tab.
 * 3) The planning scene robot using the "Show Scene Robot" checkbox in the "Scene Robot" tab.
 * 4) The planned path using the "Show Robot Visual" checkbox in the "Planned Path" tab.




 * Note that you have to select the fixed frame as World/Dummy or may some other frame is possible too, but you can't leave the map frame.
 * Play with all these checkboxes to switch on and off different visualizations.

Interacting with the Robonaut

 * Press Interact in the top menu of rviz. You should see a couple of interactive markers appear for the right arm of the Robonaut.
 * One marker (corresponding to the orange colored right arm) will be used to set the "Goal State" for motion planning. Another marker corresponding to a green colored representation of the right arm will be used to set the "Start State" for motion planning.
 * You will be able to use these markers (which are attached to the tip link of each arm) to drag the arm around and change its orientation.



Moving into collision
Note what happens when you try to move one of the arms into collision with the other. The two links that are in collision will turn red.



The "Use Collision-Aware IK" checkbox allows you to toggle the behavior of the IK solver. When the checkbox is ticked, the solver will keep attempting to find a collision-free solution for the desired end-effector pose. When it is unticked, the solver will allow collisions to happen in the solution. The links in collision will always still be visualized in red, regardless of the state of the checkbox.



Moving out of reachable workspace
Note also what happens when you try to move an end-effector out of its reachable workspace. (The interactive marker will snap back to a reachable position very quickly).



Motion Planning

 * Now, you can start motion planning with the Robonaut in the MoveIt! Rviz Plugin.
 * Move the Start State to a desired location.
 * Move the Goal State to another desired location.
 * Make sure both states are not in collision with the robot itself.
 * Make sure the Planned Path is being visualized. Also check the "Show Trail" checkbox in the Planned Path tab.
 * In the Planning tab (at the bottom), press the Plan button. You should be able to see a visualization of the arm moving and a trail.

What's Next

 * You can start using MoveIt! with a simulated robot in Gazebo. Check out this tutorial to see how this is done.