diff --git a/src/cartesian_admittance_controller.cpp b/src/cartesian_admittance_controller.cpp index e58013d..4189759 100644 --- a/src/cartesian_admittance_controller.cpp +++ b/src/cartesian_admittance_controller.cpp @@ -875,6 +875,9 @@ 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; + if (params_.ft_sensor.wrench_convention == "applied_to_environment") { + ft_wrench_ = -ft_wrench_; + } } void CartesianAdmittanceController::parse_target_adm_stiffness_() { diff --git a/src/cartesian_admittance_controller.yaml b/src/cartesian_admittance_controller.yaml index 85a691b..3d4d1a4 100644 --- a/src/cartesian_admittance_controller.yaml +++ b/src/cartesian_admittance_controller.yaml @@ -480,6 +480,12 @@ cartesian_admittance_controller: 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)." + wrench_convention: + type: string + default_value: "applied_to_robot" + description: "Sign convention of the F/T sensor wrench. 'applied_to_robot': the sensor reports the wrench the environment applies to the robot (used as is). 'applied_to_environment': the sensor reports the wrench the robot applies to the environment (sign is flipped)." + validation: + one_of<>: [["applied_to_robot", "applied_to_environment"]] variable_max_admittance_stiffness: translational: