Strengths, Weaknesses, and Developer Insights
ROSCon Hamburg 2015

Dave Coleman
dave@dav.ee

Overview

Establish Credibility 1min
Background of MoveIt! 5min
What it's done well 5min
Typical use patterns 5min
Demystifying complexity 5minAmazon Picking Challenge 5min
Where MoveIt! needs improvement 4min
Future Roadmap 10min
Q&A 5minEstablish Credibility 1min
● PhD Student At CU Boulder with Nikolaus Correll● Interned with E. Gil Jones & Ioan Sucan at Willow Garage○ Created Setup Assistant● Have used and contributed to MoveIt! since before it wasreleased● Am a MoveIt! maintainer● Have contributed to OMPL and many other ROS packages

Background of MoveIt! 5minEasy to use framework for motion planning, manipulation, 3D
perception, kinematics, control and navigation● Created at Willow Garage by
Ioan Sucan, Sachin Chitta,
many others● Collaboration between manyorganizations● Predecessor: arm_navigation
announced in March 2010
● 31 contributors to moveit_core● Written in C++ with Python
bindings● https://github.com/ros-planning/
1:00

240 people registered, 150 who attended

It's popular.
● #3 Package in ROS (ROS survey)● 700 Members on Mailing list
● Number of installations 2015: 10,089● ICRA 2015
○ 11 Papers cited/used MoveIt!
● IROS 2015
○ 5 Papers cited/used MoveIt!
● Has been run on over 65 robots worldwide

Community

Exciting Developments● Benchmarking getting rewrite○ Mark Moll / Kavraki Lab● STOMP being revived○ Jorge Nicho / SwRI

● Descartes Cartesian Planner○ Shaun Edwards / SwRI

● Collision detection plugin○ Michael Ferguson / Fetch● New release maintenance manager○ Michael Ferguson / Fetch

What it's done well 5min

6:00

Setup AssistantRviz Motion Planning Plugin

Comparing research libraries● OMPLOpen Motion Planning Library● SBPL
Search Based Planning Library● PCDProximity Collision Detection● CHOMPCovariant Hamiltonian
Optimization● IKFast
Analytical Inverse Kinematics
Solver● STOMP
Stochastic Trajectory
Optimization● KDL
Kinematics Dynamics Library Inverse Kinematics● FCL
Fast Collision Checking Library● Octomap
3D occupancy grid mapping

Cohen, B.; Sucan, I.A.; Chitta, S., "A generic infrastructure for benchmarking motion planners," inIntelligent Robots and Systems (IROS), 2012

Robot Agnostic

Flexibility (but also complexity)● Can Handle:○ Groups of joints○ Multivariable joints○ Mimic joints● Notions of:○○○○

Cartesian-Space Planning
Joint-Space Planning
Orientation Constraints
Visibility Constraints

Typical Use Patterns 5min

11:00

Rviz Motion Planning Plugin

Known commands:

Commander> a = current> go rand> wait 5> plan ahelp

show this screen

id|whichdisplay the name of the group that is operated onload [<file>]load a set of interpreted commands from a filesave [<file>]

save the currently known variables as a set of commands

use <name>switch to using the group named <name> (and load it if necessary)use|groups

show the group names that are already loadedvars

display the names of the known statesshow

display the names and values of the known statesshow <name>display the value of a state

record <name>record the current joint values under the name <name>delete <name>forget the joint values under the name <name>current

show the current state of the active groupconstrain <name>use the constraint <name> as a path constraintconstrain

clear path constraints

eef

print the name of the end effector attached to the current groupgo <name>plan and execute a motion to the state <name>go <dir> <dx>|plan and execute a motion in direction up|down|left|right|forward|backwar

go randplan and execute a motion to a random stateplan <name>plan a motion to the state <name>executeexecute a previously computed motion planrotate <x> <y> <z>plan and execute a motion to a specified orientation (about the X,Y,Z axe

tolerance

show the tolerance for reaching the goal region

tolerance <val>

set the tolerance for reaching the goal region

wait <dt>sleep for <dt> seconds

x = y

assign the value of y to x

x[idx] = val

assign a value to dimension idx of x

x = [v1 v2...]

assign a vector of values to x

trace <on|off>enable/disable replanning or looking aroundgroundadd a ground plane to the planning scene

allow replanning <true|false>

enable/disable replanningallow looking <true|false>

enable/disable looking around"move_group" Python Interface
group = moveit_commander.MoveGroupCommander("left_arm")
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
plan1 = group.plan()"move_group" C++ Interface
moveit::planning_interface::MoveGroup group("right_arm");
geometry_msgs::Pose target_pose;target_pose.orientation.w = 1.0;
target_pose.position.x = 0.28;
target_pose.position.y = -0.7;
target_pose.position.z = 1.0;
group.setPoseTarget(target_pose);
moveit::planning_interface::MoveGroup::Plan my_plan;bool success = group.plan(my_plan);

ROS Services & Actions

Pro-Tip: Use C++ classes individuallyrobot_model_loader_.reset(new robot_model_loader::RobotModelLoader("robot_descrition"));robot_model_ = robot_model_loader_->getModel();planning_scene_.reset(new planning_scene::PlanningScene(robot_model_));tf_.reset(new tf::TransformListener(nh_));psm_.reset(new planning_scene_monitor::PlanningSceneMonitor(planning_scene_, robot_model_loader_, tf_,"my_scene"));psm_->startStateMonitor("/joint_states", "");
psm_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::
UPDATE_SCENE, "my_planning_scene");visuals_tools_.reset(new MoveItVisualTools(robot_model_, planning_scene_monitor_));planning_pipeline_.reset(new planning_pipeline::PlanningPipeline(robot_model_ nh_,"planning_plugin", "request_adapters"));trajectory_execution_manager_.reset(new trajectory_execution_manager::
TrajectoryExecutionManager(robot_model_));Best for researchers who want to modify code

