MoveIt! Introduction
Overview
MoveIt! is ROS's most advanced and flexible library for motion planning and manipulation tasks. It integrates state-of-the-art inverse kinematics solvers, path planning algorithms, and collision detection into a single, unified ROS interface.
This document provides a general overview and a practical guide, specifically targeted toward the projects done in the Embedded Systems in Robotics course at Northwestern University.
Tutorials
- The Tutorials for MoveIt! are useful, so take a look.
- I recommended Quickstart, Move Group Python Interface
- For now, you can ignore the setup instructions. If the tutorial does not work, clone
ros-planning/panda_moveit_config
into your workspace, checkout thenoetic-devel
branch, and use thegit
version rather than the installed version - You may also need to clone
https://github.com/ros-planning/moveit_tutorials
Main Components
move_group
node.- The main node used by MoveIt
- Reads all of the configuration files
- Coordinates all the pieces of the motion-planning pipeline
- You can control moveit using this node's ROS API, but for most tasks there is an easier way
move_group
(python) this is the Python API that is used to simplify interaction with themove_group
node, rather than directly using themove_group
ROS API- See a tutorial here
- This is suitable for many common path-planning tasks
move_group
(C++) Is the C++ version of the python API- MoveIt is definitely a C++ first package. It is written in C++ and has python wrappers using the ROS API
- Therefore to get the most out of MoveIt using the C++ API is a must (beyond the scope of this class however)
rviz plugin
MoveIt has special plugins forrviz
to interactively perform motion planning. Mostly useful for debugging as in your programs you will likely be initiating motion planning from code.- When you install moveit, there will be special MoveIt display types that you can add to
rviz
- When you install moveit, there will be special MoveIt display types that you can add to
MoveItCpp
High-level c++ interface that does not use ROS messages as much.- Good for advanced users
Configuration
Semantic Robot Description Format
- An srdf describes information about your robot to help with motion planning
- Accompanies the
urdf
and describes additional information about the joints and links - For example, can tell MoveIt which joints to move as a single group, where the end-effector is, which joints can ignore collision detection
- The
move_group
node needs both aurdf
and ansrdf
to work- the
urdf
is in therobot_description
parameter - the
srdf
is in therobot_description_semantic
parameter
- the
- Both
urdf
andsrdf
files are usually written withxacro
- The MoveIt Setup Assistant can be used to create an SRDF
- The Baxter and Sawyer and Interbotix moveit packages already come with
srdf
files- You may need to pass the proper parameters or even edit these files for your project
Interfacing with the Robot
- Joint states should be published on the
/joint_states
topic - A
robot_state_publisher
should be publishing transforms for the robot. - A control_msgs/FollowJointTrajectory
action server is used to enable the robot to follow the trajectory
- Low-level controllers are used to turn the trajectory into actual movements
- Most robots that support moveit provide the low-level controllers and the action server.
Kinematics
- MoveIt can perform forward and inverse kinematics, as well as computing Jacobians
- It actually relies on external libraries and plugins to perform these calculations, hence it is highly flexible
Kinematics and Dynamics Library
- The Kinematics and Dynamics Library (KDL) from the Orocos project
- kdl_parser converts
urdf
into KDL "chains" which are used by KDL to perform kinematics calculations - KDL is the default kinematics solver in MoveIT
- KDL is relatively easy to configure and widely applicable
- KDL solves inverse kinematics numerically, which is often not the best way
- Slow
- Numerical issues
- kdl_parser converts
IKFast
IKFast (part of OpenRAVE) creates C++
header files that contain functions
for solving the inverse kinematics analytically.
- MoveIT can wrap these generated header files and use them to solve IK.
- MoveIT tutorial for IKFast
- Produces good solutions quickly
- Difficult to setup, not applicable to all robots
- The Theory behind IKFast
- Baxter and Sawyer have IKFast plugins. Not the easiest to setup but they do produce better results in many cases
Trac_IK
- trac_ik is a relatively new inverse kinematics solver
- MoveIt documentation for trac_ik
- developed for DARPA Robotics Challenge
- Better performance than KDL and more reliable
- Designed as a drop-in replacement for KDL
Custom
- It is also possible to implement your own custom kinematics code
- Usually this code is from analytical derivations, often taking advantage of the specific geometry of your robot.
- Two examples are pr2_kinematics and kuka_youbot
Motion Planning
The Open Motion Planning Library (OMPL) implements many different
motion planning algorithms. The move_group
node can use many of these motion planners by loading
OMPL plugins, as specified by configuration files.
Plan Requests and Results
To plan a motion, you send a "plan request" to the move_group
node, including
- Start location (of the joint or end-effector)
- End Location (of the joint or end-effector)
- Kinematic constraints
- The results can be returned as a path or sent directly to the
control_msgs/FollowJointTrajectory
server. - Much of what is done with the
move_group
node can more easily be done directly with themoveit
python API
Pre and Post Processing
- Adapters are used to pre- and post-process the motion plans
- Adapters are implemented as plugins and there are many possibilities
- Pre-processor example:
- If a joint is up against a limit (perhaps to zero it), it starts in a collision state
- The motion planners won't form a plan through a collision
FixStartStateCollision
is a pre-processor that randomly moves your initial condition so you start at a nearby collision-free state
- Post Processor example:
- Motion plans typically just provide joint angle sequences without timing information
AddTimeParemeteriazation
will set the time to reach each joint waypoint, respecting velocity and acceleration constraints
Collision Detection
- In MoveIt, the workspace and any objects that are grasped are represented in a "Planning Scene"
- The
planning_scene_monitor
in themove_group
node determines the ultimate state of the world /joint_states
, the collision elements of theurdf
and the descriptions of self collisions in thesrdf
are used to avoid self collisions- Users can manually add obstacles by publishing to
/planning_scene
or writing appropriate configuration files - RGB-D cameras can be used with the "World Geometry Monitor" to dynamically add obstacles to the scene using octomaps
- Collision detection requires a lot of computation
- Keep collision geometries simple
- Eliminate unnecessary collision checks in the SRDF
Key Configuration Files
Location
- Most projects have similar configuration files that are loaded.
- However, configuration varies based on what robot you are using
- Configuration comes with the robot, however, sometimes it makes sense to modify the default configuration to better meet your needs
- Most configuration is done through the rosparam server
- This means, if you are careful, you can override configuration values at startup in launchfiles
- Often it is easier to fork the package and modify configuration directly
Files
robot_name.srdf.xacro
This is thesrdf
for the robot. Groups joints together, turns off collision detection, etckinematics.yaml
determines which kinematics plugins to use and their settingsompl_planning.yaml
Settings for OMPL. Often you have to search through the source code to determine what a parameter does.- Looking at the code for the MoveIt Setup Assistant helps: moveit_config_data.cpp
joint_limits.yaml
Joint velocity and acceleration limits- The easiest (albeit hacky) way to slow your robot down is to temporarily edit this file
Learning MoveIt
- To get started using MoveIt, go through the tutorials tutorials (see my MoveIt! activity).
- moveit_commander tutorial (the simplest python interface)
- To understand how MoveIt works, read about its concepts (highly recommended).
- Python Interface Tutorial