From 80fa06c4602e66bf132368d7709a580f42ed9db8 Mon Sep 17 00:00:00 2001
From: Griswald Brooks <griswald.brooks@gmail.com>
Date: Thu, 7 May 2026 19:29:01 -0400
Subject: [PATCH 1/2] feat(factory_sim): admittance move-to-touch for Vacuum
 Pick Bracket

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>
---
 src/factory_sim/config/config.yaml            |  1 +
 .../control/picknik_fanuc.ros2_control.yaml   | 27 ++++--
 src/factory_sim/description/scene.xml         |  8 ++
 src/factory_sim/description/suction_tool.xml  |  5 ++
 .../vacuum_pick_bracket_part_subtree.xml      | 90 ++++++++++++++-----
 .../vacuum_touch_admittance_disable.yaml      | 54 +++++++++++
 .../vacuum_touch_admittance_params.yaml       | 55 ++++++++++++
 7 files changed, 215 insertions(+), 25 deletions(-)
 create mode 100644 src/factory_sim/objectives/vacuum_touch_admittance_disable.yaml
 create mode 100644 src/factory_sim/objectives/vacuum_touch_admittance_params.yaml

diff --git a/src/factory_sim/config/config.yaml b/src/factory_sim/config/config.yaml
index fc7d3c107..30f59ae70 100644
--- a/src/factory_sim/config/config.yaml
+++ b/src/factory_sim/config/config.yaml
@@ -96,6 +96,7 @@ ros2_control:
   controllers_active_at_startup:
     - "joint_trajectory_admittance_controller"
     - "joint_state_broadcaster"
+    - "force_torque_sensor_broadcaster"
     - "tool_attach_controller"
     - "suction_cup_controller"
   # Load but do not start these controllers so they can be activated later if needed.
diff --git a/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml b/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml
index ae9a63c13..5c6d1600b 100644
--- a/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml
+++ b/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml
@@ -5,6 +5,8 @@ controller_manager:
       type: joint_state_broadcaster/JointStateBroadcaster
     joint_trajectory_admittance_controller:
       type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
+    force_torque_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
     # A controller to enable/disable the tool attachment interface at the robot flange.
     # In sim, this is modeled as a weld constraint in Mujoco.
     # In a real robot, this controller would activate a mechanism to attach/detach the tool at the robot flange.
@@ -48,17 +50,32 @@ joint_state_broadcaster:
       - position
       - velocity
 
+force_torque_sensor_broadcaster:
+  ros__parameters:
+    # Hardware interface name. Matches the MuJoCo <site> name; MujocoSystem
+    # exposes "<site>/force.{x,y,z}" and "<site>/torque.{x,y,z}" state interfaces.
+    sensor_name: wrist_ft_sensor
+    state_interface_names:
+      - force.x
+      - force.y
+      - force.z
+      - torque.x
+      - torque.y
+      - torque.z
+    # The frame the sensor reading is published in.
+    frame_id: tool0
+
 joint_trajectory_admittance_controller:
   ros__parameters:
     # Joint group to control.
     planning_group_name: manipulator
-    # Specifies the frame/link name of the force torque sensor. Must exist in the robot description.
-    sensor_frame: link_6
-    # Specifies the frame/link name of the end-effector frame. Must exist in the robot description.
-    ee_frame: grasp_link
+    # Specifies the frame/link names of the force torque sensor(s). Must exist in the robot description.
+    sensor_frames: [tool0]
+    # Specifies the frame/link names of the end-effector frame(s). Must exist in the robot description.
+    ee_frames: [grasp_link]
     # The name of the force/torque ros2_control hardware interface which will be used in the admittance calculation.
     # If empty, the controller will run without admittance.
-    ft_sensor_name: ""
+    ft_sensor_name: wrist_ft_sensor
     # Joint accelerations to use for immediate stops (e.g. when cancelling a running trajectory).
     # Chosen to match MoveIt configs.
     stop_accelerations: [30.0, 30.0, 30.0, 30.0, 30.0, 30.0]
diff --git a/src/factory_sim/description/scene.xml b/src/factory_sim/description/scene.xml
index e3778f581..a8f1a3661 100644
--- a/src/factory_sim/description/scene.xml
+++ b/src/factory_sim/description/scene.xml
@@ -419,6 +419,14 @@
   <!-- Add welds for suction gripping of parts -->
   <include file="box-welds.xml" />
 
