[feat] Admittance Controller

[domrachev03]

Cartesian Admittance Controller with Cartesian Impedance in outer loop

Summary

• Implement a new CartesianAdmittanceController ros2_control plugin that combines an admittance inner loop (virtual mass-spring-damper) with a Cartesian impedance outer loop
• Supports runtime variable stiffness for both the admittance and impedance layers via ROS2 topics

How the Admittance Controller Works

The controller has two cascaded loops:

Inner Loop: Admittance (Virtual MSD)

The admittance layer maintains an internal pose state $x_{inner}$ that evolves as a virtual mass-spring-damper driven by external forces:

$$M_{adm} \ddot{x} + D_{adm} \dot{x} + K_{adm} (x_{inner} - x_{desired}) = F_{ext}$$

where:

• $M_{adm}$ — virtual inertia matrix ($6 \times 6$ diagonal), controls how quickly the system responds
• $D_{adm}$ — virtual damping matrix ($6 \times 6$ diagonal), controls oscillation suppression
• $K_{adm}$ — virtual stiffness matrix ($6 \times 6$ diagonal), controls how strongly the system returns to $x_{desired}$
• $F_{ext}$ — external wrench from the F/T sensor topic, transformed from the sensor measurement frame to world-aligned frame using Pinocchio's changeReferenceFrame
• $x_{desired}$ — the commanded target pose (from target_pose topic)

Note that this required an external F/T sensor. This could also be replaced by the robot's external wrench estimation (for example, as provided by a Franka manipulator). This also requires proper URDF, where there is a separate frame for F/T wrench measurement: the controller transforms measured force from local frame to world-aligned pinocchio frame

Integration uses semi-implicit Euler on SE3 manifold at each control cycle:

$$\ddot{x} = M_{adm}^{-1} \left( F_{ext} - D_{adm} \dot{x}_{inner} - K_{adm} \cdot \text{SE3Error}(x_{inner}, x_{desired}) \right)$$

$$\dot{x}_{inner} \leftarrow \dot{x}_{inner} + \ddot{x} \cdot \Delta t$$

$$x_{inner} \leftarrow \exp_6(\dot{x}_{inner} \cdot \Delta t) \cdot x_{inner}$$

Outer Loop: Impedance

The resulting position $x_{inner}$ is used as a target for the outer loop Cartesian controller. The outer controller is identical to the one at https://github.com/utiasDSL/crisp_controllers/blob/main/src/cartesian_controller.cpp. Unfortunately, I was unable to properly inherit from the Cartesian Controller, since most of its fields are private.

Adaptive stiffness parameters

Similar to #46, the admittance controller introduces variable stiffness for both inner admittance & outer impedance controllers, which could be configured separately.

Parameters

Admittance-specific parameters

admittance:
  mass_{x,y,z}:           # Virtual mass for translation (default 1.0)
  mass_{rx,ry,rz}:         # Virtual mass for rotation (default 0.1)
  stiffness_{x,y,z}:       # Admittance stiffness, translation (default 200.0)
  stiffness_{rx,ry,rz}:    # Admittance stiffness, rotation (default 10.0)
  damping_{x,y,z}:         # Admittance damping, translation (default 50.0)
  damping_{rx,ry,rz}:      # Admittance damping, rotation (default 5.0)

ft_sensor:
  topic:                    # F/T sensor topic name (default "ft_sensor_wrench")
  frame:                    # URDF frame where the sensor measures (default "" -> end_effector_frame)

variable_max_impedance_stiffness:
  translational:            # Max impedance stiffness for translation (default 900.0)
  rotational:               # Max impedance stiffness for rotation (default 60.0)

variable_max_admittance_stiffness:
  translational:            # Max admittance stiffness for translation (default 900.0)
  rotational:               # Max admittance stiffness for rotation (default 60.0)

variable_admittance_stiffness:
  enabled:                  # Enable admittance stiffness topic subscriber (default false)
  topic:                    # Topic for runtime K_adm updates (default "target_admittance_stiffness")

