Add admittance-based move-to-touch for vacuum bracket pick - #616
Draft
griswaldbrooks wants to merge 2 commits into
Draft
Add admittance-based move-to-touch for vacuum bracket pick#616griswaldbrooks wants to merge 2 commits into
griswaldbrooks wants to merge 2 commits into
Conversation
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>
Member
|
[written by AI] Stale PR sweep. Opened 2026-05-08, last commit 2026-05-08. We are cleaning up open PRs older than three months. State: GitHub reports it Recommendation: your call, and it turns on whether the vacuum bracket pick still needs force-based touch. If yes, rebase and mark it ready; conflicts only get worse from here. If the factory_sim vacuum pick moved on, close it.
|
Author
|
I think this is a valuable feature if I can get back around to it |
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