How to Use MoveIt! with stickbot_lowerleg_3dof

Introduction

This subsection demonstrates the integration of MoveIt! with ControlIt!. Specifically, it shows how MoveIt! can generate a joint trajectory for stickbot_lowerleg_3dof to follow.

Installation

Install MoveIt! Here are the official instructions. It should consist of executing the command below.

$ sudo apt-get install ros-hydro-moveit-full 

Create a MoveIt! Configuration Package

Next, use the MoveIt! Setup Assistant to create a configuration package for stickbot_lowerleg_3dof. Here is the official tutorial. The following steps customizes the tutorial for our demo:

Start MoveIt! Setup Assistant:

$ roslaunch moveit_setup_assistant setup_assistant.launch 

The following GUI should appear:

Click on the button "Create New MoveIt Configuration Package".

Under "Load URDF or COLLADA Robot Model", click on the button "Browse", then search:

[workspace path]/hcrl_robot_models/stickbot_lowerleg_3dof/models/stickbot_lowerleg_3dof/urdf/stickbot_lowerleg_3dof.urdf 

Finally, select "Load Files". You should see the message "Success! Use the left navigation pane to continue." as shown below.

On the left-side of the MoveIt! Setup Assistant, click on "Self-Collisions". Then click on "Regenerate Default Collision Matrix". You should see the following:

On the left-side of the MoveIt! Setup Assistant, click on "Planning Groups", then on the button "Add Group". For the Group Name, enter "leg". For the Kinematic Solver, select "kdl_kinematics_plugin/KDLKinematicsPlugin". Here is what you should see:

Click on the button "Add Joints", then select joints {ankleRoll, anklePitch, knee} as shown below.

Click on the button "Save". This will bring you back to the "Planning Groups" tab. Under "leg", select "Links", then click on the "Edit Selected" button. Select links {ankleLink, lowerLeg, upperleg} as shown below:

Click on the button "Save". This will bring you back to the "Planning Groups" tab. Under "leg", select "Chain", then click on the "Edit Selected" button. For the Base Link, enter "foot". For the Tip Link, enter "upperleg". You should see the following:

Click on the button "Save". This will bring you back to the "Planning Groups" tab. It should now look like the following:

On the left-side of the MoveIt! Setup Assistant, click on "Configuration Files", then on button "Browse". Click on [workspace path]/hcrl_robots_moveit_config, then "Create Folder". Name it "stickbot_lowerleg_3dof_moveit_config" then click the "Open" button. You should now see the following:

Finally, click on the button "Generate Package". You will see a warning indicating that no end effectors have been added. Close and disregard it. This is because MoveIt will default to using the end of the kinematic chain as an end effector. In this case, stickbot_lowerleg_3dof contains a single chain.

Exit the MoveIt! Setup Assistant

You should now have package "stickbot_lowerleg_3dof_moveit_config". The following commands should result in the path to this package being printed to the terminal:

$ source ~/.bashrc
$ rospack profile
$ rospack find stickbot_lowerleg_3dof_moveit_config

Connect MoveIt! to a ControlIt! Controller

The next step is to connect MoveIt! to a ControlIt! controller. Here is the official tutorial. The instructions below customize the instructions for our demo.

Create a text file stickbot_lowerleg_3dof_moveit_config/config/controllers.yaml:

$ subl `rospack find stickbot_lowerleg_3dof_moveit_config`/config/controllers.yaml 

Save the following YAML text in this file:

controller_list:
 - name: stickbot_lowerleg_3dof_controller/trajectory_follower
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
     - ankleRoll
     - anklePitch
     - knee

Open stickbot_lowerleg_3dof_moveit_config/launch/stickbot_lowerleg_3dof_moveit_controller_manager.launch.xml. Save the following text in this launch file:

<launch>
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
  <param name="/move_group/moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
  <rosparam file="$(find stickbot_lowerleg_3dof_moveit_config)/config/controllers.yaml" ns="move_group"/>
</launch>

Open stickbot_lowerleg_3dof_moveit_config/launch/demo.launch and modify it to contain only the following:

<launch>

  <include file="$(find stickbot_lowerleg_3dof_moveit_config)/launch/stickbot_lowerleg_3dof_moveit_controller_manager.launch.xml"/>

   <node pkg="nodelet" type="nodelet" name="trajectory_follower" args="standalone rapid_locomotion_planner/TrajectoryFollowerNode" respawn="true" output="screen" ns="stickbot_lowerleg_3dof_controller">
      <remap from="/pos_cmd" to="JPosTask/goalPosition"/>
  </node>

  <!-- By default, we do not start a database (it can be large) -->
  <arg name="db" default="false" />

  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />

  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find stickbot_lowerleg_3dof_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <!-- If needed, broadcast static tf for robot root -->
  

  <!-- We do not have a robot connected, so publish fake joint states -->
  <!--<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/> 
    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
  </node>-->
  
  <!-- Given the published joint states, publish tf for the robot links -->
  <!--<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />-->

  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find stickbot_lowerleg_3dof_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>  
    <arg name="fake_execution" value="false"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find stickbot_lowerleg_3dof_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- If database loading was enabled, start mongodb as well -->
  <include file="$(find stickbot_lowerleg_3dof_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"/>

</launch>

Run the Demo

Next, start the simulation as usual:

$ roslaunch stickbot_lowerleg_3dof_controlit simulate.launch

Press "play" in Gazebo and verify the robot goes into its initial pose.

Start MoveIt!:

$ roslaunch stickbot_lowerleg_3dof_moveit_config demo.launch 

This will launch rViz with MoveIt's Motion Planning plugin as shown below. Notice the interactive marker on the robot's knee.

Rotate the interactive marker in RViz. For example, in the screenshot below, we modified the marker to indicate that we want the knee to bend more. The orange model is the goal state whereas the gray model is the current state.

In RViz, under the section "Motion Planning", click on the tab "Planning". Then click on the button "Plan". You should see an animation showing the trajectory the robot will follow to go from its current state to the goal state. Here is a screenshot of what you should see:

Finally, click the button "Execute" and observe MoveIt! make the robot move to the goal position you specified by following the planned trajectory.

Notes

  1. MoveIt! communicates with ControlIt! using ROS actionlib. We're using an adapter in rapid/rapid_locomotion_planner/src/trajectory_follower_node.cpp that converts the action into publication on ROS topic /stickbot_lowerleg_3dof_controller/JPosTask/goalPosition.