Python Motion Planning — Deep Dive

Implement forward/inverse kinematics, RRT-Connect, trajectory optimization, and task-space planning in Python with PyBullet, Pinocchio, and OMPL for robot arm manipulation.

Forward Kinematics with DH Parameters

The Denavit-Hartenberg (DH) convention describes robot geometry as a chain of transformations. Each joint has four parameters: d (link offset), θ (joint angle), a (link length), α (link twist):

import numpy as np

def dh_transform(theta, d, a, alpha):
    """Compute 4x4 homogeneous transformation from DH parameters."""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)

    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d],
        [0,   0,      0,     1]
    ])

def forward_kinematics(joint_angles, dh_params):
    """Compute end-effector pose from joint angles.

    dh_params: list of (d, a, alpha) for each joint
    joint_angles: list of theta values
    """
    T = np.eye(4)
    for theta, (d, a, alpha) in zip(joint_angles, dh_params):
        T = T @ dh_transform(theta, d, a, alpha)
    return T

# Example: 6-DOF robot arm (simplified UR5-like)
ur5_dh = [
    (0.0892, 0,       np.pi/2),   # joint 1
    (0,      -0.425,  0),          # joint 2
    (0,      -0.3922, 0),          # joint 3
    (0.1093, 0,       np.pi/2),    # joint 4
    (0.0947, 0,      -np.pi/2),    # joint 5
    (0.0823, 0,       0),          # joint 6
]

angles = [0, -np.pi/4, np.pi/4, 0, np.pi/2, 0]
T_end = forward_kinematics(angles, ur5_dh)
position = T_end[:3, 3]
rotation = T_end[:3, :3]

Numerical Inverse Kinematics

When analytical IK is unavailable, iterative methods find joint angles for a desired end-effector pose:

def jacobian_ik(target_pos, target_rot, dh_params, q_init,
                max_iter=1000, pos_tol=1e-4, rot_tol=1e-3):
    """Damped least squares (Levenberg-Marquardt) IK solver."""
    q = np.array(q_init, dtype=float)
    n_joints = len(q)
    damping = 0.01

    for iteration in range(max_iter):
        T_current = forward_kinematics(q, dh_params)
        pos_error = target_pos - T_current[:3, 3]

        # Rotation error as axis-angle
        R_error = target_rot @ T_current[:3, :3].T
        angle = np.arccos(np.clip((np.trace(R_error) - 1) / 2, -1, 1))
        if angle < 1e-10:
            rot_error = np.zeros(3)
        else:
            axis = np.array([
                R_error[2,1] - R_error[1,2],
                R_error[0,2] - R_error[2,0],
                R_error[1,0] - R_error[0,1]
            ]) / (2 * np.sin(angle))
            rot_error = axis * angle

        error = np.concatenate([pos_error, rot_error])

        if np.linalg.norm(pos_error) < pos_tol and np.linalg.norm(rot_error) < rot_tol:
            return q, True

        # Compute numerical Jacobian
        J = numerical_jacobian(q, dh_params)

        # Damped least squares step
        JtJ = J.T @ J + damping**2 * np.eye(n_joints)
        dq = np.linalg.solve(JtJ, J.T @ error)
        q += dq

    return q, False

def numerical_jacobian(q, dh_params, delta=1e-6):
    """Compute 6xN Jacobian numerically."""
    n = len(q)
    J = np.zeros((6, n))
    T_base = forward_kinematics(q, dh_params)
    pos_base = T_base[:3, 3]

    for i in range(n):
        q_perturbed = q.copy()
        q_perturbed[i] += delta
        T_pert = forward_kinematics(q_perturbed, dh_params)

        # Position Jacobian
        J[:3, i] = (T_pert[:3, 3] - pos_base) / delta

        # Orientation Jacobian (simplified)
        R_diff = T_pert[:3, :3] @ T_base[:3, :3].T
        J[3:, i] = np.array([R_diff[2,1], R_diff[0,2], R_diff[1,0]]) / delta

    return J

RRT-Connect in Configuration Space

RRT-Connect grows two trees (from start and goal) and tries to connect them:

class RRTConnect:
    def __init__(self, q_start, q_goal, collision_fn, joint_limits, step_size=0.1):
        self.q_start = np.array(q_start)
        self.q_goal = np.array(q_goal)
        self.collision_fn = collision_fn  # returns True if config is in collision
        self.limits_low = np.array([l[0] for l in joint_limits])
        self.limits_high = np.array([l[1] for l in joint_limits])
        self.step_size = step_size
        self.n_dof = len(q_start)

    def plan(self, max_iter=5000):
        tree_a = [{'q': self.q_start, 'parent': -1}]
        tree_b = [{'q': self.q_goal, 'parent': -1}]

        for i in range(max_iter):
            q_rand = self._random_config()

            # Extend tree_a toward random config
            status_a, q_new_a = self._extend(tree_a, q_rand)
            if status_a == 'trapped':
                tree_a, tree_b = tree_b, tree_a  # swap trees
                continue

            # Try to connect tree_b to tree_a's new node
            status_b, q_new_b = self._connect(tree_b, q_new_a)

            if status_b == 'reached':
                # Trees connected — extract path
                path_a = self._extract_path(tree_a)
                path_b = self._extract_path(tree_b)
                path_b.reverse()

                # Determine which tree started from start
                if np.allclose(tree_a[0]['q'], self.q_start):
                    return path_a + path_b
                else:
                    return path_b + path_a

            tree_a, tree_b = tree_b, tree_a  # swap trees

        return None  # planning failed

    def _random_config(self):
        return np.random.uniform(self.limits_low, self.limits_high)

    def _extend(self, tree, q_target):
        nearest_idx = self._nearest(tree, q_target)
        q_nearest = tree[nearest_idx]['q']

        direction = q_target - q_nearest
        dist = np.linalg.norm(direction)
        if dist < 1e-6:
            return 'reached', q_nearest

        q_new = q_nearest + direction / dist * min(self.step_size, dist)

        if self.collision_fn(q_new):
            return 'trapped', None

        tree.append({'q': q_new, 'parent': nearest_idx})

        if np.linalg.norm(q_new - q_target) < self.step_size:
            return 'reached', q_new
        return 'advanced', q_new

    def _connect(self, tree, q_target):
        status = 'advanced'
        q_last = None
        while status == 'advanced':
            status, q_last = self._extend(tree, q_target)
        return status, q_last

    def _nearest(self, tree, q):
        configs = np.array([node['q'] for node in tree])
        dists = np.linalg.norm(configs - q, axis=1)
        return np.argmin(dists)

    def _extract_path(self, tree):
        path = []
        idx = len(tree) - 1
        while idx >= 0:
            path.append(tree[idx]['q'])
            idx = tree[idx]['parent']
        path.reverse()
        return path

Trajectory Smoothing and Time Parameterization

Raw RRT paths are jagged. Smoothing and adding time profiles is essential:

def shortcut_smoothing(path, collision_fn, max_attempts=200):
    """Randomly try to shortcut between non-adjacent waypoints."""
    path = [np.array(q) for q in path]

    for _ in range(max_attempts):
        if len(path) < 3:
            break

        i = np.random.randint(0, len(path) - 2)
        j = np.random.randint(i + 2, len(path))

        # Check if direct connection is collision-free
        segment = interpolate_configs(path[i], path[j], n_checks=20)
        if all(not collision_fn(q) for q in segment):
            path = path[:i+1] + path[j:]

    return path

def interpolate_configs(q1, q2, n_checks=20):
    """Linear interpolation between configurations."""
    return [q1 + t * (q2 - q1) for t in np.linspace(0, 1, n_checks)]

Time-Optimal Trajectory with Velocity Limits

def time_parameterize(path, max_velocities, max_accelerations, dt=0.01):
    """Simple trapezoidal velocity profile for each joint."""
    waypoints = np.array(path)
    n_waypoints = len(waypoints)
    n_joints = waypoints.shape[1]

    # Compute segment distances
    segments = np.diff(waypoints, axis=0)
    segment_lengths = np.linalg.norm(segments, axis=1)

    trajectory = [waypoints[0]]
    timestamps = [0.0]
    t = 0.0

    for i in range(n_waypoints - 1):
        dq = segments[i]

        # Time required for each joint (trapezoidal profile)
        times_per_joint = []
        for j in range(n_joints):
            if abs(dq[j]) < 1e-8:
                times_per_joint.append(0)
                continue
            # Time with trapezoidal velocity
            v_max = max_velocities[j]
            a_max = max_accelerations[j]
            t_accel = v_max / a_max
            d_accel = 0.5 * a_max * t_accel**2
            if 2 * d_accel > abs(dq[j]):
                t_seg = 2 * np.sqrt(abs(dq[j]) / a_max)
            else:
                t_cruise = (abs(dq[j]) - 2 * d_accel) / v_max
                t_seg = 2 * t_accel + t_cruise
            times_per_joint.append(t_seg)

        segment_time = max(times_per_joint)
        n_steps = max(int(segment_time / dt), 1)

        for step in range(1, n_steps + 1):
            frac = step / n_steps
            q = waypoints[i] + frac * dq
            t += dt
            trajectory.append(q)
            timestamps.append(t)

    return np.array(trajectory), np.array(timestamps)

