Motion Planning with MoveIt 2

Last week’s IK post left off on a warning: a valid joint solution from /compute_ik tells you the arm’s fingertip can reach a pose, not that it can get there safely. Between “here” and “there” is a path — and if the arm swings through it in a straight line in joint space, it might clip the table, hit itself, or knock over the object it’s about to pick up. That’s the job of a motion planner: given a start state, a goal, and a model of everything in the world, produce a full trajectory that gets there without hitting anything. This week is about how MoveIt 2 does that, and how to ask it to plan (and move) from Python.

What You’ll Learn

• What a planning scene is, and why the planner needs one before it can promise “collision-free”
• The difference between a sampling-based planner (OMPL), an optimizing planner (CHOMP/STOMP), and a Cartesian planner (Pilz)
• How to plan and execute a motion to a pose goal using MoveIt’s Python API (moveit_py)
• Why running the same planning request twice can return two different (both valid) paths
• The pitfalls that make a “successful” plan still the wrong thing to execute

The Planner Needs a World, Not Just a Robot

IK only needs your robot’s kinematic chain — it answers “what joint angles reach this pose” with no idea whether anything is in the way. Motion planning needs more: MoveIt keeps a live model called the planning scene, which combines the robot’s current state (from /joint_states) with every known collision object — the table, a bin, a part on a conveyor — so the planner can reject any path that would intersect one of them. If an obstacle exists in the real world but never gets added to the planning scene (as a collision object published to /planning_scene, typically from a perception pipeline built on last month’s point cloud and depth camera work), the planner will happily plan straight through it — collision-free according to what it knows, not according to reality.

Three Families of Planner

MoveIt doesn’t have one planning algorithm — it has planning pipelines, each wrapping a different underlying library, all reachable through the same request:

Pipeline	Approach	Good at	Watch out for
OMPL (default: RRTConnect)	Sampling-based — randomly grows a tree of reachable states until it connects start to goal	Fast, reliable on almost any robot; needs little tuning	Non-deterministic — the same request can return a different, sometimes longer or jerkier, valid path each time
CHOMP	Optimization — starts from a straight-line guess and iteratively smooths it away from collisions	Deterministic, converges to the same short, smooth path every time	Slower per-query than OMPL; can get stuck in tight, cluttered scenes
STOMP	Optimization — like CHOMP, but explores with randomized trajectory samples instead of a gradient	Comparable quality to CHOMP with fewer tuning headaches; handles noisy cost gradients better	Still an optimizer — no completeness guarantee in very constrained spaces
Pilz Industrial Motion Planner	Analytic — computes exact linear (LIN), joint (PTP), or circular (CIRC) motions	Fast, fully predictable paths for structured industrial moves	Not a general obstacle-avoidance planner — it won’t route around something in the way

The default ompl pipeline with RRTConnect is the right starting point for most projects: it’s what the MoveIt Setup Assistant wires up out of the box, and it needs the least tuning to get a working demo. Reach for CHOMP or STOMP when you specifically need a smoother, more repeatable trajectory (RRTConnect’s paths can look needlessly wiggly), and Pilz when you want a robot arm to move in an exact straight line the way a traditional industrial controller would.

Planning and Executing from Python

MoveIt 2’s Python API, moveit_py, wraps the request/response cycle so you don’t have to build the raw MoveGroup action goal by hand. Here’s a minimal script that plans to a pose goal and executes it:

import rclpy
from rclpy.logging import get_logger
from moveit.planning import MoveItPy
from geometry_msgs.msg import PoseStamped

def main():
    rclpy.init()
    logger = get_logger('motion_planning_client')

    # Instantiate MoveItPy and get the planning group ("arm" from your SRDF)
    robot = MoveItPy(node_name='moveit_py')
    arm = robot.get_planning_component('arm')

    # Plan from wherever the arm actually is right now
    arm.set_start_state_to_current_state()

    # Same target as last week's IK example
    pose_goal = PoseStamped()
    pose_goal.header.frame_id = 'base_link'
    pose_goal.pose.position.x = 0.4
    pose_goal.pose.position.y = 0.0
    pose_goal.pose.position.z = 0.3
    pose_goal.pose.orientation.w = 1.0
    arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link='gripper_link')

    plan_result = arm.plan()
    if plan_result:
        logger.info('Planning succeeded, executing')
        robot.execute(plan_result.trajectory, controllers=[])
    else:
        logger.error('Planning failed — no collision-free path found')

    rclpy.shutdown()

if __name__ == '__main__':
    main()

set_start_state_to_current_state() matters: without it, MoveIt plans from whatever state it last assumed, not from where the arm physically is, which can hand you a perfectly valid plan that starts with a jump. controllers=[] tells execute() to use whichever controllers are already configured for the group rather than naming them explicitly — fine for a single-arm setup, but you’ll list them by name once a robot has more than one controller to coordinate.

Same Request, Different Path

Because the default OMPL pipeline is sampling-based, calling .plan() twice for the identical start and goal can return two different trajectories — both fully valid, both collision-free, but not identical in length or smoothness:

That’s the tradeoff for RRTConnect’s speed: it’s a satisficing planner, not an optimizing one, so it stops at the first path it finds rather than searching for the best one. If your application needs the same repeatable motion every time (a pick-and-place cell where operators expect predictable behavior), that’s exactly when to reach for CHOMP, STOMP, or Pilz instead of accepting OMPL’s default variability.

Common Pitfalls

Trusting a planning scene you never populated. A “successful,” collision-free plan is only as good as the obstacles MoveIt knew about. If a box on the table was never published as a collision object, the planner has no way to avoid it — this is the single most common cause of a real robot hitting something a simulation swore was clear.

Skipping set_start_state_to_current_state(). Forgetting it plans from a stale assumed state instead of the arm’s real position, which can produce a plan that starts with a sudden, unplanned jump the moment you execute it.

Assuming OMPL will give you the same path twice. Code (and safety reviews) that depend on an exact trajectory shape will intermittently fail with the default sampling-based pipeline. Either accept the variability or switch pipelines.

Treating “planning succeeded” as “will finish executing.” A plan can still fail mid-execution — a controller can abort on an unexpected torque, or a dynamic obstacle (a person walking through) can appear that the plan never accounted for. Check the execution result, not just the plan result.

Reaching for Pilz to dodge an obstacle. Pilz’s LIN/PTP/CIRC motions are exact geometric moves, not obstacle-avoiding searches — using it around clutter will drive the arm straight through anything in its path.

Recap

What you want	How to do it
Model the world so the planner can avoid it	Publish obstacles as collision objects to the planning scene
Plan and execute from Python	moveit_py’s PlanningComponent.plan() then MoveItPy.execute()
Fast, general-purpose planning	Default ompl pipeline (RRTConnect) — little tuning, non-deterministic paths
Repeatable, smooth planning	CHOMP or STOMP — deterministic-ish, slower per query
Exact industrial-style moves	Pilz (LIN/PTP/CIRC) — no obstacle avoidance
Avoid a plan that starts with a jump	Always call set_start_state_to_current_state() first

A collision-free trajectory gets the arm to the target pose — but arriving isn’t the same as grasping. Next week closes that loop: Grasping: From Detection to Pick-and-Place.