Planning for manipulators and dynamic environments

Two settings stretch the methods of earlier lessons in different directions. Manipulators live in high-dimensional joint spaces, where sampling and optimization dominate, but their tasks are specified in the Cartesian space of the gripper: reach this grasp, keep the tool level, move along this line. Mobile robots among people and other robots face the opposite difficulty: the configuration space may be small, but obstacles move, so a plan that was valid a second ago may not be now. This lesson covers both. It explains joint-space versus task-space planning, how goals are given through inverse kinematics, how a modern arm planning pipeline such as MoveIt 2 with OMPL is organized, and how grasping shapes the plan. It then turns to dynamic environments: velocity obstacles, the dynamic window approach, model predictive local planners and, briefly, planning under uncertainty in belief space.

Joint space and task space

An arm can be planned in joint space, where the configuration is the vector of joint angles, or in task space, where it is the pose of the end effector. Joint space is where collision checking and joint limits are natural, and where sampling planners work, since every sample is a valid arm configuration; but a straight line in joint space produces a curved, sometimes surprising motion of the gripper. Task space is where the job is defined, and straight-line Cartesian motions are what users expect for approaching a grasp or inserting a part; but a Cartesian path must be converted to joint motions by inverse kinematics at every point, which can fail near singularities or joint limits, or flip between solutions. Most systems combine them: free-space transfers between distant poses are planned in joint space, and short approach, retreat and insertion moves are planned as Cartesian paths.

Goals from inverse kinematics

Users specify goals for the gripper, not the joints, so the planner must turn a target pose into joint configurations. A 6-joint arm typically has up to 8 or 16 inverse kinematics solutions for a reachable pose (elbow up or down, wrist flipped, shoulder left or right), and a 7-joint arm has infinitely many, a one-dimensional family parametrized by the elbow angle. Choosing the wrong one can make the plan long or impossible. Planners therefore sample several IK solutions, discard those in collision or beyond joint limits, and either plan to the one closest to the current configuration or plan to several at once and keep the best. Goals are often regions rather than points: any grasp within 10° of vertical around a cylinder, or any placement on a shelf. Goal-region sampling feeds IK solutions for random poses in the region to the planner, which is one reason sampling planners suit arms so well.

The MoveIt 2 and OMPL pipeline

MoveIt 2, the main open-source manipulation framework for ROS 2, organizes arm planning as a pipeline. A motion plan request gives the planning group (which joints), the goal as joint values, a pose or constraints, any path constraints such as keeping an orientation, and limits on planning time and attempts. The planning scene supplies the robot model, its current state, the collision environment and attached objects. Request adapters run before planning, for example fixing a start state that is marginally in collision or adding default workspace bounds. The planner plugin, most often OMPL with RRT-Connect, finds a joint-space path. Response adapters then post-process it: smoothing, adding time parametrization with velocity and acceleration limits (time-optimal trajectory generation), and validating the result. Alternatives such as the Pilz industrial motion planner (for deterministic point-to-point, linear and circular moves), CHOMP and STOMP, and the Cartesian path interface plug into the same pipeline. MoveIt Task Constructor chains stages, such as approach, grasp, lift and place, each with its own planner, into a single task.

# MoveIt 2 (Python API, moveit_py) sketch of a pick approach: joint-space transfer, then a Cartesian approach.
# Listing only; requires a robot description, a running MoveIt 2 setup and the moveit_py package.
from moveit.planning import MoveItPy
from geometry_msgs.msg import PoseStamped

robot = MoveItPy(node_name="pick_demo")
arm = robot.get_planning_component("manipulator")

pre_grasp = PoseStamped()
pre_grasp.header.frame_id = "base_link"
pre_grasp.pose.position.x, pre_grasp.pose.position.y, pre_grasp.pose.position.z = 0.55, 0.10, 0.30
pre_grasp.pose.orientation.x, pre_grasp.pose.orientation.y = 1.0, 0.0     # tool pointing down
pre_grasp.pose.orientation.z, pre_grasp.pose.orientation.w = 0.0, 0.0

