Setup Jaco with MoveIt

This wiki page provides instructions on how to integrate the Jaco arm with MoveIt! It uses some sample configuration and launch files in jaco_moveit.

If you only want to test the example with the Jaco hand on the table, there is a workable set-up provided in the package jaco_on_table_moveit. You may follow instructions in the Jaco MoveIt! tutorial to get started with using MoveIt! with the example Jaco robot.

If you have your own robot with a Jaco hand, you need to set it up for use with MoveIt! first. Then, follow the steps described in the following.

The screenshots in the instructions are for the example jaco_on_table setup.

1. Build your own robot model

Build your own robot model which includes the jaco arm. This step is described in detail on the jaco_description wiki page, and only briefly summarized here.

Include the jaco arm by using the file jaco_description/urdf/jaco.urdf.xacro.

For example:

    <xacro:include filename="$(find jaco_description)/urdf/jaco.urdf.xacro"/>
        ....
    <!-- include jaco_arm -->
    <xacro:jaco_arm parent="arm_support_link"
                    mass_divider="2"
                    finger_mass_divider="1">
      <origin xyz="0 0 0" rpy="0 0 0" />
      <!-- set to true if you would like to add <transmission> tags -->
      <xacro:arg name="add_transmission_pos" default="true"/>
      <xacro:arg name="add_transmission_vel" default="true"/>
    </xacro:jaco_arm>

Make sure that a gazebo plugin is loaded for the model which publishes JointState messages for all moving joints in the robot, otherwise you might get MoveIt! errors for lookupTransform: robot_state_publisher will prune the tf tree where it does not get joint state data.
You may use the plugins in jaco_gazebo, also described on this wiki page.

2. Generate URDF

You need to generate a URDF file from your xacro robot file, because MoveIt! only works with URDF. Use the following command:

rosrun xacro xacro --inorder <your-robot-xacro-filename>.urdf.xacro > <your-transformed-urdf-file>.urdf

3. MoveIt! setup assistant

Use the MoveIt! setup assistant to configure the model. See also the setup assistant tutorial.

Note: If you are using Jade, at the time of writing (Feb 2016) you have to compile the MoveIt! setup assistant in your catkin workspace. Clone from this repository.

Step 1

Launch the setup assistant.

roslaunch moveit_setup_assistant setup_assistant.launch

Click "Create new MoveIt configuration package", and select the urdf file {your-transformed-urdf-file}.urdf.

Click "Load files" to load the urdf file.

Step 2

Go to the next tab "self-collisions" and generate the collision matrix (full sampling density is the best).

Step 3 (optional)

If your model is mobile (e.g. on wheels), you can add a "virtual joint". This virtual joint represents the motion of the base of the robot in a plane. Click on the Virtual Joints pane selector. Click on Add Virtual Joint.

Choose a joint name (e.g. "virtual_joint"), and set the child link as the root link of the robot (e.g. base_footprint in PR2 and some others) and pick a name for the parent frame, e.g. "odom_combined".

Set the Joint Type as "planar" and click save.

Step 4

Create the planning groups. A planning group needs to be specified when sending planning requests to MoveIt!, which will then plan on the given group only. We are going to create one for the arm only, and also one for all. The group for "all" of the robot is only useful if the robot is mobile, but we will do it here anyway for demonstration. On some robots, you may also decide to add an extra group for the Hand only.

Click "Add group" and name it "Arm" and choose kdl_kinematics_plugin/KDLKinematicsPlugin as the kinematics solver. Leave other parameters as default. Click "Add kinematic chain", navigate down to the link to which the arm is attached, and create a chain from Base Link: 0_baseA to Tip link: 6_hand_limb. To do this, first select 0_baseA and click "Choose selected" on Base Link, then click on 6_hand_limb and click "Choose selected" on Tip Link.
Then click "Save".

Create another group called "All" in which you add all links/joints of your robot. Leave the Kinematic Solver on "None".
If your robot does not contain any other mobile joints, it is enough to add all links of the jaco arm only with "Add Links", adding all links starting from 0_baseA. IMPORTANT: If you have a mobile robot with other mobile joints than the jaco joints, you have to add the whole robot here, except the joints which are not actuated by MoveIt (e.g. wheels) - in this case, add your robot via "joints" instead of "links" (because the latter will automatically add all joints).

