[feat] Variable Impedance Controller

crisp_py/robot/robot.py

@@ -15,6 +15,7 @@
 from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default
 from scipy.spatial.transform import Rotation, Slerp
 from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64MultiArray
 
 from crisp_py.config.path import find_config, list_configs_in_folder
 from crisp_py.control.controller_switcher import ControllerSwitcherClient
@@ -105,6 +106,9 @@ def __init__(
         self._target_wrench_publisher = self.node.create_publisher(
             WrenchStamped, "target_wrench", qos_profile_system_default
         )
+        self._target_stiffness_publisher = self.node.create_publisher(
+            Float64MultiArray, self.config.target_stiffness_topic, qos_profile_system_default
+        )
         self._target_joint_publisher = self.node.create_publisher(
             JointState, self.config.target_joint_topic, qos_profile_system_default
         )
@@ -363,6 +367,8 @@ def reset_targets(self):
         self._target_pose = None
         self._target_joint = None
         self._target_wrench = None
+        # Note: stiffness is NOT reset here because it is latched by the controller
+        # and should persist across target resets
 
     def wait_until_ready(self, timeout: float = 10.0, check_frequency: float = 10.0):
         """Wait until the robot is ready for operation.
@@ -470,6 +476,33 @@ def set_target_wrench(
 
         self._target_wrench = {"force": np.array(force), "torque": np.array(torque)}
 
+    def set_stiffness(
+        self,
+        translational: List | NDArray | None = None,
+        rotational: List | NDArray | None = None,
+    ) -> None:
+        """Set the Cartesian stiffness for the impedance controller via topic.
+
+        This publishes a stiffness update to the controller's variable stiffness topic.
+        The value is latched by the controller -- it persists until a new value is published.
+        Requires the controller parameter variable_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_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

@@ -23,6 +23,7 @@ class RobotConfig:
         cartesian_impedance_controller_name (str): Name of the Cartesian impedance controller
         target_pose_topic (str): Topic name for publishing target poses
         target_joint_topic (str): Topic name for publishing target joint states
+        target_stiffness_topic (str): Topic name for publishing target stiffness
         current_pose_topic (str): Topic name for subscribing to current poses
         current_joint_topic (str): Topic name for subscribing to current joint states
         publish_frequency (float): Frequency for publishing control commands
@@ -46,6 +47,7 @@ class RobotConfig:
 
     target_pose_topic: str = "target_pose"
     target_joint_topic: str = "target_joint"
+    target_stiffness_topic: str = "target_stiffness"
     current_pose_topic: str = "current_pose"
     current_joint_topic: str = "joint_states"
     current_twist_topic: str = "current_twist"

examples/21_variable_stiffness.py

@@ -0,0 +1,37 @@
+"""Example demonstrating runtime variable stiffness for the Cartesian impedance controller.
+
+This script shows how to change the impedance stiffness at runtime using the
+variable stiffness topic. The robot maintains its current position while the
+stiffness is changed from high to medium to low.
+
+Requirements:
+    - The cartesian impedance controller must be active
+"""
+
+from crisp_py.robot import make_robot
+
+robot = make_robot("fr3")
+robot.wait_until_ready()
+
+# Switch to cartesian impedance controller
+robot.controller_switcher_client.switch_controller("cartesian_impedance_controller")
+
+print("Robot ready. Maintaining current position.")
+print()
+
+# High stiffness (current working values)
+print("Setting HIGH stiffness: translational=[900, 900, 900], rotational=[45, 45, 45]")
+robot.set_stiffness(translational=[900.0, 900.0, 900.0], rotational=[45.0, 45.0, 45.0])
+input("Press Enter to switch to MEDIUM stiffness...")
+
+# Medium stiffness
+print("Setting MEDIUM stiffness: translational=[300, 300, 300], rotational=[15, 15, 15]")
+robot.set_stiffness(translational=[300.0, 300.0, 300.0], rotational=[15.0, 15.0, 15.0])
+input("Press Enter to switch to LOW stiffness...")
+
+# Low stiffness
+print("Setting LOW stiffness: translational=[50, 50, 50], rotational=[5, 5, 5]")
+robot.set_stiffness(translational=[50.0, 50.0, 50.0], rotational=[5.0, 5.0, 5.0])
+input("Press Enter to exit...")
+
+robot.shutdown()