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..ff9f364 --- /dev/null +++ b/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()