Step 5

Go to the End Effectors pane and click "Add End Effector". Name it "Wrist" and choose "Arm" as end effector group. Parent link is "6_hand_limb". Leave Parent Group blank. Click "save".

Step 6 (optional)

Make sure you add all passive joints of your robot (e.g. wheels) to the "passive joints".

Step 7

Go to the "configuration files" tab and choose a directory into which to generate the files. This should be either in the catkin workspace, or saved elsewhere and then soft-linked to the catkin workspace.

Then, click "Generate".

If you ever need to go back to re-configure, you can use

roslaunch <your-robot-moveit-package-name> setup_assistant.launch

and click "Load files" to re-load your configuration.

Step 8

Check your setup with

roslaunch <your-robot-moveit-package-name> demo.launch

which will launch MoveIt! and RViz. You may set the end effector target pose with the visual marker and use the MoveIt! control panel in RViz to plan the joint trajectory.

4. Add convenience launch files

The launch file {your-robot-moveit-package-name>}/launch/demo.launch is only an example, eventually you need to create your own launch files. While you can surely build your own from scratch, there are a few convenience launch files available in the package jaco_moveit which will help you to get started quicker.

So now, the package "your-robot-moveit-package-name" which you have just generated will be supplemented a little with convenience launch files.

4.1 Create and test MoveIt launch file

In your-robot-moveit-package-name, create a launch file {robot-name}_moveit.launch with the follwing contents, replacing your-robot-moveit-package-name and adapting the argument values to your needs. If the robot has no virtual joint, you may remove all arguments related to this. See also $(find jaco_moveit)/launch/jaco_moveit.launch for further description of the arguments.

<launch>
    <!-- the robot base name, often base_footprint -->
    <arg name="robot_base" default="base_link"/>

    <!-- this specifies the frame for the octomap (in planning scene) -->
    <param name="octomap_frame" type="string" value="$(arg robot_base)"/>
       
   <!-- Specify joint states topic because MoveIt! reads from it for
        the current robot state  -->
    <arg name="joint_states_topic" value="/jaco/joint_state"/>
    
    <!-- Switch on or off fake execution in order to suppress errors when
         no MoveIt! execution is loaded. -->
    <arg name="load_fake_execution" default="false"/>

    <!-- Set to true if the robot should be loaded onto robot_description -->
    <arg name="load_robot" default="false"/>
 
    <include file="$(find jaco_moveit)/launch/jaco_moveit.launch">
        <arg name="moveit_package_path" value="$(find jaco_on_table_moveit)"/>
        <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
        <arg name="load_fake_execution" default="$(arg load_fake_execution)"/>
        <arg name="load_robot" value="false"/>
        <arg name="has_virtual_joint" value="false"/>
        <arg name="publish_virtual_joint_frame" value="true"/>
        <arg name="virtual_joint_parent" value="odom"/>
        <arg name="virtual_joint_child" value="$(arg robot_base)"/>
        <arg name="virtual_joint_pubfreq" value="50"/>
    </include>
</launch>

Now, test this launch file right away. It should launch the moveit planner only. For it to run stand-alone, you have to set the load_robot argument to true, because you need to have a robot_description on the parameter server.

roslaunch <your-robot-moveit-package-name> <robot-name>_moveit.launch load_robot:=true

No error messages should be printed, and you should see the message

"All is well! Everyone is happy! You can start planning now"

4.2 Create RViz launch file

Just for convenience, we also would like a separate launch file for RViz.

In your-robot-moveit-package-name, create a launch file {robot-name}_rviz.launch with the follwing contents:

	<launch>
		<include file="$(find jaco_moveit)/launch/jaco_rviz.launch">
			<arg name="moveit_package_path" value="$(find <your-robot-moveit-package-name>)"/>
		</include>
	</launch>

4.3 Create gazebo launch file [optional].

You might already have a gazebo launch file for your robot, but if you do not, now would be the time to create one.

