PickNikRobotics · griswaldbrooks · May 7, 2026 · May 8, 2026
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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,89 @@
         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. -->
+      <!-- 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"
+      />
+      <Action
+        ID="VisualizePose"
+        marker_lifetime="0.000000"
+        marker_name="touch_target"
+        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}"
+        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"
+      />
+      <!-- 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"
+      />
+      <!-- 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"
+      />
+      <!-- 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"
+        admittance_parameters_msg="{admittance_parameters_msg}"
+        goal_position_tolerance="0.100000"
+        joint_trajectory_msg="{touch_trajectory_msg}"
+        path_position_tolerance="0.250000"
+      />
+      <!-- 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"
+      />
       <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
@@ -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
@@ -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