REF: grasp sprint part 1 reference - #3362
Draft
TomCC7 wants to merge 75 commits into
Draft
Conversation
A PD-held arm rests 1e-4..1e-3 rad off target, so microradian equality gates rejected every preview-then-execute pick and every plan requested while the arm merely held position (INVALID_START / 'no longer matches the stored plan start'). 0.01 rad absorbs hold jitter and still catches a genuinely moved arm.
… manip/grasp-sprint-july-26-sim # Conflicts: # dimos/manipulation/planning/world/roboplan_world.py # dimos/robot/manipulators/xarm/blueprints/simulation.py
Compose GraspGenXModule into xarm-grasp-sim so pick's provider path runs learned proposals (autoconnect fills the GraspGenSpec ref); CC's xArm gripper config from grasp_config. xarm-perception-sim is unchanged. Adds static_box_obstacles to ManipulationModuleConfig next to floor_z: startup box obstacles for scene furniture the planner must always respect. The grasp-sim table matches data/xarm_grasp_sim/scene.xml.
Convert MuJoCo's internal closing-joint feedback to aperture values and keep direct gripper RPCs in the coordinator's hold-last cache. This prevents arm trajectory commands from closing a gripper that pick just opened. Increase the xArm simulation pre-grasp clearance from 5 cm to 10 cm and cover both regressions with focused tests.
Co-authored-by: danvi <bogdan@dimensionalos.com>
pointcloud_to_convex_hull_obj centers the hull on the mean of the points, but the obstacle was placed at obj.pose, which carries the bounding-box centre. The two differ by ~1.6cm on tall asymmetric objects, so mesh obstacles sat offset from the geometry they represent.
Publish the object cloud, the ranked grasp candidates, and the candidate currently under feasibility check as Viser display layers, so a failing pick can be inspected instead of inferred from logs. The gripper geometry comes from config rather than the generator: grasp_gen_x bakes grasp_frame_to_tcp into candidate poses and the wireframe un-applies it, so the xarm blueprint sources both from one config object. Publishing is opt-in, and every call is suppressed so visualization cannot fail a pick.
Plumb the existing WorldObstacleMonitor use_mesh_obstacles option through start_obstacle_monitor and enable it for the grasp sim. Bounding boxes envelop an object at every height, so a side approach collides with empty space; the hull follows the observed geometry. Other blueprints keep boxes.
pointcloud_to_convex_hull_obj named its output hull_{id(points)}.obj. id() is
a memory address, and CPython reuses a freed address for the next same-sized
array, so sequential callers overwrote each other: five of six scene objects
collapsed onto one hull file and every obstacle took the shape of whichever
was written last. Intermittent, because it depends on allocator reuse.
The default name is now a uuid. The obstacle monitor passes an explicit path
per object instead, so rescanning overwrites in place rather than growing the
cache without bound.
The stored-plan freshness check rejected a grasp two milliseconds after IK solved it: the preview animation runs for ~1.6s between planning and executing, and a PD-held shoulder sags past the fixed 0.01 rad threshold in that window. That is not the stale-plan case the check guards against. The tolerance is now configurable (execution_start_tolerance, default 0.05 rad, 0 disables) and the rejection message reports the measured drift so the threshold can be set from data rather than guessed.
Pick suppressed every object obstacle for the whole transaction, so the approach was planned against an empty scene and swept through whatever stood between the arm and the target. Suppression is now staged. Candidate selection and the approach plan against the full scene; only the legs that must reach into and carry the target - grasp, close, retreat, and their feasibility check - hide the target, and nothing else is ever hidden. A failed restore is recorded on the transaction so it still surfaces, and pick_suppress_all_object_obstacles remains as a diagnostic escape hatch.
…tion Brings the xarm-grasp-sim work (convex-hull obstacles, grasp visualization layers, staged target suppression, PD-sag tolerance) onto the team's perception branch (EdgeTAM segmenter, PickNPlaceModule, execution manager extraction, roboplan Cartesian planning). Conflict resolutions: - dimos/control/hardware_interface.py: sim side — set_gripper_position now checks write_gripper_position() before recording _last_commanded, so a failed write is not remembered as commanded. - dimos/manipulation/manipulation_module.py: upstream structure. Upstream extracted execution gating into PlanExecutionManager and deleted _stored_plan_freshness_error/_restore_execution_gate; the sim-side versions are dropped here. execution_start_tolerance stays in the config and is re-wired to the execution manager in a follow-up commit. - dimos/robot/manipulators/xarm/grasp_config.py: comment-only conflict. Both sides landed the identical XARM_GRASP_FRAME_TO_TCP matrix ((0,-1,0,0),(1,0,0,0),(0,0,1,0.172),(0,0,0,1)); upstream's wording kept plus upstream's XARM_TCP_TO_GRASP_FRAME inverse. - dimos/manipulation/_test_manipulation_helpers.py: deleted, per upstream's replacement with dimos/manipulation/conftest.py.
Upstream's roboplan Cartesian commit (1465011) reintroduced a 1e-6 rad start-state equality gate inside the new _validated_selection_start helper. Two regressions came back with it: - a PD-held arm rests 1e-4..1e-3 rad off target, so every plan requested while the arm merely holds position returned INVALID_START. Widened to a module-level _START_STATE_ATOL = 0.01, as in 48ca20f. - connected pose sequences plan each leg from the previous leg's endpoint, which is deliberately not the current scene state. cc064cd already removed that gate from the joint-path planner for exactly this reason (6359f79); plan_selected_joint_path now opts out again via match_current=False. Cartesian planning keeps the check. ManipulationModuleConfig.execution_start_tolerance is dropped: upstream deleted _stored_plan_freshness_error along with the rest of the execution gating, and the equivalent knob now lives on the coordinator side as JointTrajectoryTaskConfig.start_position_tolerance (also 0.05 rad by default, also reports the measured drift), reachable from a blueprint via trajectory_task(hw, start_position_tolerance=...).
0a2f8cd deleted _test_manipulation_helpers and its import but left two _make_module() call sites, so TestSafetyLift and every connected-pose test failed with NameError on the team branch. Re-adds the harness locally.
ObjectSceneRegistrationSpec gained set_prompts() and scan_scene(), which PickNPlaceModule.scan_scene() calls. SimObjectScene now answers both: scan_scene publishes and returns a Detection3DArray built from current ground truth, and set_prompts filters published detections by name substring. Scene and per-object clouds stay complete, matching a real detector whose prompts never touch the raw depth cloud.
Three new entities, none of them mutating xarm_grasp_sim's wiring: - xarm-grasp-sim-perception / -agent: the grasp scene with ObjectSceneRegistrationModule in place of SimObjectScene, fed by MujocoSimModule's wrist camera and tuned like the hardware blueprint (perception.py). Everything else — table obstacle, mesh obstacles, GraspGenX, candidate budget, grasp visualization — is unchanged. - picknplace-graspgenx-edgetam-sim: the team's prompted-EdgeTAM workflow against the same MuJoCo scene, swapping RealSenseCamera for MujocoSimModule and the xArm6 hardware for the xArm7 sim. xarm_perception_sim and xarm_grasp_sim are now plain autoconnect() assignments so the all_blueprints generator discovers them instead of relying on hand-added entries; all_blueprints.py is regenerated.
A failed plan leaves the robot stationary, so it is recoverable without a reset. Faulting it also made the scan skill's own fallback unreachable: the safe-waypoint plan set FAULT, so the "go directly to init" retry on the next line always hit "Cannot plan: state is FAULT" and the agent could only loop reset/scan. Execution failures still fault, as reset's docstring describes.
The scene only refreshes clouds inside scan_scene(), so an agent that deliberates between scanning and picking outlives max_object_pointcloud_age and every pick fails GRASP_INPUT_INVALID. Refresh once and retry before giving up; refresh_obstacles() drives the same scan for look/scan_objects.
update_object() concatenated every view, so a tracked object grew without limit: five minutes in view reached 52k points and GraspGenX's outlier removal died in torch.cdist (an N*N matrix past the CUDA int32 index limit). Re-voxelize on merge so the cloud is bounded by surface area, not view count.
…reshold Promotion was only checked when a later detection matched an existing entry, so the count that crossed the threshold never promoted on its own. With min_detections_for_permanent=1 the first detection stayed pending forever and single-view request-driven scans returned nothing.
Its constructor dlopens the native library: 937 ms, against 2 ms to encode a frame. Every camera frame logged to Rerun paid it, capping the bridge near 1 Hz and stalling it (the callback is synchronous), so panes lagged minutes behind the robot. encode/decode allocate their own per-call handles.
The wrist camera sees the arm's own links mid-trajectory, and YOLO-E registered them as objects in mid-air, dragging real detections onto the phantoms. Detect only on request, from still frames, as the team's hardware picknplace blueprint already does; each request is one view, so promotion cannot require several. vis_module carries picknplace_rerun_config so the sim gets the same segmentation/RGB/grasp panes as hardware.
TomCC7
changed the title
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
9 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
grasp sprint part 1 reference