Introduction to MoveIt!

KDL is the Kinematics and Dynamics Library; it is part of the Orocos project. Via the kdl_parser package, URDF descriptions of robots can be directly converted into KDL "chains", and then KDL libraries may be used for performing these calculations. This is the default strategy in MoveIt!, and likely the most common.

• Pros
  • Easy-to-configure
  • Works for tons of robots
• Cons
  • All IK solutions are purely numerical (they may be slow, and may suffer from numerical instabilities and difficulties finding solutions)

IKFast is part of the of the OpenRAVE project. Ideally, you can use IKFast to scan the topology of your robot, and then output C++ headers that provide reliable, stable, and fast robot arm kinematics calculations. Once these headers are available, they can be wrapped in MoveIt! APIs to enable their functions to be called by move_group. The ROS wiki has a decent tutorial on creating a MoveIt! kinematics plugin from IKFast. As an example IKFast plugin, check out the baxter_ikfast_left_arm_plugin (there is also a right arm plugin).

• Pros
  • Extremely fast, stable IK solutions.
• Cons
  • Can be difficult to setup, and may not work for all robots.

This package was announced in November 2015, and it's benchmarking results indicate that it could be a much more reliable alternative to KDL. Several students have used it in the past, and it has worked great.

For some robots, it can be advantageous to hand-code kinematics plugins for your robot. This allows you to know exactly how your code is working, and to tailor your solutions to your specific use-case. For examples, check out the pr2_kinematics and the youbot_arm_kinematics_moveit packages.

OMPL is the "Open Motion Planning Library". It provides implementations of a variety of motion planning algorithms. MoveIt! has created move_group plugins for many of these planners so that you only need to provide configuration files to be able to use the planners.

You send "plan requests" to your motion planners (typically OMPL) through the move_group node. These requests typically specify start and end locations in either joint space or Cartesian space. They may also specify a variety of kinematic constraints. The planner then searches for a collision free path from the start to the goal. You can then either just ask for the computed path to be returned, or you can have move_group forward the planned path onto your robot's control_msgs/FollowJointTrajectory action server.

MoveIt! uses a variety of "plan adapter" plugins to pre- and post-process planning requests and results. Some of these adapters are simple heuristics that are designed to help deal with real-world nuisances that can cause issues for the planners. For example, let's say you start in a pose that has the robot in self-collision, then the planner will never succeed because there is no collision-free path. However, this self-collision state may be a perfectly acceptable state, and it may be exactly the situation that you are interested in. For example, let's say your robot is resting on a joint limit, and it was intentionally put at that joint limit during a calibration procedure. Then the FixStartStateCollision plan adapter plugin may be used to randomly perturb the initial condition by small amounts to try and find a nearby, collision-free start state.

Likely the most important adapter for real-world robot execution is the AddTimeParameterization adapter. Most planners simply return sequences of joint angles; there is no specification of how fast the arm should move between these waypoints. This plugin time-parameterizes the planned path by applying velocity and acceleration constraints.

The robot itself, and the world around the robot (including grasped objects) are represented in the "Planning Scene". The move_group node implements a planning_scene_monitor that listens to several different sources to determine what the state of the world around the robot is.

1. The /joint_states topic plus the collision geometries specified in the URDF and the semantic collision descriptions in the SRDF are all used in the planning scene.
2. Various 3D sensors can be combined with the "World Geometry Monitor" to automatically add sensed collision objects to the planning scene.
3. Users may manually add various collision primitives and meshes to the planning scene via the /planning_scene topic.

The actual collision detection algorithms are relatively transparent to the user. The user simply has to setup the planning scene well, and the move_group node makes calls to the "Flexible Collision Library" to determine collision states. Note that collision detection is computationally expensive. Care should be used in setting up the SRDF and the planning scene to minimize the number of collision checks that happen.

MoveIt! requires many configuration files. The following files are the most important files to analyze for this project. For Baxter, all of the files are part of the moveit_robots package, and they are located in moveit_robots/baxter/baxter_moveit_config/config. For Sawyer, all of these files can be found in sawyer_moveit_config/config. There are many other files in each of these repositories, but I think these are the most important.

baxter.srdf/sawyer.srdf