See the jaco_description wiki page for details on how to create a gazebo launch file.

5. First test with Gazebo

Now, you can test if MoveIt! is receiving the robot state information from Gazebo correctly.

Step 1. Launch gazebo

roslaunch <your-package> <your-gazebo-launch-file>

Or if you have a more complex launch file which launches either gazebo or the real arm (e.g. jaco_ros/jaco_bringup.launch), use this one instead.

Step 2. Launch moveit

Now, additionally launch moveit:

roslaunch <your-robot-moveit-package-name> <robot-name>_moveit.launch load_fake_execution:=true

It is important to always have the argument load_robot:=false (default) if you are using Gazebo. And for this for test, we will load the fake execution (argument load_fake_execution otherwise defaults to false), because we have not configured the controllers yet.

The output should show no errors, which is a sign that the robot loaded by gazebo is recognized and the /tf transforms are ok.

Step 3. Launch rviz

Now, additionally launch RViz to see if the robot takes on the same pose as in gazebo.

roslaunch <your-robot-moveit-package-name> <robot-name>_rviz.launch

If the robot does not have the same state as in Gazebo:

Check for possible error messages on the terminals. Are the /tf transforms correct? When you add a "rviz/RobotModel" RViz object, does this one take the right state?
Have you specified the right settings in the launch files you created in the previous steps?
The current robot state is by default read from "/joint_states". The MoveIt launch file we created earlier will include another launch file which does the remapping... has this worked?

6. Integration with controllers [optional]

You could always launch the MoveIt! planner with the argument load_fake_execution:=true and just not use MoveIt! trajectory execution at all. However, if you would like to use the RViz interface "Plan & Execute", you have to add a moveit_controller_manager.

Again, you may use available helper files in jackage jaco_moveit or copy the launch/config files from there and use them as templates.

You may choose to use two available controllers:

The official MoveItSimpleControllerManager moveit_plugins/moveit_simple_controller_manager
The package moveit_controller_multidof which you need to install separately also supports the virtual joint and therefore mobile robots. The virtual joint movement will be sent on as a path to be navigated by the robot.

For both options, you will need to configure .yaml files with parameters for the controllers. They can be found in the jaco_moveit/config. The .yaml files are loaded from within the launch files you will add in Step 2.

Both options are described in the following.

6.1. Edit your package.xml

Just for completeness, add to package.xml:

<run_depend>jaco_moveit</run_depend>
<run_depend>moveit_controller_multidof</run_depend> <!-- need only if you want to use it! -->

Create a backup file of this package.xml, because when you re-generate moveit files with the setup assistant, it is often overwritten even if you do not have the checkbox for package.xml enabled.

6.2. Load the controller

To use the built-in MoveIt! controller manager, in the existing file
{your-package-name}/launch/{your robot URDF name}_moveit_controller_manager.launch.xml,
include the launch file:

	<launch>
	  <include file="$(find jaco_moveit)/launch/jaco_moveit_simple_controller_manager.launch" />
	</launch>

You may configure the parameters in jaco_moveit/config/moveit_simple_controllers.yaml

Alternatively, if you would like to use the Multi-DOF controller, include the launch file:

	<launch>
	  <include file="$(find jaco_moveit)/launch/jaco_moveit_controller_manager.launch" />
	</launch>

You may configure the parameters in jaco_moveit/config/moveit_multidof_controllers.yaml

You may of course launch the included launch files separately, but adding them to the existing {your robot URDF name}_moveit_controller_manager.launch.xml will launch it with MoveIt! automatically, so his is convenient.

6.3. Test the controllers

Now, you can test if the action connects to your robot:

Step 1. Launch gazebo

roslaunch <your-package> <your-gazebo-launch-file>

Or if you have a more complex launch file which launches either gazebo or the real arm (e.g. jaco_ros/jaco_bringup.launch), use this one instead.

Step 2. Launch moveit

roslaunch <your-robot-moveit-package-name> <robot-name>_moveit.launch

It is important to have the argument load_robot:=false (default) if you are using Gazebo, because Gazebo will already have loaded the robot.