Python Motion Planning — Core Concepts

Motion Planning vs. Path Planning

Path planning (covered in a separate topic) finds a collision-free route from A to B. Motion planning goes further: it considers the robot’s physical shape, joint limits, dynamics, and timing. A path says “go through these waypoints.” A motion plan says “move joint 1 at this velocity, joint 2 at that velocity, arriving at these configurations at these specific times.”

For mobile robots on flat ground, the distinction is small. For multi-joint robot arms, it is fundamental.

Configuration Space

A robot arm with 6 rotary joints has a 6-dimensional configuration space (C-space). Each dimension represents one joint angle. A single point in C-space completely describes the pose of every link in the arm.

The planning problem in C-space: find a path from the start configuration (current joint angles) to the goal configuration (desired joint angles) that avoids the “C-space obstacles” — the set of configurations where any part of the robot collides with the environment or itself.

The catch: you cannot simply compute the shape of C-space obstacles. For a 6-joint arm, the obstacle boundaries in 6D space are complex and cannot be pre-computed efficiently. Instead, planners sample configurations and check each one for collisions in the physical (workspace) domain.

Kinematics

Forward Kinematics

Given joint angles, compute where the end-effector (gripper) is in 3D space. This is straightforward: multiply the transformation matrices for each joint in sequence. Every robotics library provides this.

Inverse Kinematics (IK)

Given a desired end-effector position and orientation, compute the joint angles that achieve it. This is much harder:

• There may be multiple solutions (the arm can reach the same point from different configurations)
• There may be no solution (the point is out of reach)
• Near singularities (certain configurations), tiny end-effector changes require huge joint movements

IK is solved either analytically (closed-form equations for specific robot geometries) or numerically (iterative optimization). Most Python robotics libraries provide numerical IK solvers.

Sampling-Based Motion Planners

Since C-space obstacles cannot be computed explicitly, the dominant approach samples random configurations and checks them for collisions:

RRT-Connect

An extension of RRT that grows two trees simultaneously — one from the start and one from the goal. At each step, both trees try to grow toward each other. When they connect, a path is found. RRT-Connect is fast for high-dimensional spaces and is the default planner in many robotics frameworks.

PRM (Probabilistic Roadmap)

PRM pre-builds a graph of collision-free configurations and connections. For environments where the obstacles do not change and many queries are needed (like a factory robot repeating different tasks), PRM amortizes the expensive collision checking over many planning queries.

Informed RRT*

Once an initial path is found, Informed RRT* focuses sampling to an ellipsoidal region that could contain better paths, converging to the optimal solution faster than basic RRT*.

Trajectory Optimization

Sampling-based planners find feasible paths but not necessarily smooth or efficient ones. Trajectory optimization refines a path by minimizing a cost function:

• Smoothness: Minimize jerk (rate of change of acceleration) for gentle movements
• Time: Minimize total execution time
• Energy: Minimize joint torques for power efficiency
• Clearance: Maximize distance from obstacles for safety

CHOMP (Covariant Hamiltonian Optimization for Motion Planning) and TrajOpt are popular optimization-based approaches. They start with an initial guess (straight line in C-space) and iteratively push the trajectory away from obstacles while minimizing smoothness costs.

Collision Checking

The computational bottleneck in motion planning. For each sampled configuration, the planner must check if any part of the robot collides with any obstacle. This requires:

1. Computing the position of every link (forward kinematics)
2. Checking geometric intersections between link shapes and obstacle shapes
3. Often simplified using bounding volumes (spheres or boxes around each link)

Efficient collision checking libraries like FCL (Flexible Collision Library) and Bullet use spatial data structures (bounding volume hierarchies) to quickly reject non-colliding pairs. Python bindings to these libraries (through PyBullet or MoveIt) make collision checking fast.

Python Motion Planning Tools

PyBullet provides a physics simulation with built-in collision detection. It can simulate robot arms, load URDF models (the standard robot description format), and check collisions.

OMPL (Open Motion Planning Library) implements dozens of sampling-based planners. Its Python bindings allow configuring and running planners with custom state spaces and collision checkers.

Pinocchio handles kinematics and dynamics computations. It computes forward/inverse kinematics, Jacobians, and dynamics for arbitrary robot models described in URDF.

MoveIt (with ROS 2) provides a complete motion planning framework with perception integration, but its primary interface is C++ with Python wrappers.

Common Misconception

“Motion planning produces the movements you see in factory robots.” Those smooth, precise movements are typically pre-programmed trajectories — the robot follows the exact same path every time. Motion planning is needed when the environment is unknown or changes, like a robot sorting random objects on a conveyor belt or a household robot clearing a cluttered table. In structured factory settings, teach pendants (manual positioning) and playback are still dominant.

One thing to remember: Motion planning for robot arms operates in high-dimensional configuration space where obstacles cannot be pre-computed, so planners sample random configurations, check them for collisions, and build trees or roadmaps of safe movements.