motion-planning

Installation
SKILL.md

Collision-Aware Motion Planning (motion_planner.py)

Import import motion_planner as mp (prepending <skill_dir>/scripts to sys.path if needed).

CRITICAL — Never cat, head, or tail motion_planner.py or raw robot XMLs! Check README.md/runtime specs for transport, action layout, control_dt_s, and camera intrinsics, write workspace .py scripts or python3 - << 'EOF' (avoid /tmp scripts or inline python3 -c quoting errors), and call mjcf_path = mp.resolve_mjcf_path(), print(mp.describe_robot(mjcf_path)["summary_text"]), and planner.gripper_frame_info(gripper_cmd) upfront for joints/limits, TCP axes (approach_axis_ee, closing_axis_ee, slim_axis_ee), jaw widths, and asymmetric_protrusions.

1. Calibration, Hollow Geometry & Immediate t = 0 Escape

  • MJCF Root Frame & Sparse Triangulation: Express all poses/primitives in the robot MJCF root frame (never assume z_support = 0.0 or mix external world extrinsics). Compute camera poses via mp.get_camera_poses_in_robot_frame(mjcf_path, joint_positions) ({"left": left_q, "right": right_q}, {joint_name: val}, or single-arm q) and triangulate 1–2 support points (z_support, max_ray_residual_m < 0.015) plus sparse landmarks across >= 2 views via mp.triangulate_pixels (standing height h = z_top - z_support, resting center z = z_support + 0.5 * h, or mp.intersect_ray_plane — never run dense stereo, RANSAC, or scipy.optimize silhouette fitters).
  • Safety-Margin Overlay Fit (~1–1.5 cm), Hollow Primitives & Door Padding: Persist scene_model.py (build_scene, build_object). Decompose hollow bowls, cups, racks, and cavities into thin wall, floor/sill, ceiling, and rim primitives (add_vertical_cylinder base + perimeter ring of vertical add_capsule walls for round vessels, or add_oriented_box panels for box cavities — never solid blocks sealing interior goals). Model support (group="table"), ungrasped target (group="target_init", removed via scene.without("target_init") once attached), and idle arms (planner.add_other_arm_to_scene). Pad fixed walls 3–5 mm and unlatched hinged doors ~18–25 mm (group="door") (>= 12–15 cm staging clearance). Calibrate to the safety margin (~1–1.5 cm), not subpixel perfection: once wireframes in mp.render_scene_overlay enclose obstacles/rims within ~1–1.5 cm (2–3 passes max), stop refining geometry and move to execution (avoid custom Canny, vanishing-point, or numerical solvers; use close-range wrist views at pre_grasp/pre_place if needed).
  • Immediate All-Arm t = 0 Escape: Check every arm at t = 0 via planner.check_arm_config(scene, q_init). If any arm has min_clearance_m < 0.025, execute planner.escape_initial_collision(scene, q_init, target_clearance_m=0.04)["q_trajectory"] right away before planning pick/place, re-observe, update the arm's parked pose, and only re-fit scene_model.py if an object shifted.

2. Motion Planning, Lookahead Feasibility & Checkpoint Execution

  • Free-Space plan_arm vs. Linear plan_cartesian_stroke: Use planner.plan_arm (safety_margin_m=0.015) between pre-contact poses (5–8 cm clear), attaching held objects via planner.attach_object_at_world_pose(obj, q_grasp, obj_pose6d) and planner.set_gripper_opening(gripper_cmd). Always use planner.plan_cartesian_stroke(..., max_joint_vel_rad_s=0.5) (never 2-point joint interpolation) for pre_grasp -> grasp, vertical lift clear of rims, pre_place -> place -> retract, and linear cavity insertions/pushes (ignore_obstacles=("table",) or clearance_tol_m=0.003 at contact). Hold targets ~0.4–0.5 s when closing or fully opening (0.0) the gripper before lifting/retracting. Center grasps on ee_site, align closing_axis_ee across the wider clearance axis so slim_axis_ee faces narrow jambs/neighbors (or along thin rims/rods), and flip wrist roll 180° (ee_rot @ np.diag([-1, -1, 1])) so asymmetric_protrusions face open space.
  • Single-Arm Reach First & Decoupled Regrasp Staging: First check whether a single arm can both pick and reach a valid goal feature directly. When reorientation or cross-workspace transfer is required, prefer placing stably on an unobstructed surface (clear of open doors/obstacles), releasing, re-observing, and regrasping to decouple the arms at a zero-risk checkpoint — reserve mid-air bimanual handover strictly for slender/non-standing objects that cannot rest stably on a surface, and if coupled two-arm or multi-stage planning fails once or twice offline, switch immediately to surface staging or an alternate reachable goal feature rather than tuning coupled IK in a loop.
  • Lookahead Endpoint Feasibility, Then Checkpoint-by-Checkpoint Execution: Reserve >= 75% of episode budget for execution. Before grasping, run a fast endpoint feasibility check (solve_ik + check_arm_config / check_se3_pose at grasp and goal placement) to avoid kinematic dead ends — but only synthesize plan_arm / plan_cartesian_stroke trajectories up to the next safe, reversible physical checkpoint (collision escape, lifted hover, surface staging, or single-item placement), then execute right away and re-observe real camera + joint feedback rather than pre-solving multi-stage trajectories or all N items offline. After each placement, update scene with placed items at their observed resting poses and triangulate 3D position to verify it lies deep inside the goal volume (following shallow doorway-sill releases with a closed-gripper linear centerline push, or using a secure upper support feature on tall stands), parking arms >= 10 cm clear when done.
Installs
19
GitHub Stars
161
First Seen
3 days ago