diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3e2c731..94fa9f3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -83,10 +83,16 @@ generate_parameter_library(
src/twist_broadcaster.yaml
)
+generate_parameter_library(
+ cartesian_admittance_controller_parameters
+ src/cartesian_admittance_controller.yaml
+)
+
add_library(
${PROJECT_NAME}
SHARED
src/cartesian_controller.cpp
+ src/cartesian_admittance_controller.cpp
src/pose_broadcaster.cpp
src/torque_feedback_controller.cpp
src/twist_broadcaster.cpp
@@ -114,6 +120,7 @@ endif()
target_link_libraries(${PROJECT_NAME}
PRIVATE
cartesian_controller_parameters
+ cartesian_admittance_controller_parameters
pose_broadcaster_parameters
torque_feedback_controller_parameters
twist_broadcaster_parameters
diff --git a/crisp_controllers.xml b/crisp_controllers.xml
index 0b0d64e..3784f2f 100644
--- a/crisp_controllers.xml
+++ b/crisp_controllers.xml
@@ -5,6 +5,12 @@
Control the 3D pose of the end-effector using impedance control or OSC with pinocchio model.
+
+
+ Cartesian admittance controller with impedance outer loop for compliant force-based interaction.
+
+
diff --git a/docs/getting_started_controller_details.md b/docs/getting_started_controller_details.md
index e4a664b..173c46d 100644
--- a/docs/getting_started_controller_details.md
+++ b/docs/getting_started_controller_details.md
@@ -69,6 +69,47 @@ The nullspace position can be set with `robot.set_target_joint(...)` when using
It will publish a target joint position which is interpreted as the nullspace target.
+## Variable Stiffness
+
+Both the joint and Cartesian controllers support **variable stiffness**, where the proportional gains $\mathbf{K}_p$ can be dynamically adjusted at runtime via a dedicated ROS2 topic. This enables adaptive compliance — for example, lowering stiffness during contact-rich phases and increasing it for precise positioning. The variable stiffness range is bounded by configurable minimum and maximum values.
+
+## Admittance Control
+
+The admittance controller adds a force-reactive layer on top of the Cartesian impedance controller, enabling compliant interaction with the environment using an external force/torque sensor.
+
+The controller has two cascaded loops:
+
+### Inner Loop: Admittance (Virtual Mass-Spring-Damper)
+
+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
+ This requires an **external F/T sensor** (or the robot's built-in external wrench estimation, e.g. as provided by Franka manipulators). The URDF must include a separate frame for the F/T sensor measurement — the controller transforms the measured force from the local sensor frame to the world-aligned Pinocchio frame.
+
+**Integration** uses semi-implicit Euler on the SE(3) manifold at each control cycle:
+
+$$\ddot{x} = M_{adm}^{-1} \left( F_{ext} - D_{adm} \dot{x}_{inner} - K_{adm} \cdot \text{Error}(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}$$
+
+The pose error $\text{Error}(x_{inner}, x_{desired})$ is computed using separate $\mathbb{R}^3$ translational and $SO(3)$ rotational errors (rather than a full $SE(3)$ logarithmic map) to avoid unnatural screw motions.
+
+### Outer Loop: Impedance
+
+The resulting pose $x_{inner}$ from the admittance layer is used as the target for an outer Cartesian impedance controller, which computes the required joint torques (identical to the [Cartesian control](#cartesian-control) described above).
+
## Safety and extras
The actual torque commands sent to the robot are clamped to the allowed torque limits and torque rate limits defined in the config.
diff --git a/docs/index.md b/docs/index.md
index 5cf9b5b..215cc7a 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -171,8 +171,7 @@ Grippers tested in real hardware:
Many thanks community contributions:
-
-- Lev Kozlov [@lvjonok](https://github.com/lvjonok) for testing and providing interfaces for the Panda/FER and UR with pixi.
+- Ivan Domrachev [@domrachev03](https://github.com/domrachev03) and Lev Kozlov [@lvjonok](https://github.com/lvjonok) for implementing the variable stiffness and admittance controllers, and also for testing and providing interfaces for the Panda/FER and UR with pixi.
- Vincenzo Orlando [@VinsOrl09](https://github.com/lvjonok) for testing and providing interfaces for the UR robots in docker containers.
- Linus Schwarz [@Linus-Schwarz](https://github.com/Linus-Schwarz) for testing and providing interfaces for the BOTA force-torque sensors.
- Niklas Schlueter [@niklasschlueter](https://github.com/niklasschlueter) for testing and providing interfaces for the DynaArm robot.
diff --git a/include/crisp_controllers/cartesian_admittance_controller.hpp b/include/crisp_controllers/cartesian_admittance_controller.hpp
new file mode 100644
index 0000000..59dc183
--- /dev/null
+++ b/include/crisp_controllers/cartesian_admittance_controller.hpp
@@ -0,0 +1,362 @@
+#pragma once
+
+/**
+ * @file cartesian_admittance_controller.hpp
+ * @brief Cartesian admittance controller with impedance outer loop for compliant force-based interaction
+ *
+ * Maintains an internal virtual mass-spring-damper (MSD) state that evolves based on external
+ * F/T sensor readings. The MSD output becomes the desired pose for an impedance (Cartesian)
+ * outer control loop. This is a separate class that contains equivalent impedance logic
+ * to CartesianController, not derived from it.
+ */
+
+#include
+#include
+#include
+
+#include // NOLINT(build/include_order)
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#if ROS2_VERSION_ABOVE_HUMBLE
+#include
+#else
+#include
+#endif
+
+#include
+#include "realtime_tools/realtime_buffer.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace crisp_controllers {
+
+/**
+ * @brief Cartesian admittance controller with impedance outer loop
+ *
+ * This controller implements a two-layer control scheme:
+ * 1. Inner admittance loop: A virtual mass-spring-damper system driven by external F/T sensor
+ * readings, producing a desired pose that tracks the commanded pose while being compliant
+ * to external forces.
+ * 2. Outer impedance loop: Standard Cartesian impedance (or OSC) control that tracks the
+ * inner loop's output pose, with all the same compensation torques as CartesianController.
+ */
+class CartesianAdmittanceController : public controller_interface::ControllerInterface {
+public:
+ /**
+ * @brief Get the command interface configuration
+ * @return Interface configuration specifying required command interfaces
+ */
+ [[nodiscard]] controller_interface::InterfaceConfiguration
+ command_interface_configuration() const override;
+
+ /**
+ * @brief Get the state interface configuration
+ * @return Interface configuration specifying required state interfaces
+ */
+ [[nodiscard]] controller_interface::InterfaceConfiguration
+ state_interface_configuration() const override;
+
+ /**
+ * @brief Update function called periodically
+ * @param time Current time
+ * @param period Time since last update
+ * @return Success/failure of update
+ */
+ controller_interface::return_type
+ update(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+ /**
+ * @brief Initialize the controller
+ * @return Success/failure of initialization
+ */
+ CallbackReturn on_init() override;
+
+ /**
+ * @brief Configure the controller
+ * @param previous_state Previous lifecycle state
+ * @return Success/failure of configuration
+ */
+ CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+ /**
+ * @brief Activate the controller
+ * @param previous_state Previous lifecycle state
+ * @return Success/failure of activation
+ */
+ CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+ /**
+ * @brief Deactivate the controller
+ * @param previous_state Previous lifecycle state
+ * @return Success/failure of deactivation
+ */
+ CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+private:
+ // ---- Subscriptions (same as CartesianController) ----
+
+ /** @brief Subscription for target pose messages */
+ rclcpp::Subscription::SharedPtr pose_sub_;
+ /** @brief Subscription for target joint state messages */
+ rclcpp::Subscription::SharedPtr joint_sub_;
+ /** @brief Subscription for target wrench messages */
+ rclcpp::Subscription::SharedPtr wrench_sub_;
+ /** @brief Subscription for variable impedance stiffness messages */
+ rclcpp::Subscription::SharedPtr stiffness_sub_;
+
+ /** @brief Subscription for F/T sensor messages */
+ rclcpp::Subscription::SharedPtr ft_sensor_sub_;
+ /** @brief Subscription for variable admittance stiffness messages */
+ rclcpp::Subscription::SharedPtr adm_stiffness_sub_;
+
+ /** @brief Flag to indicate if multiple publishers detected */
+ bool multiple_publishers_detected_;
+ /** @brief Expected maximum number of publishers per topic */
+ size_t max_allowed_publishers_;
+
+ // ---- Methods ----
+
+ /**
+ * @brief Set the impedance stiffness and damping matrices based on parameters
+ */
+ void setStiffnessAndDamping();
+
+ /**
+ * @brief Set the admittance mass, stiffness, and damping matrices from parameters
+ */
+ void setAdmittanceParameters();
+
+ /**
+ * @brief Get the current state of the robot from hardware interfaces and update internal variables
+ * @param initialize If set to true, initialize the exponential moving average filter with the current state
+ */
+ void updateCurrentState(bool initialize = false);
+
+ /**
+ * @brief Reads the target pose in realtime loop from the buffer and parses it
+ */
+ void parse_target_pose_();
+
+ /**
+ * @brief Reads the target joint in realtime loop from the buffer and parses it
+ */
+ void parse_target_joint_();
+
+ /**
+ * @brief Reads the target wrench in realtime loop from the buffer and parses it
+ */
+ void parse_target_wrench_();
+
+ /**
+ * @brief Reads the target impedance stiffness in realtime loop from the buffer and parses it
+ */
+ void parse_target_stiffness_();
+
+ /**
+ * @brief Reads the F/T sensor data from the realtime buffer and updates the internal wrench
+ */
+ void parse_ft_sensor_();
+
+ /**
+ * @brief Reads the target admittance stiffness from the realtime buffer
+ */
+ void parse_target_adm_stiffness_();
+
+ // ---- Flags for new data ----
+ bool new_target_pose_;
+ bool new_target_joint_;
+ bool new_target_wrench_;
+ bool new_target_stiffness_ = false;
+ bool use_topic_stiffness_ = false;
+ bool new_ft_sensor_ = false;
+ bool new_target_adm_stiffness_ = false;
+ bool use_topic_adm_stiffness_ = false;
+
+ // ---- Realtime buffers (same as CartesianController) ----
+ realtime_tools::RealtimeBuffer>
+ target_pose_buffer_;
+ realtime_tools::RealtimeBuffer>
+ target_joint_buffer_;
+ realtime_tools::RealtimeBuffer>
+ target_wrench_buffer_;
+ realtime_tools::RealtimeBuffer>
+ target_stiffness_buffer_;
+
+ // ---- Admittance-specific realtime buffers ----
+ realtime_tools::RealtimeBuffer>
+ ft_sensor_buffer_;
+ realtime_tools::RealtimeBuffer>
+ target_adm_stiffness_buffer_;
+
+ // ---- Target / desired state ----
+
+ /** @brief Target position in Cartesian space */
+ Eigen::Vector3d target_position_;
+ /** @brief Target orientation as quaternion */
+ Eigen::Quaterniond target_orientation_;
+ /** @brief Target wrench in task space */
+ Eigen::VectorXd target_wrench_;
+ /** @brief Desired target position in Cartesian space after applying filtering */
+ Eigen::Vector3d desired_position_;
+ /** @brief Desired target orientation as quaternion after applying filtering */
+ Eigen::Quaterniond desired_orientation_;
+
+ // ---- Parameters ----
+
+ /** @brief Parameter listener for dynamic parameter updates */
+ std::shared_ptr params_listener_;
+ /** @brief Current parameter values */
+ cartesian_admittance_controller::Params params_;
+
+ // ---- Pinocchio model ----
+
+ /** @brief Frame ID of the end effector in the robot model */
+ int end_effector_frame_id;
+ /** @brief Frame ID of the F/T sensor measurement frame in the robot model */
+ int ft_sensor_frame_id;
+ /** @brief Pinocchio robot model */
+ pinocchio::Model model_;
+ /** @brief Pinocchio data for computations */
+ pinocchio::Data data_;
+
+ // ---- Impedance outer loop matrices ----
+
+ /** @brief Cartesian stiffness matrix (6x6) */
+ Eigen::MatrixXd stiffness = Eigen::MatrixXd::Zero(6, 6);
+ /** @brief Cartesian damping matrix (6x6) */
+ Eigen::MatrixXd damping = Eigen::MatrixXd::Zero(6, 6);
+ /** @brief Topic-provided stiffness matrix (6x6 diagonal) */
+ Eigen::Matrix topic_stiffness_ = Eigen::Matrix::Zero();
+
+ /** @brief Nullspace stiffness matrix for posture control */
+ Eigen::MatrixXd nullspace_stiffness;
+ /** @brief Nullspace damping matrix for posture control */
+ Eigen::MatrixXd nullspace_damping;
+
+ // ---- Admittance inner loop state ----
+
+ /** @brief Internal MSD pose state */
+ pinocchio::SE3 inner_SE3_;
+ /** @brief Internal MSD velocity state (6D: linear + angular) */
+ Eigen::Vector inner_motion_;
+ /** @brief First-cycle initialization flag for admittance state */
+ bool admittance_initialized_;
+ /** @brief Admittance virtual mass matrix (6x6 diagonal) */
+ Eigen::Matrix adm_mass_;
+ /** @brief Inverse of admittance virtual mass matrix */
+ Eigen::Matrix adm_mass_inv_;
+ /** @brief Admittance stiffness matrix (6x6 diagonal) */
+ Eigen::Matrix adm_stiffness_;
+ /** @brief Admittance damping matrix (6x6 diagonal) */
+ Eigen::Matrix adm_damping_;
+ /** @brief F/T sensor reading (6D wrench) */
+ Eigen::VectorXd ft_wrench_;
+ /** @brief Topic-provided admittance stiffness (6x6 diagonal) */
+ Eigen::Matrix topic_adm_stiffness_;
+
+ // ---- Joint state vectors ----
+
+ /** @brief Current joint positions with dimension nv */
+ Eigen::VectorXd q;
+ /** @brief Current joint positions with dimension nq (may differ for continuous joints) */
+ Eigen::VectorXd q_pin;
+ /** @brief Current joint velocities */
+ Eigen::VectorXd dq;
+ /** @brief Reference joint positions for posture task */
+ Eigen::VectorXd q_ref;
+ /** @brief Reference joint velocities */
+ Eigen::VectorXd dq_ref;
+ /** @brief Target joint positions for posture task */
+ Eigen::VectorXd q_target;
+
+ /** @brief Previously computed torque */
+ Eigen::VectorXd tau_previous;
+
+ /** @brief Current end effector pose */
+ pinocchio::SE3 end_effector_pose;
+ /** @brief End effector Jacobian matrix */
+ pinocchio::Data::Matrix6x J;
+ /** @brief End effector Jacobian matrix pseudoinverse */
+ Eigen::MatrixXd J_pinv;
+ /** @brief Joint-space identity matrix */
+ Eigen::MatrixXd Id_nv;
+
+ /** @brief Friction parameters 1 of size nv */
+ Eigen::VectorXd fp1;
+ /** @brief Friction parameters 2 of size nv */
+ Eigen::VectorXd fp2;
+ /** @brief Friction parameters 3 of size nv */
+ Eigen::VectorXd fp3;
+
+ /** @brief Allowed type of joints */
+ const std::unordered_set> allowed_joint_types = {
+ "JointModelRX",
+ "JointModelRY",
+ "JointModelRZ",
+ "JointModelRevoluteUnaligned",
+ "JointModelRUBX",
+ "JointModelRUBY",
+ "JointModelRUBZ",
+ };
+ /** @brief Continuous joint types that should be considered separately */
+ const std::unordered_set> continous_joint_types = {
+ "JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"};
+
+ /** @brief Maximum allowed delta values for error clipping */
+ Eigen::VectorXd max_delta_ = Eigen::VectorXd::Zero(6);
+
+ /** @brief Nullspace projection matrix */
+ Eigen::MatrixXd nullspace_projection;
+
+ /** @brief Task space error vector (6x1) */
+ Eigen::VectorXd error = Eigen::VectorXd::Zero(6);
+
+ /** @brief Task space control torque */
+ Eigen::VectorXd tau_task;
+ /** @brief Joint limit avoidance torque */
+ Eigen::VectorXd tau_joint_limits;
+ /** @brief Secondary task torque before nullspace projection */
+ Eigen::VectorXd tau_secondary;
+ /** @brief Nullspace projected secondary task torque */
+ Eigen::VectorXd tau_nullspace;
+ /** @brief Friction compensation torque */
+ Eigen::VectorXd tau_friction;
+ /** @brief Coriolis compensation torque */
+ Eigen::VectorXd tau_coriolis;
+ /** @brief Gravity compensation torque */
+ Eigen::VectorXd tau_gravity;
+ /** @brief External wrench compensation torque */
+ Eigen::VectorXd tau_wrench;
+ /** @brief Final desired torque command */
+ Eigen::VectorXd tau_d;
+
+ /** @brief Inverse of the manipulator joint mass projected in Cartesian space (6x6) */
+ Eigen::Matrix Mx_inv = Eigen::Matrix::Zero();
+ /** @brief the manipulator joint mass projected in Cartesian space (6x6) */
+ Eigen::Matrix Mx = Eigen::Matrix::Zero();
+
+ /**
+ * @brief Log debug information based on parameter settings
+ * @param time Current time for throttling logs
+ */
+ void log_debug_info(const rclcpp::Time & time);
+
+ /**
+ * @brief Check publisher count for a specific topic
+ * @param topic_name Name of the topic to check
+ * @return true if publisher count is safe (<=1), false otherwise
+ */
+ bool check_topic_publisher_count(const std::string & topic_name);
+};
+
+} // namespace crisp_controllers
diff --git a/src/cartesian_admittance_controller.cpp b/src/cartesian_admittance_controller.cpp
new file mode 100644
index 0000000..2212408
--- /dev/null
+++ b/src/cartesian_admittance_controller.cpp
@@ -0,0 +1,1063 @@
+
+#include
+#include
+#include
+
+#include // NOLINT(build/include_order)
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "pinocchio/algorithm/model.hpp"
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include "crisp_controllers/utils/fiters.hpp"
+#include "crisp_controllers/utils/torque_rate_saturation.hpp"
+
+#include "crisp_controllers/utils/fiters.hpp"
+#include "crisp_controllers/utils/ros2_version.hpp"
+#include "crisp_controllers/utils/torque_rate_saturation.hpp"
+
+#if HAS_ROS2_CONTROL_INTROSPECTION
+#include
+#endif
+
+namespace crisp_controllers {
+
+controller_interface::InterfaceConfiguration
+CartesianAdmittanceController::command_interface_configuration() const {
+ controller_interface::InterfaceConfiguration config;
+ config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+ for (const auto & joint_name : params_.joints) {
+ config.names.push_back(joint_name + "/effort");
+ }
+ return config;
+}
+
+controller_interface::InterfaceConfiguration
+CartesianAdmittanceController::state_interface_configuration() const {
+ controller_interface::InterfaceConfiguration config;
+ config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+ for (const auto & joint_name : params_.joints) {
+ config.names.push_back(joint_name + "/position");
+ }
+ for (const auto & joint_name : params_.joints) {
+ config.names.push_back(joint_name + "/velocity");
+ }
+ return config;
+}
+
+controller_interface::return_type
+CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::Duration & period) {
+
+ // 1. Update current state information with EMA filtered values
+ updateCurrentState();
+
+ // 2. Check if new targets available (pose, joint, wrench -- same as CartesianController)
+ if (new_target_pose_) {
+ parse_target_pose_();
+ new_target_pose_ = false;
+ }
+ if (new_target_joint_) {
+ parse_target_joint_();
+ new_target_joint_ = false;
+ }
+ if (new_target_wrench_) {
+ parse_target_wrench_();
+ new_target_wrench_ = false;
+ }
+ if (new_target_stiffness_) {
+ parse_target_stiffness_();
+ new_target_stiffness_ = false;
+ setStiffnessAndDamping();
+ }
+
+ // 3. Parse F/T sensor data
+ if (new_ft_sensor_) {
+ parse_ft_sensor_();
+ new_ft_sensor_ = false;
+ }
+
+ // 4. Parse variable admittance stiffness
+ if (new_target_adm_stiffness_) {
+ parse_target_adm_stiffness_();
+ new_target_adm_stiffness_ = false;
+ setAdmittanceParameters();
+ }
+
+ // 5. Forward kinematics
+ pinocchio::forwardKinematics(model_, data_, q_pin, dq);
+ pinocchio::updateFramePlacements(model_, data_);
+ end_effector_pose = data_.oMf[end_effector_frame_id];
+
+ // 6. Initialize admittance state on first cycle
+ if (!admittance_initialized_) {
+ inner_SE3_ = end_effector_pose;
+ inner_motion_.setZero();
+ admittance_initialized_ = true;
+ }
+
+ // 7. Compute admittance target pose from desired (same EMA filtering as CartesianController)
+ desired_position_ =
+ exponential_moving_average(desired_position_, target_position_, params_.filter.target_pose);
+ desired_orientation_ =
+ target_orientation_.slerp(params_.filter.target_pose, desired_orientation_);
+
+ // 8. Compute SE3 error between inner state and desired pose (on manifold)
+ // Build desired SE3 from filtered position and orientation
+ pinocchio::SE3 desired_SE3;
+ desired_SE3.translation() = desired_position_;
+ desired_SE3.rotation() = desired_orientation_.toRotationMatrix();
+
+ // Position error: desired - inner
+ Eigen::Vector3d adm_pos_error = desired_SE3.translation() - inner_SE3_.translation();
+
+ // Orientation error: SO(3) logarithmic map (world frame)
+ Eigen::Vector3d adm_rot_error =
+ pinocchio::log3(desired_SE3.rotation() * inner_SE3_.rotation().transpose());
+
+ Eigen::Vector adm_error;
+ adm_error << adm_pos_error, adm_rot_error;
+
+ // 9. Transform F/T wrench from sensor (LOCAL) frame to LOCAL_WORLD_ALIGNED frame
+ pinocchio::SE3 ft_sensor_pose = data_.oMf[ft_sensor_frame_id];
+ pinocchio::Force ft_local(ft_wrench_.head(3), ft_wrench_.tail(3));
+ pinocchio::Force ft_world = pinocchio::Force::Zero();
+ pinocchio::changeReferenceFrame(
+ ft_sensor_pose, ft_local, pinocchio::LOCAL, pinocchio::LOCAL_WORLD_ALIGNED, ft_world);
+ Eigen::Vector ft_wrench_world = ft_world.toVector();
+
+ // 10. Admittance dynamics: accel = M_inv * (F_ext - D * vel + K * error)
+ // Spring term +K*(desired - inner) is a restoring force toward the desired pose
+ Eigen::Matrix K_adm = use_topic_adm_stiffness_ ? topic_adm_stiffness_ : adm_stiffness_;
+ Eigen::Vector adm_force = ft_wrench_world - adm_damping_ * inner_motion_ + K_adm * adm_error;
+ Eigen::Vector accel = adm_mass_inv_ * adm_force;
+
+ // 10. Semi-implicit Euler integration
+ double dt = period.seconds();
+ inner_motion_ += accel * dt;
+ // Update inner_SE3_ using exponential map
+ pinocchio::SE3 delta = pinocchio::exp6(pinocchio::Motion(inner_motion_ * dt));
+ inner_SE3_ = delta * inner_SE3_;
+
+ // 11. Use inner_SE3_ as the desired for impedance outer loop
+ // Compute impedance error (same as CartesianController but using inner_SE3_ as target)
+ if (params_.use_local_jacobian) {
+ error.head(3) = end_effector_pose.rotation().transpose() *
+ (inner_SE3_.translation() - end_effector_pose.translation());
+ error.tail(3) =
+ pinocchio::log3(end_effector_pose.rotation().transpose() * inner_SE3_.rotation());
+ } else {
+ error.head(3) = inner_SE3_.translation() - end_effector_pose.translation();
+ error.tail(3) =
+ pinocchio::log3(inner_SE3_.rotation() * end_effector_pose.rotation().transpose());
+ }
+
+ // 12. Error clipping (same as CartesianController)
+ if (params_.limit_error) {
+ max_delta_ << params_.task.error_clip.x, params_.task.error_clip.y, params_.task.error_clip.z,
+ params_.task.error_clip.rx, params_.task.error_clip.ry, params_.task.error_clip.rz;
+ error = error.cwiseMax(-max_delta_).cwiseMin(max_delta_);
+ }
+
+ // 13. Jacobian, pseudo-inverse, nullspace, Mx computation (same as CartesianController)
+ J.setZero();
+ auto reference_frame = params_.use_local_jacobian
+ ? pinocchio::ReferenceFrame::LOCAL
+ : pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED;
+ pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id, reference_frame, J);
+
+ J_pinv = pseudo_inverse(J, params_.nullspace.regularization);
+ if (params_.nullspace.projector_type == "dynamic" || params_.use_operational_space) {
+ pinocchio::computeMinverse(model_, data_, q_pin);
+ data_.Minv.triangularView() =
+ data_.Minv.transpose().triangularView();
+ Mx_inv = J * data_.Minv * J.transpose();
+ Mx = pseudo_inverse(Mx_inv, params_.operational_space_regularization);
+ }
+
+ if (params_.nullspace.projector_type == "dynamic") {
+ auto J_bar = data_.Minv * J.transpose() * Mx;
+ nullspace_projection = Id_nv - J.transpose() * J_bar.transpose();
+ } else if (params_.nullspace.projector_type == "kinematic") {
+ nullspace_projection = Id_nv - J_pinv * J;
+ } else if (params_.nullspace.projector_type == "none") {
+ nullspace_projection = Eigen::MatrixXd::Identity(model_.nv, model_.nv);
+ } else {
+ RCLCPP_ERROR_STREAM_ONCE(
+ get_node()->get_logger(),
+ "Unknown nullspace projector type: " << params_.nullspace.projector_type);
+ return controller_interface::return_type::ERROR;
+ }
+
+ // 14. Task torque (impedance): tau_task = J^T * (K * error - D * J * dq) or J^T * Mx * (...)
+ if (params_.use_operational_space) {
+ tau_task << J.transpose() * Mx * (stiffness * error - damping * (J * dq));
+ } else {
+ tau_task << J.transpose() * (stiffness * error - damping * (J * dq));
+ }
+
+ // 15. All compensation torques (same as CartesianController)
+ if (model_.nq != model_.nv) {
+ tau_joint_limits = Eigen::VectorXd::Zero(model_.nv);
+ } else {
+ tau_joint_limits = get_joint_limit_torque(
+ q,
+ model_.lowerPositionLimit,
+ model_.upperPositionLimit,
+ params_.joint_limit_repulsion.safe_range,
+ params_.joint_limit_repulsion.max_torque);
+ }
+
+ tau_secondary << nullspace_stiffness * (q_ref - q) + nullspace_damping * (dq_ref - dq);
+
+ tau_nullspace << nullspace_projection * tau_secondary;
+ tau_nullspace =
+ tau_nullspace.cwiseMin(params_.nullspace.max_tau).cwiseMax(-params_.nullspace.max_tau);
+
+ tau_friction =
+ params_.use_friction ? get_friction(dq, fp1, fp2, fp3) : Eigen::VectorXd::Zero(model_.nv);
+
+ if (params_.use_coriolis_compensation) {
+ pinocchio::computeAllTerms(model_, data_, q_pin, dq);
+ tau_coriolis = pinocchio::computeCoriolisMatrix(model_, data_, q_pin, dq) * dq;
+ } else {
+ tau_coriolis = Eigen::VectorXd::Zero(model_.nv);
+ }
+
+ tau_gravity = params_.use_gravity_compensation
+ ? pinocchio::computeGeneralizedGravity(model_, data_, q_pin)
+ : Eigen::VectorXd::Zero(model_.nv);
+
+ tau_wrench << J.transpose() * target_wrench_;
+
+ tau_d << tau_task + tau_nullspace + tau_friction + tau_coriolis + tau_gravity + tau_joint_limits +
+ tau_wrench;
+
+ // 16. Torque saturation and filtering
+ if (params_.limit_torques) {
+ tau_d = saturateTorqueRate(tau_d, tau_previous, params_.max_delta_tau);
+ }
+ tau_d = exponential_moving_average(tau_d, tau_previous, params_.filter.output_torque);
+
+ // 17. Command interfaces
+ if (!params_.stop_commands) {
+ for (size_t i = 0; i < params_.joints.size(); ++i) {
+#if ROS2_VERSION_ABOVE_HUMBLE
+ (void)command_interfaces_[i].set_value(tau_d[i]);
+#else
+ command_interfaces_[i].set_value(tau_d[i]);
+#endif
+ }
+ }
+
+ tau_previous = tau_d;
+
+ // 18. Refresh parameters
+ params_listener_->refresh_dynamic_parameters();
+ if (params_listener_->is_old(params_)) {
+ params_ = params_listener_->get_params();
+ setStiffnessAndDamping();
+ setAdmittanceParameters();
+ }
+
+ log_debug_info(time);
+
+ return controller_interface::return_type::OK;
+}
+
+CallbackReturn CartesianAdmittanceController::on_init() {
+ params_listener_ = std::make_shared(get_node());
+ params_listener_->refresh_dynamic_parameters();
+ params_ = params_listener_->get_params();
+
+ return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn
+CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) {
+ auto parameters_client =
+ std::make_shared(get_node(), "robot_state_publisher");
+ parameters_client->wait_for_service();
+
+ auto future = parameters_client->get_parameters({"robot_description"});
+ auto result = future.get();
+
+ std::string robot_description_;
+ if (!result.empty()) {
+ robot_description_ = result[0].value_to_string();
+ } else {
+ RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter.");
+ return CallbackReturn::ERROR;
+ }
+
+ pinocchio::Model raw_model_;
+ pinocchio::urdf::buildModelFromXML(robot_description_, raw_model_);
+
+ RCLCPP_INFO(get_node()->get_logger(), "Checking available joints in model:");
+ for (int joint_id = 0; joint_id < raw_model_.njoints; joint_id++) {
+ RCLCPP_INFO_STREAM(
+ get_node()->get_logger(),
+ "Joint " << joint_id << " with name " << raw_model_.names[joint_id] << " is of type "
+ << raw_model_.joints[joint_id].shortname());
+ }
+
+ // Check that the passed joints exist in the kinematic tree
+ for (auto & joint : params_.joints) {
+ if (!raw_model_.existJointName(joint)) {
+ RCLCPP_ERROR_STREAM(
+ get_node()->get_logger(),
+ "Failed to configure because " << joint
+ << " is not part of the kinematic tree but it "
+ "has been passed in the parameters.");
+ return CallbackReturn::ERROR;
+ }
+ }
+ RCLCPP_INFO(
+ get_node()->get_logger(),
+ "All joints passed in the parameters exist in the kinematic tree "
+ "of the URDF.");
+ RCLCPP_INFO_STREAM(
+ get_node()->get_logger(), "Removing the rest of the joints that are not used: ");
+ // Fix all joints that are not referenced in the tree
+ std::vector list_of_joints_to_lock_by_id;
+ for (auto & joint : raw_model_.names) {
+ if (
+ std::find(params_.joints.begin(), params_.joints.end(), joint) == params_.joints.end() &&
+ joint != "universe") {
+ RCLCPP_INFO_STREAM(
+ get_node()->get_logger(), "Joint " << joint << " is not used, removing it from the model.");
+ list_of_joints_to_lock_by_id.push_back(raw_model_.getJointId(joint));
+ }
+ }
+
+ Eigen::VectorXd q_locked = Eigen::VectorXd::Zero(raw_model_.nq);
+ model_ = pinocchio::buildReducedModel(raw_model_, list_of_joints_to_lock_by_id, q_locked);
+ data_ = pinocchio::Data(model_);
+
+ for (int joint_id = 0; joint_id < model_.njoints; joint_id++) {
+ if (model_.names[joint_id] == "universe") {
+ continue;
+ }
+ if (!allowed_joint_types.count(model_.joints[joint_id].shortname())) {
+ RCLCPP_ERROR_STREAM(
+ get_node()->get_logger(),
+ "Joint type " << model_.joints[joint_id].shortname() << " is unsupported ("
+ << model_.names[joint_id]
+ << "), only revolute/continous like joints can be used.");
+ return CallbackReturn::ERROR;
+ }
+ }
+
+ // Preallocate the matrices and vectors that will be used in the control loop
+ end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
+ if (params_.ft_sensor.frame.empty()) {
+ ft_sensor_frame_id = end_effector_frame_id;
+ RCLCPP_WARN(get_node()->get_logger(),
+ "ft_sensor.frame is not set, using end_effector_frame for F/T wrench transformation. "
+ "Set ft_sensor.frame to the actual sensor measurement frame for correct results.");
+ } else {
+ 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);
+ }
+ q = Eigen::VectorXd::Zero(model_.nv);
+ q_pin = Eigen::VectorXd::Zero(model_.nq);
+ dq = Eigen::VectorXd::Zero(model_.nv);
+ q_ref = Eigen::VectorXd::Zero(model_.nv);
+ q_target = Eigen::VectorXd::Zero(model_.nv);
+ dq_ref = Eigen::VectorXd::Zero(model_.nv);
+ tau_previous = Eigen::VectorXd::Zero(model_.nv);
+ J = Eigen::MatrixXd::Zero(6, model_.nv);
+ J_pinv = Eigen::MatrixXd::Zero(model_.nv, 6);
+ Id_nv = Eigen::MatrixXd::Identity(model_.nv, model_.nv);
+
+ // Map the friction parameters to Eigen vectors
+ fp1 = Eigen::Map(params_.friction.fp1.data(), model_.nv);
+ fp2 = Eigen::Map(params_.friction.fp2.data(), model_.nv);
+ fp3 = Eigen::Map(params_.friction.fp3.data(), model_.nv);
+
+ nullspace_stiffness = Eigen::MatrixXd::Zero(model_.nv, model_.nv);
+ nullspace_damping = Eigen::MatrixXd::Zero(model_.nv, model_.nv);
+
+ setStiffnessAndDamping();
+ setAdmittanceParameters();
+
+ new_target_pose_ = false;
+ new_target_joint_ = false;
+ new_target_wrench_ = false;
+ new_target_stiffness_ = false;
+ use_topic_stiffness_ = false;
+ new_ft_sensor_ = false;
+ new_target_adm_stiffness_ = false;
+ use_topic_adm_stiffness_ = false;
+ admittance_initialized_ = false;
+
+ multiple_publishers_detected_ = false;
+ max_allowed_publishers_ = 1;
+
+ // Initialize admittance inner loop state
+ inner_SE3_ = pinocchio::SE3::Identity();
+ inner_motion_.setZero();
+ ft_wrench_ = Eigen::VectorXd::Zero(6);
+ topic_adm_stiffness_ = Eigen::Matrix::Zero();
+
+ // --- Subscriptions (same as CartesianController) ---
+ auto target_pose_callback =
+ [this](const std::shared_ptr msg) -> void {
+ if (!check_topic_publisher_count("target_pose")) {
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "Ignoring target_pose message due to multiple publishers detected!");
+ return;
+ }
+ target_pose_buffer_.writeFromNonRT(msg);
+ new_target_pose_ = true;
+ };
+
+ auto target_joint_callback =
+ [this](const std::shared_ptr msg) -> void {
+ if (!check_topic_publisher_count("target_joint")) {
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "Ignoring target_joint message due to multiple publishers detected!");
+ return;
+ }
+ target_joint_buffer_.writeFromNonRT(msg);
+ new_target_joint_ = true;
+ };
+
+ auto target_wrench_callback =
+ [this](const std::shared_ptr msg) -> void {
+ if (!check_topic_publisher_count("target_wrench")) {
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "Ignoring target_wrench message due to multiple publishers detected!");
+ return;
+ }
+ target_wrench_buffer_.writeFromNonRT(msg);
+ new_target_wrench_ = true;
+ };
+
+ pose_sub_ = get_node()->create_subscription(
+ "target_pose", rclcpp::QoS(1), target_pose_callback);
+
+ joint_sub_ = get_node()->create_subscription(
+ "target_joint", rclcpp::QoS(1), target_joint_callback);
+
+ wrench_sub_ = get_node()->create_subscription(
+ "target_wrench", rclcpp::QoS(1), target_wrench_callback);
+
+ if (params_.variable_stiffness.enabled) {
+ auto target_stiffness_callback =
+ [this](const std::shared_ptr msg) -> void {
+ if (!check_topic_publisher_count(params_.variable_stiffness.topic)) {
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "Ignoring target_stiffness message due to multiple publishers detected!");
+ return;
+ }
+ target_stiffness_buffer_.writeFromNonRT(msg);
+ new_target_stiffness_ = true;
+ };
+
+ stiffness_sub_ = get_node()->create_subscription(
+ params_.variable_stiffness.topic, rclcpp::QoS(1), target_stiffness_callback);
+
+ RCLCPP_INFO(get_node()->get_logger(), "Variable impedance stiffness enabled on topic: %s",
+ params_.variable_stiffness.topic.c_str());
+ }
+
+ // --- Admittance-specific subscriptions ---
+
+ // F/T sensor subscription
+ auto ft_sensor_callback =
+ [this](const std::shared_ptr msg) -> void {
+ ft_sensor_buffer_.writeFromNonRT(msg);
+ new_ft_sensor_ = true;
+ };
+
+ ft_sensor_sub_ = get_node()->create_subscription(
+ params_.ft_sensor.topic, rclcpp::SensorDataQoS(), ft_sensor_callback);
+
+ RCLCPP_INFO(get_node()->get_logger(), "F/T sensor subscription on topic: %s",
+ params_.ft_sensor.topic.c_str());
+
+ // Variable admittance stiffness subscription
+ if (params_.variable_admittance_stiffness.enabled) {
+ auto target_adm_stiffness_callback =
+ [this](const std::shared_ptr msg) -> void {
+ target_adm_stiffness_buffer_.writeFromNonRT(msg);
+ new_target_adm_stiffness_ = true;
+ };
+
+ adm_stiffness_sub_ = get_node()->create_subscription(
+ params_.variable_admittance_stiffness.topic, rclcpp::QoS(1), target_adm_stiffness_callback);
+
+ RCLCPP_INFO(get_node()->get_logger(), "Variable admittance stiffness enabled on topic: %s",
+ params_.variable_admittance_stiffness.topic.c_str());
+ }
+
+ // Initialize all control vectors with appropriate dimensions
+ tau_task = Eigen::VectorXd::Zero(model_.nv);
+ tau_joint_limits = Eigen::VectorXd::Zero(model_.nv);
+ tau_secondary = Eigen::VectorXd::Zero(model_.nv);
+ tau_nullspace = Eigen::VectorXd::Zero(model_.nv);
+ tau_friction = Eigen::VectorXd::Zero(model_.nv);
+ tau_coriolis = Eigen::VectorXd::Zero(model_.nv);
+ tau_gravity = Eigen::VectorXd::Zero(model_.nv);
+ tau_wrench = Eigen::VectorXd::Zero(model_.nv);
+ tau_d = Eigen::VectorXd::Zero(model_.nv);
+
+ // Initialize target state vectors
+ target_position_ = Eigen::Vector3d::Zero();
+ target_orientation_ = Eigen::Quaterniond::Identity();
+ target_wrench_ = Eigen::VectorXd::Zero(6);
+ desired_position_ = Eigen::Vector3d::Zero();
+ desired_orientation_ = Eigen::Quaterniond::Identity();
+
+ // Initialize error vector
+ error = Eigen::VectorXd::Zero(6);
+ max_delta_ = Eigen::VectorXd::Zero(6);
+
+ // Initialize nullspace projection matrix
+ nullspace_projection = Eigen::MatrixXd::Identity(model_.nv, model_.nv);
+
+#if HAS_ROS2_CONTROL_INTROSPECTION
+ if (params_.enable_introspection) {
+ RCLCPP_INFO(get_node()->get_logger(), "Enabling ROS2 Control Introspection for debugging.");
+ this->enable_introspection(true);
+ for (int i = 0; i < tau_task.size(); ++i) {
+ REGISTER_ROS2_CONTROL_INTROSPECTION("tau_task_" + std::to_string(i), &tau_task[i]);
+ REGISTER_ROS2_CONTROL_INTROSPECTION("tau_desired_" + std::to_string(i), &tau_d[i]);
+ }
+ for (int i = 0; i < error.size(); ++i) {
+ REGISTER_ROS2_CONTROL_INTROSPECTION("error_" + std::to_string(i), &error[i]);
+ }
+ REGISTER_ROS2_CONTROL_INTROSPECTION("target_position_x", &target_position_[0]);
+ REGISTER_ROS2_CONTROL_INTROSPECTION("target_position_y", &target_position_[1]);
+ REGISTER_ROS2_CONTROL_INTROSPECTION("target_position_z", &target_position_[2]);
+ for (int i = 0; i < target_orientation_.coeffs().size(); ++i) {
+ REGISTER_ROS2_CONTROL_INTROSPECTION(
+ "target_orientation_" + std::to_string(i), &target_orientation_.coeffs()[i]);
+ }
+ for (int i = 0; i < q_target.size(); ++i) {
+ REGISTER_ROS2_CONTROL_INTROSPECTION("q_target_" + std::to_string(i), &q_target[i]);
+ REGISTER_ROS2_CONTROL_INTROSPECTION("q_ref_" + std::to_string(i), &q_ref[i]);
+ }
+ } else {
+ RCLCPP_INFO(get_node()->get_logger(), "ROS2 Control Introspection is disabled.");
+ }
+#endif
+
+ RCLCPP_INFO(get_node()->get_logger(), "Admittance controller configured successfully.");
+
+ return CallbackReturn::SUCCESS;
+}
+
+void CartesianAdmittanceController::setStiffnessAndDamping() {
+ if (use_topic_stiffness_) {
+ stiffness = topic_stiffness_;
+ } else {
+ stiffness.setZero();
+ stiffness.diagonal() << params_.task.k_pos_x, params_.task.k_pos_y, params_.task.k_pos_z,
+ params_.task.k_rot_x, params_.task.k_rot_y, params_.task.k_rot_z;
+ }
+
+ // Clamp stiffness to [0, max_stiffness]
+ const double max_k_trans = params_.variable_max_impedance_stiffness.translational;
+ const double max_k_rot = params_.variable_max_impedance_stiffness.rotational;
+ for (int i = 0; i < 3; ++i) {
+ if (stiffness(i, i) < 0.0 || stiffness(i, i) > max_k_trans) {
+ RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Translational stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, stiffness(i, i), max_k_trans);
+ stiffness(i, i) = std::clamp(stiffness(i, i), 0.0, max_k_trans);
+ }
+ }
+ for (int i = 3; i < 6; ++i) {
+ if (stiffness(i, i) < 0.0 || stiffness(i, i) > max_k_rot) {
+ RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Rotational stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, stiffness(i, i), max_k_rot);
+ stiffness(i, i) = std::clamp(stiffness(i, i), 0.0, max_k_rot);
+ }
+ }
+
+ damping.setZero();
+ // 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));
+
+ nullspace_stiffness.setZero();
+ nullspace_damping.setZero();
+
+ auto weights = Eigen::VectorXd(model_.nv);
+ for (size_t i = 0; i < params_.joints.size(); ++i) {
+ weights[i] = params_.nullspace.weights.joints_map.at(params_.joints.at(i)).value;
+ }
+ nullspace_stiffness.diagonal() << params_.nullspace.stiffness * weights;
+ nullspace_damping.diagonal() << 2.0 * nullspace_stiffness.diagonal().cwiseSqrt();
+
+ if (params_.nullspace.damping >= 0.0) {
+ nullspace_damping.diagonal() = params_.nullspace.damping * weights;
+ } else {
+ nullspace_damping.diagonal() = 2.0 * nullspace_stiffness.diagonal().cwiseSqrt();
+ }
+}
+
+void CartesianAdmittanceController::setAdmittanceParameters() {
+ adm_mass_.setZero();
+ adm_mass_.diagonal() << params_.admittance.mass_x, params_.admittance.mass_y,
+ params_.admittance.mass_z, params_.admittance.mass_rx, params_.admittance.mass_ry,
+ params_.admittance.mass_rz;
+ adm_mass_inv_ = adm_mass_.inverse();
+
+ if (use_topic_adm_stiffness_) {
+ adm_stiffness_ = topic_adm_stiffness_;
+ } else {
+ adm_stiffness_.setZero();
+ adm_stiffness_.diagonal() << params_.admittance.stiffness_x, params_.admittance.stiffness_y,
+ params_.admittance.stiffness_z, params_.admittance.stiffness_rx,
+ params_.admittance.stiffness_ry, params_.admittance.stiffness_rz;
+ }
+
+ // Clamp admittance stiffness to [0, max_admittance_stiffness]
+ const double max_ak_trans = params_.variable_max_admittance_stiffness.translational;
+ const double max_ak_rot = params_.variable_max_admittance_stiffness.rotational;
+ for (int i = 0; i < 3; ++i) {
+ if (adm_stiffness_(i, i) < 0.0 || adm_stiffness_(i, i) > max_ak_trans) {
+ RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Admittance translational stiffness[%d]=%.1f out of [0, %.1f], clamping.",
+ i, adm_stiffness_(i, i), max_ak_trans);
+ adm_stiffness_(i, i) = std::clamp(adm_stiffness_(i, i), 0.0, max_ak_trans);
+ }
+ }
+ for (int i = 3; i < 6; ++i) {
+ if (adm_stiffness_(i, i) < 0.0 || adm_stiffness_(i, i) > max_ak_rot) {
+ RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Admittance rotational stiffness[%d]=%.1f out of [0, %.1f], clamping.",
+ i, adm_stiffness_(i, i), max_ak_rot);
+ adm_stiffness_(i, i) = std::clamp(adm_stiffness_(i, i), 0.0, max_ak_rot);
+ }
+ }
+
+ adm_damping_.setZero();
+ adm_damping_.diagonal() << params_.admittance.damping_x, params_.admittance.damping_y,
+ params_.admittance.damping_z, params_.admittance.damping_rx, params_.admittance.damping_ry,
+ params_.admittance.damping_rz;
+}
+
+void CartesianAdmittanceController::updateCurrentState(bool initialize) {
+ auto num_joints = params_.joints.size();
+ for (size_t i = 0; i < num_joints; i++) {
+ auto joint_name = params_.joints[i];
+ auto joint_id = model_.getJointId(joint_name);
+ auto joint = model_.joints[joint_id];
+
+#if ROS2_VERSION_ABOVE_HUMBLE
+ double q_meas = state_interfaces_[i].get_optional().value_or(q[i]);
+ double dq_meas = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]);
+#else
+ double q_meas = state_interfaces_[i].get_value();
+ double dq_meas = state_interfaces_[num_joints + i].get_value();
+#endif
+
+ q_ref[i] = initialize
+ ? q_meas
+ : exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref);
+
+ q[i] = initialize
+ ? q_meas
+ : exponential_moving_average(q[i], q_meas, params_.filter.q);
+
+ if (continous_joint_types.count(joint.shortname())) {
+ q_pin[joint.idx_q()] = std::cos(q[i]);
+ q_pin[joint.idx_q() + 1] = std::sin(q[i]);
+ } else {
+ q_pin[joint.idx_q()] = q[i];
+ }
+
+ dq[i] = initialize
+ ? dq_meas
+ : exponential_moving_average(dq[i], dq_meas, params_.filter.dq);
+
+ q_target[i] = initialize ? q_meas : q_target[i];
+ }
+}
+
+CallbackReturn
+CartesianAdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
+
+ // Update the current state with initial measurements (no EMA filtering)
+ updateCurrentState(true);
+
+ pinocchio::forwardKinematics(model_, data_, q_pin, dq);
+ pinocchio::updateFramePlacements(model_, data_);
+
+ end_effector_pose = data_.oMf[end_effector_frame_id];
+
+ target_position_ = end_effector_pose.translation();
+ target_orientation_ = Eigen::Quaterniond(end_effector_pose.rotation());
+ desired_position_ = target_position_;
+ desired_orientation_ = target_orientation_;
+
+ // Reset admittance state so it reinitializes on first update cycle
+ admittance_initialized_ = false;
+ inner_motion_.setZero();
+ ft_wrench_.setZero();
+
+ RCLCPP_INFO(get_node()->get_logger(), "Admittance controller activated.");
+ return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+CartesianAdmittanceController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) {
+ return CallbackReturn::SUCCESS;
+}
+
+void CartesianAdmittanceController::parse_target_pose_() {
+ auto msg = *target_pose_buffer_.readFromRT();
+ target_position_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
+ target_orientation_ = Eigen::Quaterniond(
+ msg->pose.orientation.w,
+ msg->pose.orientation.x,
+ msg->pose.orientation.y,
+ msg->pose.orientation.z);
+}
+
+void CartesianAdmittanceController::parse_target_joint_() {
+ auto msg = *target_joint_buffer_.readFromRT();
+ size_t num_joints = static_cast(model_.nv);
+ if (msg->position.size()) {
+ for (size_t i = 0; i < std::min(msg->position.size(), num_joints); ++i) {
+ q_target[i] = msg->position[i];
+ }
+ }
+ if (msg->velocity.size()) {
+ for (size_t i = 0; i < std::min(msg->velocity.size(), num_joints); ++i) {
+ dq_ref[i] = msg->velocity[i];
+ }
+ }
+}
+
+void CartesianAdmittanceController::parse_target_wrench_() {
+ auto msg = *target_wrench_buffer_.readFromRT();
+ target_wrench_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z,
+ msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z;
+}
+
+void CartesianAdmittanceController::parse_target_stiffness_() {
+ auto msg = *target_stiffness_buffer_.readFromRT();
+ if (msg->data.size() != 6) {
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "Variable stiffness message must have exactly 6 elements, got %zu. Ignoring.",
+ msg->data.size());
+ return;
+ }
+ const double max_k_trans = params_.variable_max_impedance_stiffness.translational;
+ const double max_k_rot = params_.variable_max_impedance_stiffness.rotational;
+ std::array vals = {msg->data[0], msg->data[1], msg->data[2],
+ msg->data[3], msg->data[4], msg->data[5]};
+ for (int i = 0; i < 3; ++i) {
+ if (vals[i] < 0.0 || vals[i] > max_k_trans) {
+ RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Topic impedance stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_trans);
+ vals[i] = std::clamp(vals[i], 0.0, max_k_trans);
+ }
+ }
+ for (int i = 3; i < 6; ++i) {
+ if (vals[i] < 0.0 || vals[i] > max_k_rot) {
+ RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Topic impedance stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_rot);
+ vals[i] = std::clamp(vals[i], 0.0, max_k_rot);
+ }
+ }
+ topic_stiffness_.setZero();
+ topic_stiffness_.diagonal() << vals[0], vals[1], vals[2], vals[3], vals[4], vals[5];
+ use_topic_stiffness_ = true;
+ RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Variable impedance stiffness received: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
+ vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]);
+}
+
+void CartesianAdmittanceController::parse_ft_sensor_() {
+ auto msg = *ft_sensor_buffer_.readFromRT();
+ ft_wrench_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z,
+ msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z;
+}
+
+void CartesianAdmittanceController::parse_target_adm_stiffness_() {
+ auto msg = *target_adm_stiffness_buffer_.readFromRT();
+ if (msg->data.size() != 6) {
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "Variable admittance stiffness must have 6 elements, got %zu. Ignoring.",
+ msg->data.size());
+ return;
+ }
+ const double max_ak_trans = params_.variable_max_admittance_stiffness.translational;
+ const double max_ak_rot = params_.variable_max_admittance_stiffness.rotational;
+ std::array avals = {msg->data[0], msg->data[1], msg->data[2],
+ msg->data[3], msg->data[4], msg->data[5]};
+ for (int i = 0; i < 3; ++i) {
+ if (avals[i] < 0.0 || avals[i] > max_ak_trans) {
+ RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Topic admittance stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, avals[i], max_ak_trans);
+ avals[i] = std::clamp(avals[i], 0.0, max_ak_trans);
+ }
+ }
+ for (int i = 3; i < 6; ++i) {
+ if (avals[i] < 0.0 || avals[i] > max_ak_rot) {
+ RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Topic admittance stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, avals[i], max_ak_rot);
+ avals[i] = std::clamp(avals[i], 0.0, max_ak_rot);
+ }
+ }
+ topic_adm_stiffness_.setZero();
+ topic_adm_stiffness_.diagonal() << avals[0], avals[1], avals[2], avals[3], avals[4], avals[5];
+ use_topic_adm_stiffness_ = true;
+ RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ "Variable admittance stiffness received: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
+ avals[0], avals[1], avals[2], avals[3], avals[4], avals[5]);
+}
+
+void CartesianAdmittanceController::log_debug_info(const rclcpp::Time & time) {
+ if (!params_.log.enabled) {
+ return;
+ }
+ if (params_.log.robot_state) {
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "nq: " << model_.nq << ", nv: " << model_.nv);
+
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "end_effector_pos" << end_effector_pose.translation());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "q: " << q.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "q_pin: " << q_pin.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "dq: " << dq.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "J: " << J);
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000,
+ "inner_SE3 pos: " << inner_SE3_.translation().transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000,
+ "inner_motion: " << inner_motion_.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000,
+ "ft_wrench: " << ft_wrench_.transpose());
+ }
+
+ if (params_.log.control_values) {
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "error: " << error.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "max_delta: " << max_delta_.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "q_ref: " << q_ref.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "dq_ref: " << dq_ref.transpose());
+ }
+
+ if (params_.log.controller_parameters) {
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "stiffness: " << stiffness);
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "damping: " << damping);
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "nullspace_stiffness: " << nullspace_stiffness);
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "nullspace_damping: " << nullspace_damping);
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000,
+ "adm_mass: " << adm_mass_.diagonal().transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000,
+ "adm_stiffness: " << adm_stiffness_.diagonal().transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000,
+ "adm_damping: " << adm_damping_.diagonal().transpose());
+ }
+
+ if (params_.log.limits) {
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "joint_limits: " << model_.lowerPositionLimit.transpose() << ", "
+ << model_.upperPositionLimit.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "velocity_limits: " << model_.velocityLimit);
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "effort_limits: " << model_.effortLimit);
+ }
+
+ if (params_.log.computed_torques) {
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "tau_task: " << tau_task.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "tau_joint_limits: " << tau_joint_limits.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "tau_nullspace: " << tau_nullspace.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "tau_friction: " << tau_friction.transpose());
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "tau_coriolis: " << tau_coriolis.transpose());
+ }
+
+ if (params_.log.dynamic_params) {
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "M: " << data_.M);
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 1000, "Minv: " << data_.Minv);
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 1000,
+ "nullspace projector: " << nullspace_projection);
+ }
+
+ if (params_.log.timing) {
+ auto t_end = get_node()->get_clock()->now();
+ RCLCPP_INFO_STREAM_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 2000,
+ "Control loop needed: " << (t_end.nanoseconds() - time.nanoseconds()) * 1e-6 << " ms");
+ }
+}
+
+bool CartesianAdmittanceController::check_topic_publisher_count(const std::string & topic_name) {
+ std::vector topic_info;
+ try {
+ topic_info = get_node()->get_publishers_info_by_topic(topic_name);
+ } catch (const std::exception & e) {
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 5000,
+ "Failed to get publisher info for topic '%s': %s", topic_name.c_str(), e.what());
+ return true; // Allow message through if check fails
+ }
+ size_t publisher_count = topic_info.size();
+
+ if (publisher_count > max_allowed_publishers_) {
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 2000,
+ "Topic '%s' has %zu publishers (expected max: %zu). Multiple command sources detected!",
+ topic_name.c_str(),
+ publisher_count,
+ max_allowed_publishers_);
+
+ if (!multiple_publishers_detected_) {
+ RCLCPP_ERROR(
+ get_node()->get_logger(),
+ "SAFETY WARNING: Multiple publishers detected on topic '%s'! "
+ "Ignoring commands from this topic to prevent conflicting control signals.",
+ topic_name.c_str());
+ multiple_publishers_detected_ = true;
+ }
+ return false;
+ }
+
+ if (multiple_publishers_detected_ && publisher_count <= max_allowed_publishers_) {
+ RCLCPP_INFO(
+ get_node()->get_logger(),
+ "Publisher conflict resolved on topic '%s'. Resuming message processing.",
+ topic_name.c_str());
+ multiple_publishers_detected_ = false;
+ }
+
+ return true;
+}
+
+} // namespace crisp_controllers
+#include "pluginlib/class_list_macros.hpp"
+// NOLINTNEXTLINE
+PLUGINLIB_EXPORT_CLASS(
+ crisp_controllers::CartesianAdmittanceController, controller_interface::ControllerInterface)
diff --git a/src/cartesian_admittance_controller.yaml b/src/cartesian_admittance_controller.yaml
new file mode 100644
index 0000000..4636a55
--- /dev/null
+++ b/src/cartesian_admittance_controller.yaml
@@ -0,0 +1,487 @@
+cartesian_admittance_controller:
+ joints:
+ type: string_array
+ description: "Names of the joints"
+
+ end_effector_frame:
+ type: string
+ description: "Name of the end-effector frame"
+
+ base_frame:
+ type: string
+ default_value: ""
+ description: "Name of the base/root frame as reference for kinematics computations"
+
+ use_operational_space:
+ type: bool
+ default_value: False
+ description: "Whether we use operational space control or cartesian impedance control"
+
+ operational_space_regularization:
+ type: double
+ default_value: 0.01
+ description: "Regularization (damping) for Mx pseudo-inverse. Higher values improve stability near singularities but reduce accuracy. Valid range is 0.001-1.0; typical useful values are in the 0.01-0.5 range."
+ validation:
+ bounds<>: [0.001, 1.0]
+
+ task:
+ k_pos_x:
+ type: double
+ default_value: 500.0
+ description: "Stiffness in the x direction for the translation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ d_pos_x:
+ type: double
+ default_value: -1.0
+ description: "Damping in the x direction for the translation. If negative, computed as 2*sqrt(stiffness)"
+ validation:
+ bounds<>: [-1.0, 5000.0]
+ k_pos_y:
+ type: double
+ default_value: 500.0
+ description: "Stiffness in the y direction for the translation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ d_pos_y:
+ type: double
+ default_value: -1.0
+ description: "Damping in the y direction for the translation. If negative, computed as 2*sqrt(stiffness)"
+ validation:
+ bounds<>: [-1.0, 5000.0]
+ k_pos_z:
+ type: double
+ default_value: 500.0
+ description: "Stiffness in the z direction for the translation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ d_pos_z:
+ type: double
+ default_value: -1.0
+ description: "Damping in the z direction for the translation. If negative, computed as 2*sqrt(stiffness)"
+ validation:
+ bounds<>: [-1.0, 5000.0]
+ k_rot_x:
+ type: double
+ default_value: 30.0
+ description: "Stiffness in the x direction for the orientation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ d_rot_x:
+ type: double
+ default_value: -1.0
+ description: "Damping in the x direction for the orientation. If negative, computed as 2*sqrt(stiffness)"
+ validation:
+ bounds<>: [-1.0, 5000.0]
+ k_rot_y:
+ type: double
+ default_value: 30.0
+ description: "Stiffness in the y direction for the orientation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ d_rot_y:
+ type: double
+ default_value: -1.0
+ description: "Damping in the y direction for the orientation. If negative, computed as 2*sqrt(stiffness)"
+ validation:
+ bounds<>: [-1.0, 5000.0]
+ k_rot_z:
+ type: double
+ default_value: 30.0
+ description: "Stiffness in the z direction for the orientation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ d_rot_z:
+ type: double
+ default_value: -1.0
+ description: "Damping in the z direction for the orientation. If negative, computed as 2*sqrt(stiffness)"
+ validation:
+ bounds<>: [-1.0, 5000.0]
+ error_clip:
+ x:
+ type: double
+ description: "Max allowed error in x direction."
+ default_value: 0.1
+ y:
+ type: double
+ description: "Max allowed error in y direction."
+ default_value: 0.1
+ z:
+ type: double
+ description: "Max allowed error in z direction."
+ default_value: 0.1
+ rx:
+ type: double
+ description: "Max allowed error in the rotational x direction (local or global frame)."
+ default_value: 0.5
+ ry:
+ type: double
+ description: "Max allowed error in the rotational y direction (local or global frame)."
+ default_value: 0.5
+ rz:
+ type: double
+ description: "Max allowed error in the rotational z direction (local or global frame)."
+ default_value: 0.5
+
+ nullspace:
+ stiffness:
+ type: double
+ default_value: 1.0
+ description: "Stiffness of the nullspace controller."
+ validation:
+ bounds<>: [0.0, 500.0]
+ damping:
+ type: double
+ default_value: -1.0
+ description: "Damping of the nullspace controller. If negative, then 2 sqrt(stiffness) is used."
+ validation:
+ bounds<>: [-1.0, 500.0]
+ weights:
+ __map_joints:
+ value:
+ type: double
+ default_value: 1.0
+ description: "Weights for each joint for the nullspace parameters."
+ validation:
+ bounds<>: [0.0, 500.0]
+ regularization:
+ type: double
+ default_value: 0.000001
+ description: "Regularization value for the pseudo-inverse."
+ validation:
+ bounds<>: [0.000001, 0.0001]
+ projector_type:
+ type: string
+ default_value: "kinematic"
+ description: "Which type of nullspace projector should be used: kinematic, dynamic or none."
+ validation:
+ one_of<>: [["kinematic", "dynamic", "none"]]
+ max_tau:
+ type: double
+ default_value: 5
+ description: "Maximal torque value that can be applied in the nullspace. This is used to be sure that we do not interfere with the main task."
+
+ noise:
+ add_random_noise:
+ type: bool
+ default_value: false
+ description: "Whether or not we add random noise to the torques. This is just to test for stability."
+ amplitude:
+ type: double
+ default_value: 0.0
+ description: "Magnitude of the added noise, if added."
+ validation:
+ bounds<>: [0.0, 10.0]
+
+
+ use_friction:
+ type: bool
+ default_value: false
+ description: "Use friction compensation"
+
+ use_coriolis_compensation:
+ type: bool
+ default_value: false
+ description: "Use coriolis compensation"
+
+ use_gravity_compensation:
+ type: bool
+ default_value: false
+ description: "Add use_gravity_compensation. Double check that your robot is not already doing this behind the scenes (which is the case for Franka Robotics). In this case you should leave this parameter to false!"
+
+ use_local_jacobian:
+ type: bool
+ default_value: false
+ description: "If true, we use the local jacobian for computations, otherwise we use the world jacobian."
+
+ log:
+ enabled:
+ type: bool
+ default_value: false
+ description: "If set so to true, log some debugging statements."
+ robot_state:
+ type: bool
+ default_value: false
+ description: "If set so to true, log the robot state like the joint state and end effector position."
+ control_values:
+ type: bool
+ default_value: false
+ description: "If set so to true, log the control values like error to target."
+ limits:
+ type: bool
+ default_value: false
+ description: "If set so to true, log the limits of each joint."
+ controller_parameters:
+ type: bool
+ default_value: false
+ description: "If set so to true, log the controller parameters like the stiffness, damping..."
+ computed_torques:
+ type: bool
+ default_value: false
+ description: "If set so to true, log the individual computed torques."
+ dynamic_params:
+ type: bool
+ default_value: false
+ description: "If set so to true, log the dynamic_params like the mass matrix, operational mass matrix..."
+ timing:
+ type: bool
+ default_value: false
+ description: "If true, the time required by the loop will be displayed."
+
+ filter:
+ target_pose:
+ type: double
+ default_value: 0.1
+ description: "Amount of smoothing for the target pose when using the EMA. The EMA is applied at each step"
+ validation:
+ bounds<>: [0.1, 1.0]
+ q:
+ type: double
+ default_value: 0.5
+ description: "Amount of smoothing for the target pose when using the EMA."
+ validation:
+ bounds<>: [0.0, 1.0]
+ dq:
+ type: double
+ default_value: 0.5
+ description: "Amount of smoothing for the target pose when using the EMA."
+ validation:
+ bounds<>: [0.0, 1.0]
+ q_ref:
+ type: double
+ default_value: 0.5
+ description: "Amount of smoothing for the target pose when using the EMA."
+ validation:
+ bounds<>: [0.0, 1.0]
+
+ output_torque:
+ type: double
+ default_value: 0.5
+ description: "Amount of smoothing for the output torque when using the EMA."
+ validation:
+ bounds<>: [0.0, 1.0]
+
+ limit_error:
+ type: bool
+ default_value: true
+ description: "Limit\\clamp the error computed as the difference between the current pose and target pose."
+
+ limit_torques:
+ type: bool
+ default_value: true
+ description: "Limit torques"
+
+ joint_limit_repulsion:
+ enabled:
+ type: bool
+ default_value: true
+ description: "Enable joint limit repulsion torques that push joints away from their limits"
+ safe_range:
+ type: double
+ default_value: 0.1
+ description: "Distance from joint limit where repulsion starts [rad]"
+ validation:
+ bounds<>: [0.01, 1.0]
+ max_torque:
+ type: double
+ default_value: 5.0
+ description: "Maximum repulsion torque at joint limit [Nm]"
+ validation:
+ bounds<>: [0.0, 50.0]
+
+ max_delta_tau:
+ type: double
+ default_value: 0.5
+ description: "Max allowed change in torque per joint per control loop."
+
+ stop_commands:
+ type: bool
+ default_value: False
+ description: "If set to true, we will stop sending commands to the robot. This is mainly for debugging useful."
+
+ friction:
+ fp1:
+ type: double_array
+ default_value: [0.54615, 0.87224, 0.64068, 1.2794, 0.83904, 0.30301, 0.56489]
+ description: "Friction parameters part 1"
+ fp2:
+ type: double_array
+ default_value: [ 5.1181, 9.0657, 10.136, 5.5903, 8.3469, 17.133, 10.336]
+ description: "Friction parameters part 2"
+ fp3:
+ type: double_array
+ default_value: [ 0.039533, 0.025882, -0.04607, 0.036194, 0.026226, -0.021047, 0.0035526]
+ description: "Friction parameters part 3"
+
+ variable_max_impedance_stiffness:
+ translational:
+ type: double
+ default_value: 900.0
+ description: "Maximum allowed translational stiffness value. Stiffness values above this will be clamped."
+ validation:
+ bounds<>: [0.0, 5000.0]
+ rotational:
+ type: double
+ default_value: 60.0
+ description: "Maximum allowed rotational stiffness value. Stiffness values above this will be clamped."
+ validation:
+ bounds<>: [0.0, 100.0]
+
+ variable_stiffness:
+ enabled:
+ type: bool
+ default_value: false
+ description: "Enable receiving stiffness updates via a ROS2 topic. When enabled, stiffness values from the topic override parameter-based stiffness."
+ topic:
+ type: string
+ default_value: "target_stiffness"
+ description: "Topic name for receiving variable stiffness updates (std_msgs/Float64MultiArray with 6 elements: [k_pos_x, k_pos_y, k_pos_z, k_rot_x, k_rot_y, k_rot_z])"
+
+ enable_introspection:
+ type: bool
+ default_value: false
+ description: "Whether or not to enable introspection for this controller. This will allow us to visualize the internal state of the controller in plotjuggler using the introspection topics."
+
+ admittance:
+ mass_x:
+ type: double
+ default_value: 1.0
+ description: "Virtual mass for admittance in x translation"
+ validation:
+ bounds<>: [0.001, 100.0]
+ mass_y:
+ type: double
+ default_value: 1.0
+ description: "Virtual mass for admittance in y translation"
+ validation:
+ bounds<>: [0.001, 100.0]
+ mass_z:
+ type: double
+ default_value: 1.0
+ description: "Virtual mass for admittance in z translation"
+ validation:
+ bounds<>: [0.001, 100.0]
+ mass_rx:
+ type: double
+ default_value: 0.1
+ description: "Virtual mass for admittance in x rotation"
+ validation:
+ bounds<>: [0.001, 100.0]
+ mass_ry:
+ type: double
+ default_value: 0.1
+ description: "Virtual mass for admittance in y rotation"
+ validation:
+ bounds<>: [0.001, 100.0]
+ mass_rz:
+ type: double
+ default_value: 0.1
+ description: "Virtual mass for admittance in z rotation"
+ validation:
+ bounds<>: [0.001, 100.0]
+ stiffness_x:
+ type: double
+ default_value: 200.0
+ description: "Admittance stiffness in x translation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ stiffness_y:
+ type: double
+ default_value: 200.0
+ description: "Admittance stiffness in y translation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ stiffness_z:
+ type: double
+ default_value: 200.0
+ description: "Admittance stiffness in z translation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ stiffness_rx:
+ type: double
+ default_value: 10.0
+ description: "Admittance stiffness in x rotation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ stiffness_ry:
+ type: double
+ default_value: 10.0
+ description: "Admittance stiffness in y rotation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ stiffness_rz:
+ type: double
+ default_value: 10.0
+ description: "Admittance stiffness in z rotation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ damping_x:
+ type: double
+ default_value: 50.0
+ description: "Admittance damping in x translation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ damping_y:
+ type: double
+ default_value: 50.0
+ description: "Admittance damping in y translation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ damping_z:
+ type: double
+ default_value: 50.0
+ description: "Admittance damping in z translation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ damping_rx:
+ type: double
+ default_value: 5.0
+ description: "Admittance damping in x rotation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ damping_ry:
+ type: double
+ default_value: 5.0
+ description: "Admittance damping in y rotation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+ damping_rz:
+ type: double
+ default_value: 5.0
+ description: "Admittance damping in z rotation"
+ validation:
+ bounds<>: [0.0, 5000.0]
+
+ ft_sensor:
+ topic:
+ type: string
+ default_value: "ft_sensor_wrench"
+ description: "Topic name for external force/torque sensor (geometry_msgs/WrenchStamped)"
+ frame:
+ type: string
+ default_value: ""
+ description: "Name of the F/T sensor measurement frame in the URDF. If empty, defaults to end_effector_frame (legacy behavior)."
+
+ variable_max_admittance_stiffness:
+ translational:
+ type: double
+ default_value: 900.0
+ description: "Maximum allowed translational admittance stiffness value. Values above this will be clamped."
+ validation:
+ bounds<>: [0.0, 5000.0]
+ rotational:
+ type: double
+ default_value: 60.0
+ description: "Maximum allowed rotational admittance stiffness value. Values above this will be clamped."
+ validation:
+ bounds<>: [0.0, 100.0]
+
+ variable_admittance_stiffness:
+ enabled:
+ type: bool
+ default_value: false
+ description: "Enable receiving admittance stiffness updates via a ROS2 topic"
+ topic:
+ type: string
+ default_value: "target_admittance_stiffness"
+ description: "Topic name for variable admittance stiffness (Float64MultiArray, 6 elements)"
diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index 2315707..2799042 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -523,7 +523,7 @@ void CartesianController::updateCurrentState(bool initialize) {
auto joint_id = model_.getJointId(joint_name); // pinocchio joind id might be different
auto joint = model_.joints[joint_id];
-#if ROS2_VERSION_ABOVE_JAZZY
+#if ROS2_VERSION_ABOVE_HUMBLE
double q_meas = state_interfaces_[i].get_optional().value_or(q[i]);
double dq_meas = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]);
#else