使い方
- インストールコマンドをコピーし、自分の環境へ導入する前にソースを確認します。
- 必要なツール、認証情報、対応環境を原文で確認します。
- 入力と成果物を指定し、出力を資料と照合します。
npx skills add https://github.com/google-deepmind/robotics-antigravity-plugin --skill motion-planning開始前の確認
- 導入にはモデル契約、API枠、ファイルへのアクセス許可は含まれません。
- スター数や導入数は活動の指標であり、独立した品質・安全性試験ではありません。
見出し、例、コード、表、リンク、参照画像を含む原文を表示しています。
<!-- disableFinding(LINEOVER80) --> <!-- disableFinding(HEADING_FUNCTION) -->
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! CheckREADME.md/runtime specs for transport, action layout,control_dt_s, and camera intrinsics, write workspace.pyscripts orpython3 - << 'EOF'(avoid/tmpscripts or inlinepython3 -cquoting errors), and callmjcf_path = mp.resolve_mjcf_path(),print(mp.describe_robot(mjcf_path)["summary_text"]), andplanner.gripper_frame_info(gripper_cmd)upfront for joints/limits, TCP axes (approach_axis_ee,closing_axis_ee,slim_axis_ee), jaw widths, andasymmetric_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.0or mix external world extrinsics). Compute camera poses viamp.get_camera_poses_in_robot_frame(mjcf_path, joint_positions)({"left": left_q, "right": right_q},{joint_name: val}, or single-armq) and triangulate 1–2 support points (z_support,max_ray_residual_m < 0.015) plus sparse landmarks across>= 2views viamp.triangulate_pixels(standing heighth = z_top - z_support, resting centerz = z_support + 0.5 * h, ormp.intersect_ray_plane— never run dense stereo, RANSAC, orscipy.optimizesilhouette 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_cylinderbase + perimeter ring of verticaladd_capsulewalls for round vessels, oradd_oriented_boxpanels for box cavities — never solid blocks sealing interior goals). Model support (group="table"), ungrasped target (group="target_init", removed viascene.without("target_init")once attached), and idle arms (planner.add_other_arm_to_scene). Pad fixed walls3–5 mmand unlatched hinged doors `~18–25 mm` (`group="door"`) (>= 12–15 cmstaging clearance). Calibrate to the safety margin (`~1–1.5 cm`), not subpixel perfection: once wireframes inmp.render_scene_overlayenclose obstacles/rims within~1–1.5 cm(2–3passes max), stop refining geometry and move to execution (avoid custom Canny, vanishing-point, or numerical solvers; use close-range wrist views atpre_grasp/pre_placeif needed). - Immediate All-Arm `t = 0` Escape: Check every arm at
t = 0viaplanner.check_arm_config(scene, q_init). If any arm hasmin_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-fitscene_model.pyif 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 cmclear), attaching held objects viaplanner.attach_object_at_world_pose(obj, q_grasp, obj_pose6d)andplanner.set_gripper_opening(gripper_cmd). Always use `planner.plan_cartesian_stroke(..., max_joint_vel_rad_s=0.5)` (never 2-point joint interpolation) forpre_grasp -> grasp, vertical lift clear of rims,pre_place -> place -> retract, and linear cavity insertions/pushes (ignore_obstacles=("table",)orclearance_tol_m=0.003at contact). Hold targets `~0.4–0.5 s` when closing or fully opening (0.0) the gripper before lifting/retracting. Center grasps onee_site, alignclosing_axis_eeacross the wider clearance axis soslim_axis_eefaces narrow jambs/neighbors (or along thin rims/rods), and flip wrist roll180°(ee_rot @ np.diag([-1, -1, 1])) soasymmetric_protrusionsface 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_poseat 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 allNitems offline. After each placement, updatescenewith 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 cmclear when done.
3. motion_planner (mp) API (pose6d = [x, y, z, yaw, pitch, roll])
- Introspection & SE(3):
mp.resolve_mjcf_path() -> str,mp.describe_robot(mjcf_path=None) -> {"arms", "cameras", "summary_text"},mp.rpy_to_matrix(r, p, y),mp.matrix_to_rpy(rot),mp.quat_wxyz_to_matrix(q_wxyz),mp.pose6d_from_pos_rot(pos, rot=None),mp.pos_rot_from_pose6d(pose6d) -> (pos, rot),mp.make_transform(pos, rot=None),mp.invert_transform(tf). - `Scene3D` & `MultiSphereObject`:
scene = mp.Scene3D():.add_sphere(name, center, radius, group=""),.add_aligned_box(name, center, half_extents, group=""),.add_oriented_box(name, center, half_extents, rotation, group=""),.add_capsule(name, point_a, point_b, radius, group=""),.add_vertical_cylinder(name, center, radius, height, group=""),.add_object_spheres(obj, pose6d, name_prefix="", group=""),.without(*names_or_groups),.clone().obj = mp.MultiSphereObject(name="object"):.add_sphere(local_center, radius),.add_capsule_spheres(p0, p1, radius, num_spheres=5).mp.check_se3_pose(obj, scene, pose6d) -> {"valid", "collisions", "min_clearance_m"},mp.plan_object_se3(obj, scene, start_pose6d, goal_pose6d, safety_margin_m=0.010) -> {"solved", "waypoints", "interpolated"}. - `RobotArmPlanner`:
planner = mp.RobotArmPlanner(mjcf_path=None, arm="left", gripper_cmd=0.0)(.dof,.q_min,.q_max,.joint_names,.ee_site_name):.fk(q) -> (ee_pos, ee_rot),.gripper_frame_info(gripper_cmd=None) -> {"approach_axis_ee", "closing_axis_ee", "slim_axis_ee", "open_inner_gap_m", "closed_inner_gap_m", "open_outer_width_m", "slim_profile_width_m", "asymmetric_protrusions", "top_down_rot_hint"},.set_gripper_opening(gripper_cmd),.attach_object_at_world_pose(obj, q_grasp, obj_world_pose6d) -> T_ee_obj,.ee_pose_for_object_pose(obj_world_pose6d) -> (ee_pos, ee_rot),.attached_object_pose(q) -> pose6d,.detach_object(),.add_other_arm_to_scene(scene, other_arm_q, other_gripper_cmd=0.0),.check_arm_config(scene, q, ignore_obstacles=(), clearance_tol_m=0.0) -> {"valid", "self_collisions", "collisions", "min_clearance_m", "nearest_pair"},.solve_ik(target_pos, target_rot=None, q_seed=None, scene=None, num_restarts=16, ignore_obstacles=(), clearance_tol_m=0.0) -> {"success", "q", "pos_err_m", "rot_err_rad", "collision_check"},.escape_initial_collision(scene, q_start, target_clearance_m=0.015, control_dt_s=0.04) -> {"escaped", "q_escaped", "q_trajectory"},.plan_cartesian_stroke(q_start, target_pos, target_rot=None, num_waypoints=8, scene=None, ignore_obstacles=(), clearance_tol_m=0.0, max_joint_vel_rad_s=0.55, control_dt_s=0.04) -> {"solved", "waypoints", "q_trajectory", "min_clearance_m"},.plan_arm(scene, q_start, q_goal, max_joint_vel_rad_s=1.25, control_dt_s=0.04, safety_margin_m=0.015) -> {"solved", "waypoints", "q_trajectory"},mp.resample_waypoints_cosine(waypoints, max_joint_vel_rad_s=1.25, control_dt_s=0.04) -> np.ndarray. - Cameras & Overlay:
mp.get_camera_poses_in_robot_frame(mjcf_path, joint_positions=None) -> {cam_key: {"pos", "rot", "mjcf_name"}},mp.triangulate_pixels(uv_by_camera, cam_poses, cam_intrinsics) -> {"point_3d", "max_ray_residual_m"},mp.intersect_ray_plane(u_col, v_row, cam_pose, cam_intrinsics, plane_z=0.0),mp.render_scene_overlay(mjcf_path, joint_positions, camera_image_paths, scene, output_path, objects=(), camera_intrinsics=None) -> str.