Add admittance-based move-to-touch for vacuum bracket pick#616
Draft
griswaldbrooks wants to merge 4 commits intomainfrom
Draft
Add admittance-based move-to-touch for vacuum bracket pick#616griswaldbrooks wants to merge 4 commits intomainfrom
griswaldbrooks wants to merge 4 commits intomainfrom
Conversation
…ndling cartesian fixes Addresses two distinct bugs surfaced in the factory_sim package by PickNikRobotics/moveit_pro#18180. 1. Bracket spawn-position errors at sim reset. <replicate count=3> in description/scene.xml interacts badly with MuJoCo 3.2.7's keyframe parser, silently corrupting the first replica's qpos slot -- bracket-0-0 lands under the pedestal on every reset. MuJoCo 3.3.5 hard-errors on the same input ("Keyframe 'default' has invalid qpos size, got 48, should be 13"). Fix replaces <replicate> with three explicit <include> blocks of per-bracket geometry files. 2. Tool-handling cartesian rejections at the holder. Two cartesian motions in the suction-gripper subtrees deterministi- cally land link_6 at the suction_gripper world object (depth = 0): - Pick Up Tool from Holder's +0.10m approach push. - Place Tool in Tool Holder's 0.35m post-detach Retract. The pair link_6 <-> suction_gripper has no ACM entry (default NEVER), so the planner rejects both segments. Previously masked by <replicate>'s non-deterministic IK ordering. Fix opts each segment out of environment-collision checks; the cartesian paths are purely vertical along tool0 Z, so wrist swings into other env are precluded by the path itself. Plus three small simulator-side fixes folded in (keyframe qpos row order corrected to match MuJoCo's body compile order; default joint path tolerance bumped to 0.65 to clear joint-6 deviation against the 0.6 limit; MuJoCo arena bumped from 15M to 64M to prevent contact- constraint overflow during the place phase, which had been segfaulting ros2_control_node via the viewer's next sync). Edits: - description/scene.xml -- keyframe qpos rows reordered; <replicate> block replaced with three explicit <include>s; arena bumped to 64M. - description/bracket_collision_geometry_{0,1,2}.xml -- new per-bracket geometry files referenced by the includes. - config/control/picknik_fanuc.ros2_control.yaml -- default_path_- tolerance bumped to 0.65. - objectives/pick_up_tool_from_holder.xml -- ignore_environment_- collisions="true" on the +0.10m SetupMTCMoveAlongFrameAxis; SetupMTCIgnoreCollisionsBetweenObjects(link_5;link_6;suction_- gripper) added matching the sibling subtree pattern. - objectives/retract.xml -- parameterized ignore_environment_- collisions as a new input port with default "false" so existing call sites keep their hardcoded behavior; threaded through to the underlying SetupMTCMoveAlongFrameAxis. - objectives/place_tool_in_tool_holder.xml -- passes ignore_- environment_collisions="true" at its 0.35m Retract call site so the post-detach retreat clears the same depth = 0 contact. Refs PickNikRobotics/moveit_pro#18180 Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
…tory-sim-issues Fix: 18180 factory_sim Pick and Place sim setup and tool handling
Wires a wrist force/torque sensor through MuJoCo, ros2_control, and the
admittance controller, then rewrites Vacuum Pick Bracket Part Subtree to
descend onto the bracket under z-axis admittance instead of an open-loop
+0.055 m cartesian push. The FTS reads the weld constraint force at the
suction-tool flange interface, so when the cup contacts the bracket the
admittance back-drives the trajectory while MuJoCo activates the
gripper-to-bracket weld at the actual contact pose.
FTS wiring:
- Add a <site name="wrist_ft_sensor"> at gripper_base origin (suction_tool.xml)
and a top-level <sensor><force/><torque/></sensor> in scene.xml. MujocoSystem
auto-registers these as wrist_ft_sensor/{force,torque}.{x,y,z} state
interfaces using the site name.
- Add force_torque_sensor_broadcaster to controller_manager and configure
it (sensor_name: wrist_ft_sensor, frame_id: tool0). Add it to
controllers_active_at_startup.
- Set JTAC ft_sensor_name: wrist_ft_sensor and migrate sensor_frame /
ee_frame to the plural sensor_frames / ee_frames forms (silences the
deprecation warnings).
Subtree rewrite (vacuum_pick_bracket_part_subtree.xml):
- Drop SetupMTCMoveAlongFrameAxis (the open-loop push the prior comment
flagged as wanting force feedback).
- Plan the touch as a separate cartesian segment (PlanCartesianPath, +0.07 m
along grasp_link Z, position-only) for a 1.5 cm overshoot beyond the prior
5.5 cm push so admittance has room to back off.
- Tare the FTS (CallTriggerService /joint_trajectory_admittance_controller/
zero_fts/tool0), enable z-only admittance, ExecuteTrajectory with loose
path/goal tolerance and force threshold off.
- Restore plain JTAC tracking with an admittance-disable params yaml so
downstream MTC motions aren't offset.
Two new params yamls:
- vacuum_touch_admittance_params.yaml: z-only, 180 N/m stiffness, heavy
damping, mirroring lab_sim/objectives/push_button_admittance_params.yaml.
- vacuum_touch_admittance_disable.yaml: all axes off.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Two bugs in the prior commit that prevented Vacuum Pick Bracket Part
Subtree from completing under Pick Brackets from Left Bin's Repeat
loop:
1. SetAdmittanceParameters reads BT XML attributes as parameter
overrides, so the human-readable `name="..."` annotation was being
interpreted as a missing parameter name and aborting with
"Failed to parse admittance controller parameters: Parameter
'<the name string>' not found". Replaced the BT step labels with
XML comments above each Action so intent is still readable.
2. AddPoseStampedToVector appends. The parent BT wraps this subtree
in <Repeat num_cycles="3">, and the {touch_path} blackboard variable
is inherited across SubTree invocations, so iteration 2+ sent a
path with stale waypoints to PlanCartesianPath. The planner
rejected it: "Bad initial conditions: Waypoint N in path closer
than '2 * blending_radius' to neighbors. Distance to next waypoint:
~0". Added ResetPoseStampedVector before the append so the path
starts empty each iteration.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
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
2 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.
Summary
Replaces the open-loop +0.055 m cartesian push in
Vacuum Pick Bracket Part Subtreewith a force-feedback move-to-touch:<site name="wrist_ft_sensor">on the suction tool flange + MuJoCo<sensor><force/><torque/></sensor>block.MujocoSystemauto-registerswrist_ft_sensor/{force,torque}.{x,y,z}state interfaces. Addsforce_torque_sensor_broadcasterand points JTAC at it (ft_sensor_name: wrist_ft_sensor). Migrates JTACsensor_frame/ee_frameto the pluralsensor_frames/ee_framesforms (silences deprecation warnings).lab_sim/objectives/push_button_with_a_trajectory.xmlshape). Tares the FTS via/joint_trajectory_admittance_controller/zero_fts/tool0before each touch, plans+0.07 malonggrasp_linkZ (1.5 cm overshoot), executes with admittance + loose path tolerance, then disables admittance for downstream MTC motions in the BT.vacuum_touch_admittance_params.yaml(z-only, 180 N/m, heavy damping — copies push-button defaults) andvacuum_touch_admittance_disable.yaml(all axes off).Notes / caveats
v9.2; once Fix: 18180 factory_sim Pick and Place sim setup and tool handling #613's v9.2 merges intomainthe diff will reduce to just the two move-to-touch commits.SetAdmittanceParametersreads BT XML attributes as parameter overrides — do not addname="..."to those Action calls. Documented inline in the subtree.AddPoseStampedToVectorappends. The parentPick Brackets from Left Binruns the subtree under<Repeat num_cycles="3">, so the{touch_path}blackboard entry would carry stale waypoints across iterations. Subtree resets it viaResetPoseStampedVectorfirst./force_torque_sensor_broadcaster/wrenchtopic during a run to confirm a force spike actually triggers admittance back-off (vs. the cup just barely missing the bracket and reaching geometric goal).Test plan
factory_sim, confirm 5 controllers active includingforce_torque_sensor_broadcasterand/force_torque_sensor_broadcaster/wrenchpublishes withframe_id: tool0.Pick Brackets from Left Binend-to-end (3 picks). Confirm each iteration: tool pickup → vision → MTC approach → tare → admittance descent → cup-bracket weld activation → AttachURDF → drop on jig → drop in right bin → return home.force.zon the wrench topic spikes when contact happens.Pick and Place Brackets from Left Bin) to verify the admittance-disable step at the end of the subtree leaves downstream motions clean.🤖 Generated with Claude Code