feat(manip): let the voxel map define the planner's world - #3875
feat(manip): let the voxel map define the planner's world#3875mustafab0 wants to merge 1 commit into
Conversation
The planner only avoided registered objects, so anything the detector did not name was invisible to it and the one thing standing in for the table was floor_z: a 0.6 x 1.2 x 0.2 m box centred at (0.7, 0, -0.12). Not a floor, one bench, pinned to one spot, leaving everything nearer than x=0.4 unguarded. It is gone. The wrist camera now feeds PointCloudSelfFilter and RayTracingVoxelMap into ManipulationModule.voxel_map, and what the camera sees defines the obstacles. Four things were needed to make the chain carry data: both cameras default enable_pointcloud to False; the self filter needs a capture-time transform for every collision link and drops a whole cloud when one is missing; its stock 20 ms TF tolerance cannot bracket the 10 Hz period ManipulationModule publishes robot TF at, so most clouds found no transform at all; and the two edges whose stream names differ are remapped. Two consequences. The target object is itself mapped geometry once the map feeds the planner, so a collision-checked plan into it can only be rejected; the pregrasp-to-grasp leg and the retreat are now straight-line move_linear servos with checking off, which is what that primitive is for. And #3714 added the OCTREE obstacle type and its planner support but no rendering, so the viser scene's dispatch raised "unsupported obstacle type" and drew a grey proxy box per cell instead of the map. Occupied cells draw as one point cloud sized to the cell edge: a box per cell would be tens of thousands of scene nodes.
❌ 1 Tests Failed:
View the top 1 failed test(s) by shortest run time
To view more test analytics, go to the Test Analytics Dashboard |
Greptile SummaryThis change replaces the fixed xArm floor obstacle with wrist-camera voxel mapping, connects the voxel map to the manipulation planner, adds octree visualization, and uses linear Cartesian motion for final grasp, placement, and retreat legs. The final Cartesian legs disable collision checking for the entire planner scene. That bypass applies to unrelated mapped obstacles as well as the intended grasp or placement contact, so the arm, gripper, or carried object can be commanded through nearby geometry. The xArm configuration also removes the fixed floor without preventing planning until a non-empty voxel map is installed; because an empty map removes the octree, planning can proceed with neither an octree nor table obstacle. T-Rex validation blocked
Confidence Score: 3/5Not safe to merge until final contact motions retain protection from unrelated obstacles and planning is prevented when table-protection geometry is unavailable. The implementation directly passes disabled collision checking from the final motion helper through Cartesian planning and execution. The xArm setup also depends on an octree that can be absent or removed without preventing planning. The focused runtime harnesses could not execute because required Python runtime components were unavailable. Files Needing Attention:
 T-Rex LogsWhat T-Rex did
|
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| result = self._manipulation.move_linear( | ||
| end.position.x - start.position.x, | ||
| end.position.y - start.position.y, | ||
| end.position.z - start.position.z, | ||
| planning_group, | ||
| check_collision=False, | ||
| ) |
There was a problem hiding this comment.
_servo passes check_collision=False for every final grasp, placement, and retreat leg. move_linear propagates that setting to Cartesian planning and executes accepted plans without a later collision recheck, so this is not limited to the contacted target: a route through a table edge, adjacent object, or other mapped obstacle is accepted as well. Represent the intended target contact as a narrow allowed-contact exception while keeping collision checks enabled for the rest of the planning scene.
1 participant
The planner only avoided registered objects, so anything the detector did not name was invisible to it, and the one thing standing in for the table was
floor_z: a 0.6 x 1.2 x 0.2 m box centred at (0.7, 0, -0.12). Not a floor, one bench, pinned to one spot, leaving everything nearer than x=0.4 unguarded. It is gone. The wrist camera now feedsPointCloudSelfFilterandRayTracingVoxelMapintoManipulationModule.voxel_mapon both the arm and the sim, and what the camera sees defines the obstacles. The removal and the replacement ship together on purpose — dropping the box first would leave the hardware planner with no ground at all.Four things were needed to make the chain carry data, all of them silent when wrong. Both cameras default
enable_pointcloudto False. The self filter needs a capture-time transform for every collision link and drops a whole cloud when one is missing, so the model publishes all fifteen. Its stock 20 ms TF tolerance cannot bracket the 10 Hz periodManipulationModulepublishes robot TF at, so most clouds found no transform at all; the override is one full publish period, derived from that rate rather than picked — the module default is arguably wrong for anyManipulationModule-fed filter, which is worth a follow-up. And the two edges whose stream names differ are remapped.Two consequences. The target object is itself mapped geometry once the map feeds the planner, so a collision-checked plan into it can only ever be rejected; the pregrasp-to-grasp leg and the retreat are now straight-line
move_linearservos with checking off, which is what that primitive is for, and only the approach to the pregrasp stays a checked plan. And #3714 added theOCTREEobstacle type and its planner support but no rendering anywhere, so the viser scene's dispatch raisedunsupported obstacle typeand drew a greyMESH RENDER FAILEDproxy box per cell over the map. Octrees now render as a new case in that same dispatch — occupied cells as one point cloud sized to the cell edge, because a box per cell is tens of thousands of scene nodes.Verified in the grasp sim:
mapping/voxel-mapinstalls as an octree obstacle and the self filter stops dropping clouds once TF is up.Eighth of nine in the xArm grasping re-landing stack.