arm.set_start_state_to_current_state()
arm.set_goal_state(pose_stamped_msg=pre_grasp, pose_link="tool0")
plan = arm.plan()                     # OMPL (e.g. RRTConnect) + smoothing + time parametrization
if plan:
    robot.execute(plan.trajectory, controllers=[])
# The final 10 cm approach is then planned as a straight Cartesian path to the grasp pose.

Grasping and approach

Grasping turns a single goal into a small sequence. The grasp pose itself is usually chosen by a separate grasp planner or learned model (ROB-400 covers grasping in depth). The motion planner must then reach a pre-grasp pose a few centimetres back along the approach direction, move straight in, close the gripper, attach the object to the robot model so it is collision-checked during the next motion, lift straight out to a retreat pose, and plan the transfer. During the final approach the gripper must be allowed to come close to, and eventually touch, the object, so the allowed collision matrix is adjusted for that phase. Many failed picks in practice come not from the main planner but from these transitions: an approach direction blocked by a bin wall, an IK solution that cannot move straight in, or forgetting to attach the object so the arm swings it into a shelf.

Moving obstacles and velocity obstacles

When obstacles move, a path that is collision-free against a snapshot of the world can lead into a collision later. The simplest response is to replan frequently, but planning must then account for where obstacles will be. The velocity obstacle (Fiorini and Shiller, 1998) captures this for local avoidance: for a robot and a moving obstacle, both modelled as discs, the velocity obstacle is the set of robot velocities that would lead to a collision within a time horizon, assuming the obstacle keeps its current velocity. Geometrically it is a cone in velocity space, apex at the obstacle's velocity, containing all relative velocities that point at the combined disc. The robot chooses a velocity outside every velocity obstacle, preferably close to its desired velocity. Reciprocal velocity obstacles and ORCA (optimal reciprocal collision avoidance, van den Berg and colleagues, 2011) assume that other agents share the avoidance effort, which prevents oscillation between robots using the same method and is widely used for robot fleets and crowd simulation.

Dynamic window and MPC local planners

Mobile robots usually split planning into a global planner on the cost map, replanned at about 1 Hz, and a local planner or controller running at 10 to 50 Hz that follows the global path while avoiding obstacles the global plan did not foresee. The dynamic window approach (DWA; Fox, Burgard and Thrun, 1997) samples linear and angular velocities that the robot can reach within one control period given its acceleration limits (the dynamic window), simulates each for a short horizon, discards those that collide, and scores the rest by progress toward the goal, clearance and speed. Model predictive control generalizes this: at each step, optimize a sequence of controls over a horizon of one to a few seconds, subject to the dynamics, velocity and acceleration limits and collision constraints with predicted obstacle positions, apply the first control and repeat. Nav2's Model Predictive Path Integral (MPPI) controller samples thousands of control sequences on the processor and weights them by cost, in the spirit of STOMP (lesson 9). Timed Elastic Bands optimize a short trajectory as a band of timed poses. All of these are local: they can get stuck behind a large obstacle, which is why they follow a global plan.

Planning under uncertainty

Everything so far assumed the robot knows its own state and the obstacles' positions exactly, or at least well enough that a safety margin covers the error. When uncertainty is large, as for a robot localized with a particle filter in a featureless corridor (ROB-310 and ROB-340) or a person whose next move is unpredictable, planners can reason about it directly. Chance-constrained planning requires the probability of collision to stay below a threshold, for example by inflating obstacles by a multiple of the position standard deviation. Belief-space planning treats the robot's probability distribution over its state as the thing being planned: a path that passes near distinctive landmarks may be longer but keep the robot well localized, and therefore safer, than a shorter path through a featureless area. The full problem is a partially observable Markov decision process (POMDP) and is intractable in general, but approximate methods, such as planning with Gaussian beliefs propagated by a Kalman filter, are used in practice for navigation and grasping under uncertainty.