PyBullet Integration

PyBullet provides physics simulation with collision detection:

import pybullet as p
import pybullet_data

def setup_pybullet_scene():
    physics_client = p.connect(p.DIRECT)  # headless
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setGravity(0, 0, -9.81)

    plane = p.loadURDF("plane.urdf")
    robot = p.loadURDF("franka_panda/panda.urdf", useFixedBase=True)

    # Add an obstacle
    obstacle = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.1, 0.2])
    p.createMultiBody(baseMass=0, baseCollisionShapeIndex=obstacle,
                      basePosition=[0.5, 0, 0.3])

    return robot

def collision_check(robot_id, joint_angles):
    """Check if configuration causes collision."""
    n_joints = p.getNumJoints(robot_id)
    for i, angle in enumerate(joint_angles):
        p.resetJointState(robot_id, i, angle)

    p.performCollisionDetection()
    contacts = p.getContactPoints(bodyA=robot_id)
    # Filter self-collisions from adjacent links
    real_collisions = [c for c in contacts if abs(c[3] - c[4]) > 1]
    return len(real_collisions) > 0

def pybullet_ik(robot_id, target_pos, target_orn=None):
    """Use PyBullet's built-in IK solver."""
    end_effector_idx = 11  # Panda end-effector link
    if target_orn is not None:
        joint_angles = p.calculateInverseKinematics(
            robot_id, end_effector_idx, target_pos, target_orn,
            maxNumIterations=100, residualThreshold=1e-5
        )
    else:
        joint_angles = p.calculateInverseKinematics(
            robot_id, end_effector_idx, target_pos
        )
    return joint_angles[:7]  # Panda has 7 DOF

OMPL Python Bindings

OMPL provides state-of-the-art planners:

from ompl import base as ob
from ompl import geometric as og

def plan_with_ompl(start_q, goal_q, collision_fn, joint_limits, planning_time=5.0):
    n_dof = len(start_q)
    space = ob.RealVectorStateSpace(n_dof)

    bounds = ob.RealVectorBounds(n_dof)
    for i, (lo, hi) in enumerate(joint_limits):
        bounds.setLow(i, lo)
        bounds.setHigh(i, hi)
    space.setBounds(bounds)

    si = ob.SpaceInformation(space)

    def is_valid(state):
        q = [state[i] for i in range(n_dof)]
        return not collision_fn(q)

    si.setStateValidityChecker(ob.StateValidityCheckerFn(is_valid))
    si.setup()

    start = ob.State(space)
    goal = ob.State(space)
    for i in range(n_dof):
        start[i] = start_q[i]
        goal[i] = goal_q[i]

    pdef = ob.ProblemDefinition(si)
    pdef.setStartAndGoalStates(start, goal)

    planner = og.RRTConnect(si)
    planner.setProblemDefinition(pdef)
    planner.setup()

    solved = planner.solve(planning_time)

    if solved:
        path = pdef.getSolutionPath()
        path.interpolate(50)  # densify path
        return [[path.getState(i)[j] for j in range(n_dof)]
                for i in range(path.getStateCount())]

    return None

One thing to remember: A complete motion planning pipeline chains IK (to find goal joint angles), RRT-Connect (to find a collision-free path in C-space), shortcut smoothing (to remove unnecessary detours), and time parameterization (to respect velocity and acceleration limits) — all testable in Python with PyBullet before deployment on real hardware.

pythonmotion-planningroboticsmanipulationkinematics

See Also

  • Python Behavior Trees Robotics How robots make decisions using a tree-shaped rulebook that keeps them organized, like a flowchart that tells a robot what to do in every situation.
  • Python Bluetooth Ble How Python connects to fitness trackers, smart locks, and wireless sensors using the invisible radio signals all around you.
  • Python Circuitpython Hardware Why CircuitPython makes wiring up LEDs, sensors, and motors as easy as plugging in a USB drive.
  • Python Computer Vision Autonomous How self-driving cars use cameras and Python to see the road, spot pedestrians, read signs, and understand traffic — like giving a car human eyes and a brain.
  • Python Home Assistant Automation How Python turns your home into a smart home that reacts to you automatically, like a helpful invisible butler.

Related Topics

ab-testing How tech companies run thousands of experiments at once to improve their products — the scientific method applied to everything from button colors to recommendation algorithms. 3 levels → activation-functions Why neural networks need these tiny mathematical functions — and how ReLU's simplicity accidentally made deep learning possible. 3 levels → ai-agents-architecture How AI systems go from answering questions to actually doing things — the design patterns that turn language models into autonomous agents that browse, code, and plan. 3 levels →