variable_stiffness:
  enabled:                  # Enable impedance stiffness topic subscriber (default false)
  topic:                    # Topic for runtime impedance K updates (default "target_stiffness")

All CartesianController parameters are also available for the impedance outer loop (task.*, nullspace.*, filter.*, friction.*, etc.).

Changes

File	Change
NEW cartesian_admittance_controller.hpp	Full header with admittance state (inner_SE3_, inner_motion_), MSD matrices, F/T sensor buffer/frame ID, variable stiffness buffers
NEW cartesian_admittance_controller.cpp	Complete implementation: admittance integration with correct F/T frame transform, impedance outer loop, all compensation torques, variable stiffness with max bounds and 100ms throttled logging
NEW cartesian_admittance_controller.yaml	All parameters: impedance outer loop, admittance MSD, F/T sensor (topic + frame), variable stiffness with enable gates and max bounds
CMakeLists.txt	Add parameter generation, source file, link library
crisp_controllers.xml	Register plugin

Notes

• The adaptive stiffness is a completely optional part of this implementation. To use adaptive stiffness, changes in crisp_py are required. PR with the corresponding change will be opened shortly
• I am concerned about the need to reimplement the Cartesian controller inside the admittance controller implementation. If you agree, we may modify Cartesian (and maybe other) controllers to make most fields protected instead of private. I believe this would simplify implementation and keep the outer Cartesian controller implementation always up-to-date between the admittance outer and regular Cartesian controllers.
• Special thanks to @lvjonok for participating in the discussion & preliminary review process.

[danielsanjosepro]

First of all, thanks for this contribution! This is amazing :) Have you tested this with the FR3 or with the UR robots?

[Copilot]

Pull request overview

Adds a new ROS2 ros2_control plugin implementing a cascaded Cartesian admittance (inner virtual mass-spring-damper) with a Cartesian impedance/OSC outer loop, including optional runtime stiffness updates via topics.

Changes:

• Introduces CartesianAdmittanceController controller implementation + header.
• Adds a full parameter schema (cartesian_admittance_controller.yaml) and generates a new parameter library target.
• Registers the controller as a plugin and links it into the main library build.

Reviewed changes

Copilot reviewed 5 out of 5 changed files in this pull request and generated 8 comments.

Show a summary per file

File	Description
src/cartesian_admittance_controller.cpp	Implements the admittance inner loop, impedance outer loop, subscriptions, and safety checks.
include/crisp_controllers/cartesian_admittance_controller.hpp	Declares controller class, state, buffers, and parameter plumbing.
src/cartesian_admittance_controller.yaml	Defines controller parameters for outer-loop impedance + inner-loop admittance + F/T sensor + variable stiffness.
CMakeLists.txt	Adds parameter generation for the new controller and links the generated library.
crisp_controllers.xml	Registers the new controller plugin for pluginlib discovery.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

setStiffnessAndDamping() checks if (params_.nullspace.damping) but nullspace.damping is a double with default -1.0 (meaning “auto”). Since -1.0 is truthy, this branch will run and assign a negative damping, which is unstable. Use an explicit numeric check (e.g., > 0.0) to decide whether to override the computed damping.

[Copilot]

The admittance orientation error is computed by taking the vector part of the quaternion error ([x,y,z]) and rotating it into world frame. This is not the SO(3) logarithm and produces incorrect magnitudes for non-small rotations (e.g., near 180°), which can destabilize the admittance spring term. Consider computing rotational error via pinocchio::log3(R_inner.transpose() * R_desired) (and then mapping to the desired reference frame) to match the impedance loop’s error definition.

[danielsanjosepro]

+1 if I am not mistaken this is how we do it in the cartesian controller

[Copilot]

ft_sensor_frame_id is set via model_.getFrameId(params_.ft_sensor.frame) without validating that the frame name exists. If the parameter is misspelled or missing from the URDF, this can silently select an unintended frame (or an invalid id) and corrupt the wrench transform. Add an explicit model_.existFrame(params_.ft_sensor.frame) (or equivalent) check and fail configuration with a clear error if the frame does not exist.