Demystifying Complexity 5min

16:00

Many Plugins

Planning Scene Monitor

Planners
OMPL

SBPL

CHOMP/STOMP●
●

●
●
●
●
●
●

●
●

●
●
●
●
●

●

Lydia Kavraki's lab
Sampling-basedplanners
Stochastic
Probabilisticallycomplete
Typically no optimality
guarantees
Computationally fastMore reliable runtime
for real-world
applications
Many variants ofalgorithms available●●●

Maxim Likhachev's lab
Graph-based planners
Deterministic
Resolution complete
Optimality guarantees
Requires pre-processing
phaseComputationallyexpensive
More reliable solutions
for real-world
applications
Renewed work fromMichael Ferguson●●

Kalakrishnan et al
Optimization-basedplanner that generates
smooth well behaved
collision free motion
paths in reasonable time
Can incorporate
additional objectivefunctions - collision
avoidance andsmoothness
CHOMP being
resurrected by ROS
Industrial groupExperience Planners

Planner Request Adapters● AddTimeParameterization○ Modifies the timestamps of a kinematic (position-based)trajectory to respect velocity and acceleration constraints○ Uses iterative parabolic time parameterization● FixWorkspaceBounds○ If no minimum workspace bounds is specified, sets to a default● FixStartStateBounds○ Tweaks joints to not be outside joint limits○ Accounts for floating point and encoder noise● FixStartStateCollision○ Tweaks start state to not be in collision with environment○ Creates a new planning request with modified start state● FixStartStatePathConstraints○ Plans separate path from invalid start state to valid start state
Adapts research theory to real world hardware

IK Solvers● KDL○ Kinematics Dynamics Library, OROCOS● IKFast○ OpenRave Analytical● Robot-specific custom solvers○ PR2

Amazon Picking Challenge 5min

21:00

Workspace Analysis

Baxter Parallel Electric Gripper

Yale OpenHand 3-Finger Gripper

Kinova Jaco2 + 1m Vertical Gantry

Architecture(Pretty Standard)

DepthMap
ROS Msg

SDF Fusion

EmbeddedControllers

2x Asus Xtion Pros

Spatio-Temporal
Segmentation

Jaco2 +1m VerticalGantry

Custom Obj
ROS MsgUSB

BoundingBox

PickNikManager

PlanningScene
ros_control

Velocity-based
Trajectory Controller
ManipulationManager

GraspGenerator

GraspFilter

GraspPlanner
Joint Trajectory
ROS Msg

KDL CartesianPlannerOMPL
RRTConnect

Trajectory ActionServerMotion Planninghttps://www.youtube.com/watch?v=isVq-IfGUTw&feature=youtu.beChallenge Takeaways
●●
●●
●
●
●
●

Simplest possible grasping → suction
Low cost hardware → visual servoing
Reduce calibration needs
2 mobile bases won → larger workspace
Slim arms → better reachability
Good visualizations → introspection and development
Perception and manipulation teams must work closely
Test whole system working together often

MoveIt! used by at least 10 teams:○ PickNik, Z.U.N., University of Washington, Team IntBot,
NUS_SMART_HAND, Team Applied Robotics, Team WPI,
University of Alberta Team, Plocka Packa, Team CVAP
None of the winning teams used MoveIt!

Team RBO - 1st PlaceTU Berlin
148 pointsBarrett WAM arm●
Backdrivability key toskillful interactions with theenvironment
Nomadic XR4000 mobile base●Omnidirectional /holonomic●
Very large workspace
Did not rely on motion planningHybrid automaton composed ofsequences of controllers withsensor-based transitions.
Vacuum attachment tool withsuction cup drilled into side offender“Simple but robust” RGB object
recognition algorithm

Team MIT - 2nd Place
MIT88 points
ABB 1600ID●
Sub-millimeter precision●Internal canals for cables
Custom dual-purpose end effector●Aviation-grade aluminum●Spatula-like finger nail
●Suction also
Used MIT Drake (LocomotionGroup at MIT) for motion
planning
Automatically chooses whichmotion primitive to use basedon dynamics simulator●grasp, suck, scoop,toppling, push-rotate
Kinect2 cameras mounted onframe, Realsense on arm
Outsourced perception to arobotics startup - Caspen
Robotics