This is the SRDF file for Baxter/Sawyer that is used by default. This SRDF defines planning groups for each of the arms, and a planning group for both arms together. There are several group states that define "neutral" poses that can be directly planned to, i.e., you can refer to them by name rather than specifying all of the joint angles. One way you could leverage this file is by specifying your own named group states. Typically, we don't use this SRDF file directly; instead, we use xacro to build a properly configured SRDF at run-time from the baxter.srdf.xacro file. For Sawyer, see the sawyer.srdf.xacro file. One of the most common issues that people will encounter is having SRDF files that don't match the state of the grippers attached to the end of the robot's arm(s). For example, let's say you have grippers plugged into a Rethink arm, then your URDF should also have grippers (via the mutable_robot_state_publisher) and the robot will automatically publish the gripper position on the /robot/joint_states topic. You then need to also have an SRDF that properly represents these grippers or MoveIt! will constantly think that your fingers are in collision with the end of the arm (the SRDF would explicitly ignore this collision).

kinematics.yaml

This file specifies the inverse kinematics solvers for the robot. Baxter has been configured to use either IKFast or KDL. If you are having issues with IK, you can try editing this file. Sawyer doesn't currently have an IKFast plugin (that I'm aware of), but you could still edit this file to try a different IK solver (like trac_ik or bio_ik).

ompl_planning.yaml

This is the file that defines a variety of planner configurations for each of the move groups. Frankly, the parameters that are available for each planner are not well documented. In my opinion, your best bet is to search through source code, and refer to OMPL documentation. The best documentation I've found is in the MoveIt! Setup Assistant source code. Currently the default planner for Baxter is RRT Connect which, in my experience, works well for Baxter. Sawyer's planner file includes some nice comments about what the planner options mean. It doesn't explicitly set a default planner, so I believe it will default to the SBL planner.

joint_limits.yaml

This file defines the velocity and acceleration limits for each of the robot's joints. Looking at the values in this file, it is clear that these limits are not necessarily physically realistic. If you are having issues with the Joint Trajectory Action Server preempting because execution errors are too high, you could try lowering some of these limits. Note that Sawyer's copy of this file includes some commented out choices for "fast" and "slow" limits. Of course, you could also use the move_group/trajectory_execution/execution_velocity_scaling dynamic reconfigure parameter to modulate the execution speed.

There are many launch files in the PATH_TO_WORKSPACE/src/moveit_robots/baxter/baxter_moveit_config/launch directory (or the sawyer_robot/sawyer_moveit_config/launch directory). However, the most important file is the move_group.launch file. This includes many other launch files, and those launch files eventually include the configuration files from the config directory. Note, this file may not work out-of-the-box with either robot. There are so many things that can be configured and must be configured correctly that you should expect to need to configure this launch file, and possibly other launch files. Don't be afraid to either fork these repos and push your edits to your fork, or to copy the files that need edited into your package. Here are a few primary things to look out for:

Gripper Settings

If your SRDF that gets loaded by your launch file stack does not include the grippers for the robot, but you are connected to a robot that has grippers you may have issues motion planning because MoveIt! will always detect collisions between parts of the gripper and the end of the arm. This would be because the URDF has collision objects associated with the grippers and you haven't explicitly told MoveIt! to ignore these collisions. Be sure that the URDF and SRDF agree with each other. To control this, you can use arguments to the SRDF xacro files, if you are not connected to a real robot you can use arguments to the URDF xacro files, and if you are connected to a real robot you can use the mutable_robot_state_publisher.

Plugin List

The move_group node used to look at a capabilities parameter to decide which plugins to load. Since Kinetic, all plugins are loaded automatically so this parameter isn't strictly necessary, but the launch files for both robots still set this parameter, and you may encounter some issues with legacy plugins. The one I for sure know can cause problems is the MoveGroupExecuteService plugin. This used to be a plugin that offered a service call to execute a trajectory that had been planned with MoveIt!, but it is now deprecated. If you have references to this plugin in your launch file you'll want to delete this. The replacement plugin is MoveGroupExecuteTrajectoryAction. Check out my change to Sawyer's move_group.launch.

Joint States

On both robots, the joint states are published on /robot/joint_states topic and the default launch files have multiple remapping steps to ensure that nodes are looking at this topic instead of /joint_states. One issue I've encountered occasionally is related to the fact that the robots publish joint state information on that topic for the arm and for the grippers separately. I've had numerous issues when I have grippers connected related to move_group complaining that it cannot get complete robot states. The way I've typically fixed this is by using the joint_state_publisher to assemble the messages containing incomplete joint state information and publish as complete joint state information on the normal /joint_states topic. Check out the current state of my launch files to see how I've implemented changes to deal with this.