+  <!-- Wrist force/torque sensor on the suction tool's flange interface site.
+       MujocoSystem auto-registers these as ros2_control state interfaces named
+       "wrist_ft_sensor/{force,torque}.{x,y,z}". -->
+  <sensor>
+    <force site="wrist_ft_sensor" />
+    <torque site="wrist_ft_sensor" />
+  </sensor>
+
   <keyframe>
     <!-- qpos layout, in compile order: -->
     <!-- robot [joint_1..joint_6] (6) -->
diff --git a/src/factory_sim/description/suction_tool.xml b/src/factory_sim/description/suction_tool.xml
index 04b27afe3..1146b7d49 100644
--- a/src/factory_sim/description/suction_tool.xml
+++ b/src/factory_sim/description/suction_tool.xml
@@ -14,6 +14,11 @@
         armature="0"
       />
       <site name="suction_gripper_attach_site" pos="0 0 0" euler="0 0 0" />
+      <!-- F/T sensor site at the tool-flange interface. When the suction tool is
+           welded to tool0 (tool_attachment_one), MuJoCo reports the weld
+           constraint force at this site, which is the wrist-to-tool reaction
+           force needed for admittance control. -->
+      <site name="wrist_ft_sensor" pos="0 0 0" euler="0 0 0" />
 
       <geom type="cylinder" size="0.03 0.02" pos="0 0 0.02" rgba=".1 .1 .1 1" />
       <body name="suction_cylinder" gravcomp="1" pos="0 0 0.04">
diff --git a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml
index 044a4d878..105c7ba73 100644
--- a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml
+++ b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml
@@ -1,9 +1,8 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Vacuum Pick Bracket Part Subtree">
-  <!--//////////-->
   <BehaviorTree
     ID="Vacuum Pick Bracket Part Subtree"
-    _description="Picks the bracket part using vacuum gripper.&#10;The pick and approach poses specific to the bracket part are hardcoded into this subtree."
+    _description="Picks the bracket part using vacuum gripper.&#10;Approaches via MTC, then descends with admittance-based move-to-touch so the suction cup seats on the bracket under force feedback (instead of an open-loop push)."
   >
     <Control ID="Sequence" name="">
       <SubTree ID="Activate Vacuum" _collapsed="true" />
@@ -20,14 +19,17 @@
         output_pose="{approach_pose}"
         translation_xyz="0;0;0.15"
         quaternion_xyzw="-0.0; -1.0; -0.0; -0.0"
-        name="Create approach pose before straight pick"
+        name="Create approach pose above bracket"
       />
+      <!-- MTC: plan and execute the long approach to a hover pose above the
+           bracket. No admittance for this phase — JTAC tracks the trajectory
+           rigidly. -->
       <Action
         ID="InitializeMTCTask"
         task="{mtc_task}"
         trajectory_monitoring="false"
         controller_names="joint_trajectory_admittance_controller"
-        task_id="pick_part"
+        task_id="approach_part"
         timeout="-1.000000"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
@@ -53,28 +55,76 @@
         trajectory_sampling_rate="100"
         velocity_scale_factor="1.000000"
       />
-      <Action
-        ID="SetupMTCMoveAlongFrameAxis"
-        acceleration_scale="0.5"
-        axis_frame="tool0"
-        axis_x="0.000000"
-        axis_y="0.000000"
-        axis_z="1.000000"
-        hand_frame="tool0"
-        ignore_environment_collisions="false"
-        max_distance="0.055"
-        min_distance="0.055"
-        planning_group_name="manipulator"
-        task="{mtc_task}"
-        velocity_scale="1.000000"
-        name="This move is done to push the vacuum gripper into the part a little to get it to collide. This causes the mujoco weld to activate. An improvement would be to detect the force on the gripper and stop"
-      />
       <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
       <SubTree
         ID="Execute MTC Solution"
         solution="{mtc_solution}"
         goal_duration_tolerance="-1.0"
       />