Team Grizzly - 3rd Place
Oakland University
w/Dataspeed Inc.
35 points
●
●
●
●
●

Rethink Baxter
Custom Mobile Base
Yale OpenHand
Suction gripper
Kinect2 on Head

Custom Cartesian motion
planning algorithm accepted
position and orientationcommands from the perception
systemWhere MoveIt! needs improvement 4min(and where you can help!)

26:00

Motion Planner Reliability● Sometimes fails with difficult to understand explanations● Sometimes generates very suboptimal paths
Solutions:● Hybridize several planning attempts (threads)● Plan with cost functions, e.g. RRT*, PRM*
● Increase the time MoveIt! spends on smoothing paths● For some applications, planners other than OMPL's defaults
are better● Improve user feedback to diagnose setup issues○ Check your joint space is parameterized correctly (<2pi)○ Introspection tools

Obstacle Clearance
Can generate plans that come very close to obstacles
Solutions:● Add out of box support for biasing trajectories away fromobstacles● Cost-based OMPL, STOMP, CHOMP

Grasping Support
Difficult to generate grasps in MoveIt!
Solution:● Provide default URDF + SRDF compatible
grasp generator● Clearer documentation on how tointegrate third party grasping pipelineDocumentation● Need more exhaustive documentation fromcommunity support (you!)● Tutorials for how to use MoveIt! beyond
quick start demo
● Make it easier for our many users tocontribute backFuture Roadmap 10min
Community meeting's end of year goals● Integrate better support for humanoid kinematics● Integrate benchmarks updates● Resurrect support for other types of planners(STOMP)

30:00

Visual Servoing Support● Once a trajectory is planned, no easy way to integrate visual ortactile feedback
Solution:● Position/pose-based visual
servoing (PBVS)● Hooks to modify plan basedon alignment of target object● Ability to add meta-data to
trajectories indicating whento use VS, what objects totrack● Requires much tighter
coupling with controllers,planners, and perception
systemPlanning with Behaviors
Cartesian Straight Line+ Impedance Control
Grasp Tool
Cartesian Straight Line+ Visual Servoing+ Impedance Control

Free Space Planning
OMPLFree Space Planning
OMPL

Semi-constrained
trajectory using
Descartes

Cartesian Straight Line+ Impedance Control

Sense-Plan-Act & ROS Control
PointCloud2
ROS Msg

Octomap &Planning Scene

Realworld

Image/Depth
SensorPosition-based
Trajectory

Geometric Planner

Position, Velocity,
Acceleration
TrajectoryTime
Parameterization

Trajectory ActionServerEmbeddedControllers

Joint Trajectory
ROS Msg

Velocity-based
Trajectory Controller

Real-time ReactiveControl●●●●●Ethernet,etc

Faster connection for streaming commands
Integrate ros_control with Setup AssistantRename MoveIt ControllerManager to ControllerInterface
More advanced plugin than SimpleControllerManager
Switching controllers

Affordance Templates
Human in the loop tools for high level commands such as
more sophisticated interactive markers

Calibration● Integrate better calibration packages○ ROS Industrial - industrial_calibration○ Fetch Robotics - robot_calibration● More clearly document how this should be integrated

Code Frequency
moveit_core2012

2013

2014

WG Closes

moveit_ros

2015Stability vs. Progress● MoveIt! needs to stay current● Other motion planning frameworks are very capable○ OpenRave, MIT Drake, MuJuCo + Whole Body Planning, etc

Distributed Software Collaboration Is Hard
● Currently we have 50 open pull requests● Need continuous integration badly● Need more simulation tests
If there are good features worth upgrading
to, breaking changes are tolerable.

Proposal: Consolidate to One Repo

Too many repos to keep synced
moveit_core

packages not yet in ros-planning group:

moveit_ros

moveit_benchmarks

moveit_planners

moveit_visual_tools

moveit_docs

moveit_simple_grasps

moveit_msgs

moveit_python

moveit_robots

moveit_web

moveit_ikfast

moveit_whole_body_ik

moveit_commanderindustrial_moveit

moveit_kinematic_tests
moveit_advanced
moveit_setup_assistantmoveit_metapackages
moveit_plugins
moveit_resources
moveit_pr2

Plus many ros-planning packages not prefixedwith moveit_*Overall● MoveIt! is awesome● Successful because it is easy for beginners● Needs many more features and improvements● Stability (stagnation) should not be the #1 focus● Please contribute!

Q & A 5min
Do you like to MoveIt MoveIt?

Thanks to Mike Ferguson, Sachin Chitta, Ioan
Sucan, Shaun Edwards, Jon Bohren, Conor
Brew, Acorn Pooley, Dave Hershberger, Chris
Lewis, Jorge Nicho, Ben Chretien, Adolfo
Rodriguez. Kei Okada, Stefan Kohlbrecher, andmany more...
40:00