From 9c083c91a66d05d1c3dc8ed54f7c629732b5fdd6 Mon Sep 17 00:00:00 2001 From: domrachev03 Date: Fri, 20 Mar 2026 21:25:53 +0900 Subject: [PATCH 1/3] feat: expose variable stiffness interface and add example Add runtime Cartesian stiffness control to crisp_py, matching the C++ controller's target_stiffness topic (Float64MultiArray with 6 elements). Changes: - RobotConfig: add target_stiffness_topic field - Robot: create stiffness publisher and set_stiffness() method - Robot.reset_targets: document that stiffness is intentionally not reset - examples/21_variable_stiffness.py: demo high/medium/low stiffness switching --- crisp_py/robot/robot.py | 33 ++++++++++++++++++++++++ crisp_py/robot/robot_config.py | 2 ++ examples/21_variable_stiffness.py | 43 +++++++++++++++++++++++++++++++ 3 files changed, 78 insertions(+) create mode 100644 examples/21_variable_stiffness.py diff --git a/crisp_py/robot/robot.py b/crisp_py/robot/robot.py index bff51be..8c4defc 100644 --- a/crisp_py/robot/robot.py +++ b/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. diff --git a/crisp_py/robot/robot_config.py b/crisp_py/robot/robot_config.py index a635d8b..bcf32ee 100644 --- a/crisp_py/robot/robot_config.py +++ b/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" diff --git a/examples/21_variable_stiffness.py b/examples/21_variable_stiffness.py new file mode 100644 index 0000000..b9f2ffd --- /dev/null +++ b/examples/21_variable_stiffness.py @@ -0,0 +1,43 @@ +"""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: + - variable_stiffness.enabled must be set to true in the controller parameters + - 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") + +# Enable variable stiffness on the controller +robot.cartesian_controller_parameters_client.set_parameters( + [("variable_stiffness.enabled", True)] +) + +print("Robot ready. Maintaining current position.") +print() + +# High stiffness (default-like values) +print("Setting HIGH stiffness: translational=[600, 600, 600], rotational=[30, 30, 30]") +robot.set_stiffness(translational=[600.0, 600.0, 600.0], rotational=[30.0, 30.0, 30.0]) +input("Press Enter to switch to MEDIUM stiffness...") + +# Medium stiffness +print("Setting MEDIUM stiffness: translational=[200, 200, 200], rotational=[15, 15, 15]") +robot.set_stiffness(translational=[200.0, 200.0, 200.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() From cdaa5244f702e259b1bb301f74316fe0a246915c Mon Sep 17 00:00:00 2001 From: domrachev03 Date: Fri, 20 Mar 2026 21:37:34 +0900 Subject: [PATCH 2/3] fix: update variable stiffness example with actual working parameters Use k_pos=900, k_rot=45 as high stiffness (matching actual controller config), and adjust medium values accordingly. --- examples/21_variable_stiffness.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/21_variable_stiffness.py b/examples/21_variable_stiffness.py index b9f2ffd..70dd099 100644 --- a/examples/21_variable_stiffness.py +++ b/examples/21_variable_stiffness.py @@ -25,14 +25,14 @@ print("Robot ready. Maintaining current position.") print() -# High stiffness (default-like values) -print("Setting HIGH stiffness: translational=[600, 600, 600], rotational=[30, 30, 30]") -robot.set_stiffness(translational=[600.0, 600.0, 600.0], rotational=[30.0, 30.0, 30.0]) +# 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=[200, 200, 200], rotational=[15, 15, 15]") -robot.set_stiffness(translational=[200.0, 200.0, 200.0], rotational=[15.0, 15.0, 15.0]) +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 From 05c4f9e70484a8c101e22ae8d3817010eb5842e2 Mon Sep 17 00:00:00 2001 From: domrachev03 Date: Sat, 21 Mar 2026 17:10:32 +0900 Subject: [PATCH 3/3] fix: remove variable_stiffness.enabled param set from example The subscriber is now always created, so no need to enable it at runtime. --- examples/21_variable_stiffness.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/examples/21_variable_stiffness.py b/examples/21_variable_stiffness.py index 70dd099..ff9f364 100644 --- a/examples/21_variable_stiffness.py +++ b/examples/21_variable_stiffness.py @@ -5,7 +5,6 @@ stiffness is changed from high to medium to low. Requirements: - - variable_stiffness.enabled must be set to true in the controller parameters - The cartesian impedance controller must be active """ @@ -17,11 +16,6 @@ # Switch to cartesian impedance controller robot.controller_switcher_client.switch_controller("cartesian_impedance_controller") -# Enable variable stiffness on the controller -robot.cartesian_controller_parameters_client.set_parameters( - [("variable_stiffness.enabled", True)] -) - print("Robot ready. Maintaining current position.") print()