MoveIt 2 Introduction
Overview
MoveIt 2 is ROS's motion planning and manipulation library. 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.
MoveIt is a large package and there are several methods of interacting with it. We will primarily use the python MoveIt API, but it is also accessible from C++ and via ROS 2.
Getting Started
Install moveit: sudo apt install ros-jazzy-moveit
.
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
- Documentation for move_group Node
- move_group_interface is a C++ library that makes interacting with the move_group node easier.
- It is analogous to how Broadcasters and Listeners make interfacing with the
/tf
topic easier even though you could publish/subscribe directly. - In ROS 1 there were python bindings that made using the ROS API easier, but this does not exist in ROS 2 (yet).
- It is analogous to how Broadcasters and Listeners make interfacing with the
- MoveItCpp is C++ API for MoveIt.
- Rather than communicating with a separate
move_group
node, it essentially makes the node you are using into themove_group
node. - One advantage is that there can be less latency related to sending messages back and forth between your node and the move_group node.
- One disadvantage is that it is more complicated and requires configuring the node properly with the numerous MoveIt options, and there is not necessarily one central location for tracking robot state.
- Rather than communicating with a separate
- moveit_py Python Library.
- A python library (wrapping the MoveItCpp library) is officially released as a part of
moveit
- It does not currently have all the functionality we require.
- A python library (wrapping the MoveItCpp library) is officially released as a part of
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
Configuration
- While the specifics may differ, the concepts between MoveIt and MoveIt 2 are similar so there is still useful information to be gleaned from the MoveIt! documentation
Semantic Robot Description Format
- An srdf describes information about your robot to help with motion planning
- An SRDF 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 robot manufacturer typically provides the SRDF file
Interfacing with the Robot
MoveIt is highly configurable, but I describe the default expected interface here.
- Joint states should be published on the
/joint_states
topic. - A
robot_state_publisher
should be publishing transforms for the robot and the/robot_description
topic. - The
/execute_trajectory
action (of typemoveit_msgs/action/ExecuteTrajectory
) tells moveit to execute the trajectory - MoveIt executes a trajectory by interacting with low-level controllers (typically provided via the ros2_control package).
- 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 for you.
- Depending on the requirements, integrating an existing controller into the robot requires C++.
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
- The
move_group
node provides the/compute_ik
(of type moveit_srvs/srv/GetPositionIK and the/compute_fk
(of type GetPositionFK) services- Note that the current state of the robot is always known via the
/joint_states
topic and thetf
location of the end-effector
- Note that the current state of the robot is always known via the
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, send a "plan request" to the move_group
node.
- The (
/move_action
)- The
move_action
allows specifying:- The start location (of a link, the joints, or the end-effector)
- The End Location (of a link, the joints, or end-effector)
- Kinematic constraints
- The
- The results can be returned as a path or executed immediately
- Because this is an action, the planning/motion can be interrupted and paused
- The service (
/plan_kinematic_path
) can also be used to plan a general path and returns it as the response
Pre and Post Processing
ROS 2
- Post-processing enables you to adjust the timing to the path, according to jint_lmits.
ROS 1
- 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, but the exact structure is project-dependent.
- 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 ROS parameters associated with the node implementing the
move_group
. - To change parameters there are a few options:
- Forking the package and modifying the parameters (it is unfortunate that we need to resort to this but often it is the easiest and most reliable way)
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
- MoveIt Concepts documentation explains various aspects of how MoveIt works
- The primary MoveIt 2 documentation has a few tutorials, but let's just say that they are currently quite immature.
- Along with the documentation, the MoveIt Activity is designed to get you up to speed with how
MoveIt
works - The documentation for MoveIt is still evolving rapidly, therefore experimentation is necessary