Suggested change

	ft_sensor_frame_id = model_.getFrameId(params_.ft_sensor.frame);
	RCLCPP_INFO(get_node()->get_logger(),
	"Using F/T sensor frame: %s (id=%d)", params_.ft_sensor.frame.c_str(), ft_sensor_frame_id);
	if (!model_.existFrame(params_.ft_sensor.frame)) {
	RCLCPP_ERROR(
	get_node()->get_logger(),
	"F/T sensor frame '%s' does not exist in the robot model. Please check the "
	"ft_sensor.frame parameter and the URDF frames.",
	params_.ft_sensor.frame.c_str());
	return CallbackReturn::ERROR;
	}
	ft_sensor_frame_id = model_.getFrameId(params_.ft_sensor.frame);
	RCLCPP_INFO(
	get_node()->get_logger(),
	"Using F/T sensor frame: %s (id=%d)",
	params_.ft_sensor.frame.c_str(),
	ft_sensor_frame_id);

[Copilot]

setStiffnessAndDamping() computes task damping from the parameter stiffness values (params_.task.k_*) even when topic stiffness overrides are active. If the intent is “critical damping when d_* < 0”, this should be based on the effective stiffness actually used (topic or params), otherwise the controller can become under/over-damped when stiffness changes at runtime.

Suggested change

	// For each axis, use explicit damping if > 0, otherwise compute from stiffness
	damping.diagonal()
	<< (params_.task.d_pos_x > 0 ? params_.task.d_pos_x : 2.0 * std::sqrt(params_.task.k_pos_x)),
	(params_.task.d_pos_y > 0 ? params_.task.d_pos_y : 2.0 * std::sqrt(params_.task.k_pos_y)),
	(params_.task.d_pos_z > 0 ? params_.task.d_pos_z : 2.0 * std::sqrt(params_.task.k_pos_z)),
	(params_.task.d_rot_x > 0 ? params_.task.d_rot_x : 2.0 * std::sqrt(params_.task.k_rot_x)),
	(params_.task.d_rot_y > 0 ? params_.task.d_rot_y : 2.0 * std::sqrt(params_.task.k_rot_y)),
	(params_.task.d_rot_z > 0 ? params_.task.d_rot_z : 2.0 * std::sqrt(params_.task.k_rot_z));
	// For each axis, use explicit damping if > 0, otherwise compute critical damping
	// from the effective stiffness currently in use (stiffness matrix diagonal).
	damping.diagonal()
	<< (params_.task.d_pos_x > 0 ? params_.task.d_pos_x : 2.0 * std::sqrt(stiffness(0, 0))),
	(params_.task.d_pos_y > 0 ? params_.task.d_pos_y : 2.0 * std::sqrt(stiffness(1, 1))),
	(params_.task.d_pos_z > 0 ? params_.task.d_pos_z : 2.0 * std::sqrt(stiffness(2, 2))),
	(params_.task.d_rot_x > 0 ? params_.task.d_rot_x : 2.0 * std::sqrt(stiffness(3, 3))),
	(params_.task.d_rot_y > 0 ? params_.task.d_rot_y : 2.0 * std::sqrt(stiffness(4, 4))),
	(params_.task.d_rot_z > 0 ? params_.task.d_rot_z : 2.0 * std::sqrt(stiffness(5, 5)));

[domrachev03]

And this is actually intended. The question is: when we update the controller's stiffness, should we update the damping accordingly?

After spending some time thinking about it, I decided that it would be better not to change it and always keep the same. Hence, damping is always derived from the original parameters of the controller, if they are not specified

[Copilot]

The docstring for parse_ft_sensor_() says it “applies filtering”, but the implementation currently only copies the wrench into ft_wrench_ without any filtering. Either update the comment to match behavior or add the intended filtering (e.g., EMA / deadband) to avoid misleading maintainers.

Suggested change

	* @brief Reads the F/T sensor data from the realtime buffer and applies filtering
	* @brief Reads the F/T sensor data from the realtime buffer and updates the internal wrench

