[feat] Admittance Controller & Variable stiffness support

crisp_py/config/control/default_admittance.yaml

@@ -0,0 +1,58 @@
+# Default admittance controller parameters
+
+# Impedance outer loop
+limit_error: true
+limit_torques: true
+max_delta_tau: 2.0
+nullspace.stiffness: 5.0
+stop_commands: false
+
+task.k_pos_x: 900.0
+task.k_pos_y: 900.0
+task.k_pos_z: 900.0
+task.k_rot_x: 45.0
+task.k_rot_y: 45.0
+task.k_rot_z: 45.0
+
+task.d_pos_x: 60.0
+task.d_pos_y: 60.0
+task.d_pos_z: 60.0
+task.d_rot_x: 10.0
+task.d_rot_y: 10.0
+task.d_rot_z: 10.0
+
+task.error_clip.rx: 0.5
+task.error_clip.ry: 0.5
+task.error_clip.rz: 0.5
+task.error_clip.x: 0.1
+task.error_clip.y: 0.1
+task.error_clip.z: 0.1
+
+use_coriolis_compensation: true
+use_friction: true
+use_gravity_compensation: false
+use_local_jacobian: true
+use_operational_space: false
+
+# Admittance parameters (virtual MSD)
+admittance.mass_x: 1.0
+admittance.mass_y: 1.0
+admittance.mass_z: 1.0
+admittance.mass_rx: 0.1
+admittance.mass_ry: 0.1
+admittance.mass_rz: 0.1
+admittance.stiffness_x: 200.0
+admittance.stiffness_y: 200.0
+admittance.stiffness_z: 200.0
+admittance.stiffness_rx: 10.0
+admittance.stiffness_ry: 10.0
+admittance.stiffness_rz: 10.0
+admittance.damping_x: 50.0
+admittance.damping_y: 50.0
+admittance.damping_z: 50.0
+admittance.damping_rx: 5.0
+admittance.damping_ry: 5.0
+admittance.damping_rz: 5.0
+
+# F/T sensor
+ft_sensor.topic: "ft_sensor_wrench"

crisp_py/robot/robot.py

@@ -9,6 +9,7 @@
 import rclpy.executors
 import yaml
 from geometry_msgs.msg import PoseStamped, TwistStamped, WrenchStamped
+from std_msgs.msg import Float64MultiArray
 from numpy.typing import NDArray
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
@@ -85,6 +86,9 @@ def __init__(
         self.joint_controller_parameters_client = ParametersClient(
             self.node, target_node=self.config.joint_trajectory_controller_name
         )
+        self.admittance_controller_parameters_client = ParametersClient(
+            self.node, target_node=self.config.cartesian_admittance_controller_name
+        )
 
         self._current_pose = None
         self._target_pose = None
@@ -112,6 +116,11 @@ def __init__(
         self._target_joint_publisher = self.node.create_publisher(
             JointState, self.config.target_joint_topic, qos_profile_system_default
         )
+        self._target_admittance_stiffness_publisher = self.node.create_publisher(
+            Float64MultiArray,
+            self.config.target_admittance_stiffness_topic,
+            qos_profile_system_default,
+        )
         if self.config.use_tf_pose:
             self._tf_pose = TfPose(
                 self.node,
@@ -503,6 +512,33 @@ def set_stiffness(
         msg.data = list(translational) + list(rotational)
         self._target_stiffness_publisher.publish(msg)
 
+    def set_admittance_stiffness(
+        self,
+        translational: List | NDArray | None = None,
+        rotational: List | NDArray | None = None,
+    ) -> None:
+        """Set the admittance stiffness for the admittance controller via topic.
+
+        This publishes an admittance stiffness update to the controller's variable
+        admittance stiffness topic. The value is latched by the controller.
+        Requires the controller parameter variable_admittance_stiffness.enabled to be true.
+
+        Args:
+            translational: Stiffness values [kx, ky, kz] for position. If None, zeros are used.
+            rotational: Stiffness values [krx, kry, krz] for orientation. If None, zeros are used.
+        """
+        if translational is None:
+            translational = [0.0, 0.0, 0.0]
+        if rotational is None:
+            rotational = [0.0, 0.0, 0.0]
+
+        assert len(translational) == 3, "Translational stiffness must be a 3D vector"
+        assert len(rotational) == 3, "Rotational stiffness must be a 3D vector"
+
+        msg = Float64MultiArray()
+        msg.data = list(translational) + list(rotational)
+        self._target_admittance_stiffness_publisher.publish(msg)
+
     def _wrench_to_wrench_msg(self, wrench: dict) -> WrenchStamped:
         """Convert a wrench dictionary to a ROS WrenchStamped message.
 

crisp_py/robot/robot_config.py

@@ -43,8 +43,11 @@ class RobotConfig:
 
     default_controller: str = "cartesian_impedance_controller"
     cartesian_impedance_controller_name: str = "cartesian_impedance_controller"
+    cartesian_admittance_controller_name: str = "cartesian_admittance_controller"
     joint_trajectory_controller_name: str = "joint_impedance_controller"
 
+    target_admittance_stiffness_topic: str = "target_admittance_stiffness"
+
     target_pose_topic: str = "target_pose"
     target_joint_topic: str = "target_joint"
     target_stiffness_topic: str = "target_stiffness"

examples/22_admittance_variable_stiffness.py

@@ -0,0 +1,48 @@
+"""Example demonstrating variable admittance stiffness for the admittance controller.
+
+This script shows how to change the admittance stiffness at runtime. The robot
+maintains its position while the admittance compliance is changed from stiff
+(resists external forces) to compliant (yields to external forces).
+
+Requirements:
+    - cartesian_admittance_controller must be loaded and available
+    - variable_admittance_stiffness.enabled must be set to true
+    - An F/T sensor must be publishing on ft_sensor_wrench topic
+"""
+
+from crisp_py.robot import make_robot
+
+robot = make_robot("fr3")
+robot.wait_until_ready()
+
+# Switch to admittance controller
+robot.controller_switcher_client.switch_controller("cartesian_admittance_controller")
+
+# Load default admittance parameters
+robot.admittance_controller_parameters_client.load_param_config(
+    "config/control/default_admittance.yaml"
+)
+
+print("Admittance controller active. An F/T sensor is required.")
+print("Push the robot gently to feel the compliance change.")
+print()
+
+# High admittance stiffness (stiff - resists external forces)
+print("Setting HIGH admittance stiffness: translational=[500, 500, 500], rotational=[30, 30, 30]")
+print("The robot should feel stiff and resist external forces.")
+robot.set_admittance_stiffness(translational=[500.0, 500.0, 500.0], rotational=[30.0, 30.0, 30.0])
+input("Push the robot. Press Enter for MEDIUM stiffness...")
+
+# Medium admittance stiffness
+print("Setting MEDIUM admittance stiffness: translational=[100, 100, 100], rotational=[10, 10, 10]")
+print("The robot should be moderately compliant.")
+robot.set_admittance_stiffness(translational=[100.0, 100.0, 100.0], rotational=[10.0, 10.0, 10.0])
+input("Push the robot. Press Enter for LOW stiffness...")
+
+# Low admittance stiffness (compliant - yields to external forces)
+print("Setting LOW admittance stiffness: translational=[20, 20, 20], rotational=[2, 2, 2]")
+print("The robot should feel very compliant and yield to pushes.")
+robot.set_admittance_stiffness(translational=[20.0, 20.0, 20.0], rotational=[2.0, 2.0, 2.0])
+input("Push the robot. Press Enter to exit...")
+
+robot.shutdown()