UP | HOME

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-iron-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 to this library but they do not yet exist in ROS 2
  • MoveItCpp is C++ API for MoveIt. Rather than communicating with a separate move_group node, it essentially makes the node you are using into the move_group node.
    • One advantage is that there can be less overhead 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
  • 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
    • We are using the moveit fork where this is being developed (in the nuws. It might be ready by the end of October
  • rviz plugin MoveIt has special plugins for rviz 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
  • 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 a urdf and an srdf to work
    • the urdf is in the robot_description parameter
    • the srdf is in the robot_description_semantic parameter
  • Both urdf and srdf files are usually written with xacro
  • 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.
  • The /execute_trajectory action (of type moveit_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 and the compute_fk services
    • Note that the current state of the robot is always known via the /joint_states topic and the tf location of the end-effector

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

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 service (/plan_kinematic_path) plans a general path and returns it as the response
  • More commonly the action interface is used (/move_action)
    • The move_action allows specifying:
      1. The start location (of a link, the joints, or the end-effector)
      2. The End Location (of a link, the joints, or end-effector)
      3. Kinematic constraints
  • The results can be returned as a path or executed immediately
  • Because this is an action, the planning/motion can be interrupted and paused

Pre and Post Processing

ROS 2

  1. 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 the move_group node determines the ultimate state of the world
  • /joint_states, the collision elements of the urdf and the descriptions of self collisions in the srdf 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:
    1. 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 the srdf for the robot. Groups joints together, turns off collision detection, etc
  • kinematics.yaml determines which kinematics plugins to use and their settings
  • ompl_planning.yaml Settings for OMPL. Often you have to search through the source code to determine what a parameter does.
  • 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

Author: Matthew Elwin