Add admittance-based move-to-touch for vacuum bracket pick

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.

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]
@@ -81,7 +98,7 @@ joint_trajectory_admittance_controller:
     # provide the gravity vector in the base frame.
     gravity_vector: [0.0, 0.0, -9.8]
     # Default maximum joint-space deviation accepted along the trajectory, if not specified in the goal message.
-    default_path_tolerance: 0.6
+    default_path_tolerance: 0.65
     # Default maximum joint-space deviation accepted at the trajectory end-point, if not specified in the goal message.
     default_goal_tolerance: 0.05
     # Default maximum absolute force torque readings allowed from the force torque sensor along each Cartesian space