[Copilot]

base_frame.description currently reads "Name of the end-effector frame" (copy/paste from end_effector_frame). This is misleading for users configuring the controller; update it to describe the base/root frame used for kinematics (or whatever base_frame actually means here).

Suggested change

	description: "Name of the end-effector frame"
	description: "Name of the base/root frame used as reference for kinematics computations"

[domrachev03]

Note that this typo is present in cartesian controller as well: https://github.com/utiasDSL/crisp_controllers/blob/3b234cb1a9c1f8554458b0303bfe36c65552e438/src/cartesian_controller.yaml#L13

[danielsanjosepro]

Thanks again for this 🙏 I have a just few discussion points :)

[danielsanjosepro]

Is this even possible?

[danielsanjosepro]

In general, I really like the way it is structured (a few redundant comments in the update function) but the code looks great :)
There just a few things we could discuss before we proceed:

• this controller shares a lot of logic with the cartesian controller and it would make sense to abstract this logic in some way. The pro of doing this is that we dont have so much code duplication. The con is that we need to refactor the other controller, and make sure that we stay backwards compatible (in the case of a refactoring I would rather make a major version bump for the whole package)
• similar to the cartesian_controller, this controller does a few mem allocations inside the update loop which could potentially be pre allocated. But I guess this is still not too bad and we can live with this :)

One final comment: add yourself (and Lev as well) to the docs with your contribution and also feel free to add an extra description of the admittance controller somewhere. Ideally in the controller details section!

Let us discuss the first point if you want, if you feel like the refactor is just much unnecessary work (including testing the change) we can also consider leave it as it is

[domrachev03]

Thank you for your feedback! Regarding shared logic with the Cartesian controller, I also noticed that it would actually require a bit more refactoring than just changing some fields from private to protected (as I originally suggested).

With that in mind, let's first modify, debug & merge this PR as a standalone controller, then proceed with refactoring separately. Let me know what you think!

[domrachev03]

First of all, thanks for this contribution! This is amazing :) Have you tested this with the FR3 or with the UR robots?

Thank you for taking the time to review them! We've tested it only on Franka Panda robot yet. I think it would be better to test this on both of them to make sure we're not missing anything. I will try to run it on UR and let you know when it would work properly!

[domrachev03]

Hello, @danielsanjosepro! I believe I have addressed most of your suggestions, and commented the rest. Also I've tested admittance controller using pixi_ur_ros2, see lvjonok/pixi_ur_ros2#5. Finally, I've modified documentation to include myself in contributors & briefly describe admittance controller implementation.

Before we proceed, I have several concerns:

1. I am little bit afraid of admittance controller behavior in case F/T sensor topic stops publishing data. From what I've observed, the controller will proceed with last observed value, which (in case it's non-zero) would result in system divergence. However, I have not found any safeguards in other controllers, so I wonder whether I should fix this for admittance controller or not?
2. I found a small typo: wrong gh profile link in https://github.com/KAIST-IRiS-Haptics-Telerobotics/crisp_controllers/blob/3b234cb1a9c1f8554458b0303bfe36c65552e438/docs/index.md?plain=1#L176
3. I am not sure why ROS2 rolling pipeline is failing, and what is an intended way to fix it. If we would proceed with merging, could you suggest what may be the issue, please?

With that in mind, I believe we could proceed with merging this PR. Once again, thank you for your detailed feedback & opinion!

[danielsanjosepro]

Thanks for testing this out! It is always nice to have a few examples using the controller :)

1. this can happen in a later PR: a guard clause at the beginning of the update call could stop the controller if the last timestamp of the ft sensor exceeds a value, then we just safely deactivate the controller. It would be also nice in the future to add the option to use the ft measuremnts as a state interface instead of a topic (to reduce jitter in the ft sensor readings and make the controller more robust). But it is better to include all of these things later
2. upsi, thanks!
3. Yes, the ROS rolling pipeline is also haunting me. It needs to be addressed separately anyway, so dont worry about it:)

Thank you for this great addition!