Variable Stiffness Controller

[domrachev03]

Runtime Variable Stiffness for Cartesian Impedance Controller

Summary

• Add runtime variable stiffness support to CartesianController via a ROS2 topic

Motivation

Some modern learning-based approaches, such as Advanced Compliance Policy suggest extending the action space of the policy with stiffness prediction, for Variable Stiffness Controller. This could be easily supported by crisp_controllers as well with minimal changes. This PR does exactly that

Behaviour

This PR adds a topic-based override: publishing a std_msgs/Float64MultiArray with 6 elements to the target_stiffness topic immediately overrides the stiffness matrix. The pipeline is as follows:

1. On startup, stiffness comes from parameters (existing behaviour, unchanged)
2. When a message is published to target_stiffness, the controller clips impedance to bounds and switches to using the topic-provided stiffness
3. The topic stiffness persists indefinitely until overridden by another message

Testing

From command line

# Set stiffness: [k_pos_x, k_pos_y, k_pos_z, k_rot_x, k_rot_y, k_rot_z]
ros2 topic pub --once /target_stiffness std_msgs/msg/Float64MultiArray \
  "{data: [900.0, 900.0, 900.0, 45.0, 45.0, 45.0]}"

# Lower stiffness for compliant behaviour
ros2 topic pub --once /target_stiffness std_msgs/msg/Float64MultiArray \
  "{data: [50.0, 50.0, 50.0, 5.0, 5.0, 5.0]}"

Parameters Added

variable_stiffness:
  enabled:       # bool, default false (unused — subscriber always created)
  topic:         # string, default "target_stiffness"

Notes

• To use this functionality, we need to extend crisp_py by adding one more method to the robot class. The implementation is ready and tested, so if you're interested in adding this feature, we're ready to open PR in crisp_py as well
• Special thanks to @lvjonok for participating in the discussion & preliminary review process.

[danielsanjosepro]

Hey! Thank you a lot for the addition. It is definitely a nice feature (better than calling params services to modify it). I just left a few comments but it definitely looks good :)

[danielsanjosepro]

This will spam messages if we are constantly changing stiffness, consider removing this or throttle it. Alternatively add a param to trigger logging

[danielsanjosepro]

This file already defines stiffness bounds (see a few lines above). The bounds here are below what we defined above. Probably it makes sense to call this variable_max_stiffness or something similar to not confuse it with the other one

[domrachev03]

@danielsanjosepro thank you for your comments, I believe I've fixed both issues

[domrachev03]

Hey, so what is the status of this PR? We've addressed your comments, so I wonder whether we could proceed further? Or should we make further changes?

[danielsanjosepro]

It lools good to me thanks for this!!