+      <!-- Move-to-touch: descend along grasp_link +Z under admittance until
+           the suction cup contacts the bracket. The 7 cm overshoot guarantees
+           contact within the planned path; admittance back-drives the
+           trajectory once the FT sensor reads the contact reaction. -->
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="grasp_link"
+        stamped_pose="{touch_target_pose}"
+        position_xyz="0;0;0.07"
+        name="Touch target: 7 cm below current cup pose along grasp_link Z"
+      />
+      <Action
+        ID="VisualizePose"
+        marker_lifetime="0.000000"
+        marker_name="touch_target"
+        marker_size="0.050000"
+        pose="{touch_target_pose}"
+      />
+      <Action
+        ID="AddPoseStampedToVector"
+        input="{touch_target_pose}"
+        vector="{touch_path}"
+      />
+      <Action
+        ID="PlanCartesianPath"
+        acceleration_scale_factor="0.200000"
+        blending_radius="0.020000"
+        debug_solution="{touch_debug_solution}"
+        ik_cartesian_space_density="0.010000"
+        ik_joint_space_density="0.100000"
+        joint_trajectory_msg="{touch_trajectory_msg}"
+        path="{touch_path}"
+        planning_group_name="manipulator"
+        position_only="true"
+        tip_links="grasp_link"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="0.100000"
+      />
+      <Action
+        ID="CallTriggerService"
+        service_name="/joint_trajectory_admittance_controller/zero_fts/tool0"
+        name="Tare wrist FTS so admittance reads zero at the start"
+      />
+      <Action
+        ID="SetAdmittanceParameters"
+        admittance_parameters_msg="{admittance_parameters_msg}"
+        config_file_name="vacuum_touch_admittance_params.yaml"
+        name="Enable z-only admittance for the touch"
+      />
+      <Action
+        ID="ExecuteTrajectory"
+        absolute_force_torque_threshold="1000;1000;1000;1000;1000;1000"
+        admittance_parameters_msg="{admittance_parameters_msg}"
+        goal_position_tolerance="0.100000"
+        joint_trajectory_msg="{touch_trajectory_msg}"
+        path_position_tolerance="0.250000"
+        name="Descend with admittance — back-drives on contact"
+      />
+      <Action
+        ID="SetAdmittanceParameters"
+        admittance_parameters_msg="{admittance_parameters_msg}"
+        config_file_name="vacuum_touch_admittance_disable.yaml"
+        name="Restore plain JTAC tracking (admittance off) for downstream motions"
+      />
       <Action
         ID="AddURDF"
         urdf_name="{part_name}"
diff --git a/src/factory_sim/objectives/vacuum_touch_admittance_disable.yaml b/src/factory_sim/objectives/vacuum_touch_admittance_disable.yaml
new file mode 100644
index 000000000..96657a6c4
--- /dev/null
+++ b/src/factory_sim/objectives/vacuum_touch_admittance_disable.yaml
@@ -0,0 +1,54 @@
+SetAdmittanceParameters:
+  controller_name: ""
+  end_effector_frame: "grasp_link"
+  control_frame: "grasp_link"
+  admittance:
+    # All axes off — restores plain trajectory tracking after the touch so
+    # downstream MTC motions in the subtree run without admittance offsets.
+    selected_axes:
+      x: False
+      y: False
+      z: False
+      rx: False
+      ry: False
+      rz: False
+    mass_position:
+      x: 0.1
+      y: 0.1
+      z: 0.1
+      min: 0.001
+      max: 100.0
+    mass_rotation:
+      x: 0.1
+      y: 0.1
+      z: 0.1
+      min: 0.001
+      max: 100.0
+    stiffness_position:
+      x: 180.0
+      y: 180.0
+      z: 180.0
+      min: 0.0
+      max: 1000.0
+    stiffness_rotation:
+      x: 180.0
+      y: 180.0
+      z: 180.0
+      min: 0.0
+      max: 1000.0
+    damping_ratio_position:
+      x: 100.0
+      y: 100.0
+      z: 100.0
+      min: 0.0
+      max: 100.0
+    damping_ratio_rotation:
+      x: 100.0
+      y: 100.0
+      z: 100.0
+      min: 0.0
+      max: 100.0
+    joint_damping:
+      value: 1.0
+      min: 0.0
+      max: 100.0
diff --git a/src/factory_sim/objectives/vacuum_touch_admittance_params.yaml b/src/factory_sim/objectives/vacuum_touch_admittance_params.yaml
new file mode 100644
index 000000000..3244214a0
--- /dev/null
+++ b/src/factory_sim/objectives/vacuum_touch_admittance_params.yaml
@@ -0,0 +1,55 @@
+SetAdmittanceParameters:
+  controller_name: ""
+  end_effector_frame: "grasp_link"
+  control_frame: "grasp_link"
+  admittance:
+    # Compliant only along the cup-approach axis (grasp_link +Z). All other
+    # axes are stiff so the cup tracks the planned x/y/yaw exactly while
+    # admittance handles the touch.
+    selected_axes:
+      x: False
+      y: False
+      z: True
+      rx: False
+      ry: False
+      rz: False
+    mass_position:  # kg
+      x: 0.1
+      y: 0.1
+      z: 0.1
+      min: 0.001
+      max: 100.0
+    mass_rotation:  # kg*m^2
+      x: 0.1
+      y: 0.1
+      z: 0.1
+      min: 0.001
+      max: 100.0
+    stiffness_position:  # N/m
+      x: 180.0
+      y: 180.0
+      z: 180.0
+      min: 0.0
+      max: 1000.0
+    stiffness_rotation:  # N*m/rad
+      x: 180.0
+      y: 180.0
+      z: 180.0
+      min: 0.0
+      max: 1000.0
+    damping_ratio_position:  # unitless
+      x: 100.0
+      y: 100.0
+      z: 100.0
+      min: 0.0
+      max: 100.0
+    damping_ratio_rotation:  # unitless
+      x: 100.0
+      y: 100.0
+      z: 100.0
+      min: 0.0
+      max: 100.0
+    joint_damping:  # 1/s
+      value: 1.0
+      min: 0.0
+      max: 100.0

