Motion Planning
Batched motion planning with goal IK, trajectory optimization stages, and retry.
Lifecycle
__init__copies configuration and constructs goal IK, the optimizer stages, and their retiming.warmup(batch_size)fixes the batch size and allocates all batch-shaped state.solve_offline(...), whichsolve_offline_numpyandsolve_offline_torchwrap, places the waypoints, builds candidates, solves and retries trajectories, then retimes them with the last stage'sretimeand computesmotion_time.solve_online(...)runs a fresh gradient solve with executed-history smoothing, or updates the retained MPPI action distribution.
Planner configuration.
Robot model.
One target link or a sequence, using names or indices.
Optional collision scene.
Warp device.
Fix the planner batch size and allocate its batch-shaped buffers.
Number of queries solved together.
Set which actuated joints IK and trajectory optimization may change; before the first solve it also limits the trajectory collision Jacobians to the active joints, which later masks must stay within.
One value per actuated joint; zero pins the joint.
Plan batched Warp trajectories through waypoints, start first and goal last; the NumPy and Torch APIs wrap it.
Lifecycle
- Parse segments: each segment's world collision flag, fixed duration and lock.
- Solve waypoint IK: each pose but a final goal by goal IK (scene collision, no self collision), resting at its neighbors' blend.
- Create split: each segment's frames by its duration, else evenly, then the locked frames and collision.
- Solve:
_solve_offline_attemptwith retries, one CUDA graph undercuda_graph="solve"but for durations; it builds each guess's goal by goal IK, its later poses' answers nearest its neighbors' blend, and straight lines through its waypoints, on frames resplit by the time each line needs; after the first stage, several segments resplit their frames again. - Retime: every plan under the last stage's retime, its fixed segments held still, and its motion time.
Ordered list of at least two: joint arrays (batch, dofs) in robot.spec.actuated_joint_names order, wp_vec7 arrays (batch,) or (batch, target links) posing the target links, or dicts of target link names to wp_vec7 (batch,) posing some of them (other keys are ignored). A link a dict leaves out is free at its frame, so each link reaches its own waypoints in turn: the first guess keeps it at its pose of the waypoint before (blended toward any later joint waypoint; a start pose's at seed_q or the next joint waypoint, one needed), and the stages move it from there only as their costs pull. Every link passes each waypoint at the same instant, and the goal names all. An inner waypoint holds the links it names at its joints or IK answer, contact included.
A floating base's start poses, wp_vec7 (batch,); such plans take a start and a goal.
Optional IK seed: the goal IK's initialization, or the rest of a pose waypoint before any joint one.
Optional scene index per batch row; defaults to zero.
One flag per segment (waypoint i to i + 1); False drops world collision there (self collision stays), and from the checks of anchors only such segments touch, e.g. a contact. Defaults to all True. Every segment_* and waypoint_* list applies to all batch rows alike.
Seconds per segment, or None to let the planner time it. A duration gives the segment round(duration / dt) frames (at least two) in the solve, keeps them through the resplit and is its time in interpolate; None segments share the rest of num_timesteps, which the durations must fit.
One flag per segment; True keeps its frames on the straight joint-space line between its waypoints (the tool's path between them is not straight), whatever it passes through, and holds the links both its waypoints name.
Objects the robot carries, on the spheres max_attached_spheres reserves on the target link each one's link is rigid with: checked by world and self collision over their segments (an inner waypoint: those both its segments carry), in the stages, their scores and waypoint and goal IK.
Link pairs whose relative pose the gradient stages hold over their segments, at most max_fixed_relative_poses.
last and only their rows return; more raise ValueError("Batch size mismatch").
NumPy wrapper for solve_offline: a pose waypoint is a dict of target links to 4x4 or [x, y, z, qw, qx, qy, qz] poses, unbatched values broadcast across queries, and the result holds owned copies.
Torch wrapper for solve_offline, as solve_offline_numpy with 4x4 pose matrices; plans without waypoint IK stay on device.
Solve one online planning update.
Current configurations with q shaped (batch, dofs), and base poses for a floating base.
Optional wp_vec7 pose goals, (batch,) or (batch, target links).
Optional solved IK goals or joint-space goals with shape (batch, dofs).
Optional state returned by the previous update.
Optional scene indices with shape (batch,).
The optimized trajectories, a borrowed buffer, and the caller-owned online state.
NumPy wrapper for solve_online.
Endpoint accuracy of state against exactly one pose or joint goal, independent of term weights.
Trajectories with shape (batch, frames, dofs); any planner's output works.
Optional batched pose goals.
Optional batched joint-space goals.
Success threshold on translation error.
Success threshold on rotation error.
Success threshold on joint error, for a joint goal.
Success threshold on scene penetration.
Buffers to write in place, from an earlier call; allocated when omitted.
Endpoint errors and success per batch element.
compute_metrics for numpy inputs, returning numpy arrays.
Configuration for offline or online motion planning.
Steps per plan; a plan has one more frame.
Planning step (s).
Collision spheres reserved on each target link for attached_objects, which every solve places there.
Link pairs reserved for fixed_relative_poses, which every solve sets.
Position and rotation weights of fixed_relative_poses, per m and per rad of drift.
Joint acceleration limit (rad/s²): the stages' bounds and ranking, MPPI rollouts, retimes and resplits.
Joint jerk limit (rad/s³): the stages' bounds and ranking, retimes and resplits.
0: the last stage also optimizes each free segment's duration, from the query's step, at this cost per second
of plan time; its derivative bounds use the optimized durations, which the returned retime only lengthens.
Goal IK, which turns pose goals into joint goals; fewer goals than trajectory candidates repeat across them.
Spread of the sampled seed candidates, as a fraction of each joint range; MPPI stages sample their own.
Random-midpoint detours per detour candidate (first round, retries); the shortest collision-free one seeds it.
Optimizer stages, run in order on the candidate population every planning round.
0: each later stage optimizes at this multiple of the previous stage's fastest feasible retimed step.
Retime between stages (stage_dt_scale); its minimum_dt floors the later stages' steps.
Reruns of a failed last stage whose predecessor passed, at a growing step; each query keeps its better plan.
Iterations of each stage retry.
Factor each stage retry grows the step by.
Rounds retrying failed plans with new goals and seeds; needs a gradient first stage with several candidates.
Retime of the returned plan; None keeps the planning step.
Resplit a waypoint plan's frames by the time each part needs, so a short segment takes fewer: its straight lines' before the solve, and its paths' after the first stage, which the later gradient stages smooth; False keeps the even split.
CUDA graphs: none, each stage's and goal IK's solve, or the whole NumPy solve with its device-side retries; a capture makes a planner's first solve take seconds longer.
Planner output with offline timing or online state.
Offline: q_traj (batch, frames, dofs) with frames = num_timesteps + 1, motion_time (batch,) (s) and waypoint_indices each waypoint's frame in q_traj (one per waypoint, 0 first, the same for every row).
Joint trajectories sampled every dt (s) or slightly less across motion_time.
Each segment between waypoints spreads ceil(its time / dt) steps evenly over its frames, so every waypoint is a sample; a plan then holds its last frame, so a batch shares one length. A segment with a fixed segment_fixed_duration takes that time; the others run at the plan's retimed step or segment_duration.
Example
>>> import numpy as np >>> q_traj = np.array([[[0.0], [1.0], [2.0]]], np.float32) >>> MotionPlanResult(q_traj, motion_time=np.array([1.0], np.float32)).interpolate(0.25)[0, :, 0] array([0. , 0.5, 1. , 1.5, 2. ], dtype=float32)
from robokit.helpers.motion_plan.motion_planner import MotionPlanResult import numpy as np q_traj = np.array([[[0.0], [1.0], [2.0]]], np.float32) MotionPlanResult(q_traj, motion_time=np.array([1.0], np.float32)).interpolate(0.25)[0, :, 0]
Control step (s).
Fraction of the plan's speed; 0.5 plays its retimed segments over twice their time.
Joint positions with shape (batch, samples, dofs), NumPy or Torch as q_traj.
Objective configuration
GradientTrajectoryOptimizerConfig and MppiTrajectoryOptimizerConfig accept terms, a list of ResidualTask templates. Objective weights and cost options are task constructor parameters. Use config.task(name) to retrieve a named task; for TrajectoryTask, configure its dense_task. deepcopy(config) creates an independent preset whose task attributes can be assigned directly. Config-level cost-weight arguments have been removed. IK objectives are supplied through MotionPlannerConfig.goal_ik.