From f2ecc91c2b008086c4f3197ff3384dd51c69ca89 Mon Sep 17 00:00:00 2001
From: Griswald Brooks <griswald.brooks@gmail.com>
Date: Thu, 7 May 2026 20:54:07 -0400
Subject: [PATCH 2/2] fix(factory_sim): make admittance move-to-touch run
 end-to-end

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>
---
 .../vacuum_pick_bracket_part_subtree.xml      | 23 +++++++++++++++----
 1 file changed, 18 insertions(+), 5 deletions(-)

diff --git a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml
index 105c7ba73..236adf775 100644
--- a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml
+++ b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml
@@ -65,12 +65,12 @@
            the suction cup contacts the bracket. The 7 cm overshoot guarantees
            contact within the planned path; admittance back-drives the
            trajectory once the FT sensor reads the contact reaction. -->
+      <!-- Touch target: 7 cm below the current cup pose along grasp_link +Z. -->
       <Action
         ID="CreateStampedPose"
         reference_frame="grasp_link"
         stamped_pose="{touch_target_pose}"
         position_xyz="0;0;0.07"
-        name="Touch target: 7 cm below current cup pose along grasp_link Z"
       />
       <Action
         ID="VisualizePose"
@@ -79,6 +79,15 @@
         marker_size="0.050000"
         pose="{touch_target_pose}"
       />
+      <!-- Clear the path vector — this subtree runs once per Repeat iteration
+           in the parent BT, and AddPoseStampedToVector appends. Without the
+           reset, iteration N+ sends a path with stale waypoints to
+           PlanCartesianPath, which rejects it ("waypoint closer than
+           2*blending_radius to neighbors"). -->
+      <Action
+        ID="ResetPoseStampedVector"
+        vector="{touch_path}"
+      />
       <Action
         ID="AddPoseStampedToVector"
         input="{touch_target_pose}"
@@ -99,17 +108,22 @@
         trajectory_sampling_rate="100"
         velocity_scale_factor="0.100000"
       />
+      <!-- Tare the wrist FTS so admittance reads zero at the start of the descent. -->
       <Action
         ID="CallTriggerService"
         service_name="/joint_trajectory_admittance_controller/zero_fts/tool0"
-        name="Tare wrist FTS so admittance reads zero at the start"
       />
+      <!-- Enable z-only admittance for the touch.
+           NOTE: SetAdmittanceParameters reads BT XML attributes as parameter
+           overrides, so do not add a `name=` attribute here — it would be
+           interpreted as a missing parameter name and abort. -->
       <Action
         ID="SetAdmittanceParameters"
         admittance_parameters_msg="{admittance_parameters_msg}"
         config_file_name="vacuum_touch_admittance_params.yaml"
-        name="Enable z-only admittance for the touch"
       />
+      <!-- Descend with admittance enabled — back-drives the trajectory once
+           the FTS reads contact reaction. -->
       <Action
         ID="ExecuteTrajectory"
         absolute_force_torque_threshold="1000;1000;1000;1000;1000;1000"
@@ -117,13 +131,12 @@
         goal_position_tolerance="0.100000"
         joint_trajectory_msg="{touch_trajectory_msg}"
         path_position_tolerance="0.250000"
-        name="Descend with admittance — back-drives on contact"
       />
+      <!-- Restore plain JTAC tracking before downstream MTC motions. -->
       <Action
         ID="SetAdmittanceParameters"
         admittance_parameters_msg="{admittance_parameters_msg}"
         config_file_name="vacuum_touch_admittance_disable.yaml"
-        name="Restore plain JTAC tracking (admittance off) for downstream motions"
       />
       <Action
         ID="AddURDF"