From 66c079b90e90b4c0f700062e26c7e769ee8070b3 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Fri, 23 Aug 2024 17:08:29 +0200 Subject: [PATCH 1/6] add effector mass management --- .../ros2_control/fd.r2c_hardware.xacro | 3 +- .../include/fd_hardware/fd_effort_hi.hpp | 3 ++ fd_hardware/src/fd_effort_hi.cpp | 29 ++++++++++++++++--- 3 files changed, 30 insertions(+), 5 deletions(-) diff --git a/fd_description/ros2_control/fd.r2c_hardware.xacro b/fd_description/ros2_control/fd.r2c_hardware.xacro index 40e269c..b800ea3 100644 --- a/fd_description/ros2_control/fd.r2c_hardware.xacro +++ b/fd_description/ros2_control/fd.r2c_hardware.xacro @@ -2,7 +2,7 @@ + params="robot_id plugin_name:='FDHapticInterface' interface_id:='-1' effector_mass:='-1.0' use_fake_hardware:='false' use_orientation:='false' orientation_is_actuated:='false' use_clutch:='false' emulate_button:='false'"> @@ -13,6 +13,7 @@ ${interface_id} ${emulate_button} fd_inertia + ${effector_mass} diff --git a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp index 4c8c90c..4d3e215 100644 --- a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp +++ b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp @@ -68,6 +68,9 @@ class FDEffortHardwareInterface : public hardware_interface::SystemInterface /// If true, the button will be emulated from clutch joint (for omega 6 / sigma 7, see SDK doc) bool emulate_button_ = false; + /// Interface mass in Kg, not used if negative + double effector_mass_ = -1.0; + std::string inertia_interface_name_; // Store the command for the robot diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp index f741dff..0c753ce 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/fd_hardware/src/fd_effort_hi.cpp @@ -176,6 +176,14 @@ CallbackReturn FDEffortHardwareInterface::on_init( inertia_interface_name_ = "fd_inertia"; } + auto it_interface_mass = info_.hardware_parameters.find("effector_mass"); + if (it_interface_mass != info_.hardware_parameters.end()) { + effector_mass_ = hardware_interface::stod(it_interface_id->second); + RCLCPP_INFO(LOGGER, "Interface mass parameter found: %lf Kg", effector_mass_); + } else { + effector_mass_ = -1.0; + } + // Contingency for emulated button // (commanded clutch force might be always left to NaN...) if (emulate_button_ && @@ -436,9 +444,9 @@ bool FDEffortHardwareInterface::connectToDevice() } // Retrieve the mass of the device - double effector_mass = 0.0; - if (dhdGetEffectorMass(&effector_mass, interface_ID_) == DHD_NO_ERROR) { - RCLCPP_INFO(LOGGER, "dhd : Effector Mass = %f (g)", effector_mass * 1000.0); + double current_effector_mass = 0.0; + if (dhdGetEffectorMass(¤t_effector_mass, interface_ID_) == DHD_NO_ERROR) { + RCLCPP_INFO(LOGGER, "dhd : Effector Mass = %f (g)", current_effector_mass * 1000.0); } else { RCLCPP_WARN(LOGGER, "dhd : Impossible to retrieve effector mass !"); } @@ -457,9 +465,22 @@ bool FDEffortHardwareInterface::connectToDevice() disconnectFromDevice(); return false; } + // Set effector mass + if (effector_mass_ > 0.0) { + RCLCPP_INFO( + LOGGER, + "dhd : Changing effector mass from %fto %f (g)!", + current_effector_mass * 1000.0, + effector_mass_ * 1000.0); + if (dhdSetEffectorMass(effector_mass_, interface_ID_) < DHD_NO_ERROR) { + RCLCPP_ERROR(LOGGER, "dhd : Failed to set effector mass!"); + disconnectFromDevice(); + return false; + } + } // Gravity compensation if (dhdSetGravityCompensation(DHD_ON, interface_ID_) < DHD_NO_ERROR) { - RCLCPP_WARN(LOGGER, "dhd : Could not enable the gravity compensation !"); + RCLCPP_ERROR(LOGGER, "dhd : Could not enable the gravity compensation !"); disconnectFromDevice(); return false; } else { From 55ef43f71b1fae6f0353395976604992643a6f7d Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Fri, 23 Aug 2024 18:07:55 +0200 Subject: [PATCH 2/6] fix effector mass parameter reading --- fd_hardware/src/fd_effort_hi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp index 0c753ce..106bc3d 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/fd_hardware/src/fd_effort_hi.cpp @@ -178,7 +178,7 @@ CallbackReturn FDEffortHardwareInterface::on_init( auto it_interface_mass = info_.hardware_parameters.find("effector_mass"); if (it_interface_mass != info_.hardware_parameters.end()) { - effector_mass_ = hardware_interface::stod(it_interface_id->second); + effector_mass_ = hardware_interface::stod(it_interface_mass->second); RCLCPP_INFO(LOGGER, "Interface mass parameter found: %lf Kg", effector_mass_); } else { effector_mass_ = -1.0; From 0c4946509bc248b6f921391f3a7998036cd75ccb Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Fri, 23 Aug 2024 18:08:33 +0200 Subject: [PATCH 3/6] add option to ignore orientation readings (i.e., exporting the interfaces, but setting them to zero) --- .../ros2_control/fd.r2c_hardware.xacro | 3 +- .../include/fd_hardware/fd_effort_hi.hpp | 3 ++ fd_hardware/src/fd_effort_hi.cpp | 41 ++++++++++++++++--- 3 files changed, 40 insertions(+), 7 deletions(-) diff --git a/fd_description/ros2_control/fd.r2c_hardware.xacro b/fd_description/ros2_control/fd.r2c_hardware.xacro index b800ea3..95faaa9 100644 --- a/fd_description/ros2_control/fd.r2c_hardware.xacro +++ b/fd_description/ros2_control/fd.r2c_hardware.xacro @@ -2,7 +2,7 @@ + params="robot_id plugin_name:='FDHapticInterface' interface_id:='-1' effector_mass:='-1.0' use_fake_hardware:='false' use_orientation:='false' orientation_is_actuated:='false' use_clutch:='false' emulate_button:='false' ignore_orientation_readings:='false'"> @@ -14,6 +14,7 @@ ${emulate_button} fd_inertia ${effector_mass} + ${ignore_orientation_readings} diff --git a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp index 4d3e215..f545075 100644 --- a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp +++ b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp @@ -68,6 +68,9 @@ class FDEffortHardwareInterface : public hardware_interface::SystemInterface /// If true, the button will be emulated from clutch joint (for omega 6 / sigma 7, see SDK doc) bool emulate_button_ = false; + // If true, the reading will emulate an omega 3 interface + bool ignore_orientation_ = false; + /// Interface mass in Kg, not used if negative double effector_mass_ = -1.0; diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp index 106bc3d..c8173a3 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/fd_hardware/src/fd_effort_hi.cpp @@ -184,6 +184,15 @@ CallbackReturn FDEffortHardwareInterface::on_init( effector_mass_ = -1.0; } + + auto it_ignore_orientation = info_.hardware_parameters.find("ignore_orientation_readings"); + if (it_ignore_orientation != info_.hardware_parameters.end()) { + ignore_orientation_ = hardware_interface::parse_bool(it_ignore_orientation->second); + } else { + ignore_orientation_ = false; + } + RCLCPP_INFO(LOGGER, "Ignoring orientation readings: %s", ignore_orientation_ ? "true" : "false"); + // Contingency for emulated button // (commanded clutch force might be always left to NaN...) if (emulate_button_ && @@ -290,12 +299,17 @@ hardware_interface::return_type FDEffortHardwareInterface::read( flag += dhdGetPosition( &hw_states_position_[0], &hw_states_position_[1], &hw_states_position_[2], interface_ID_); - if (dhdHasWrist(interface_ID_) && hw_states_position_.size() == 4) { + if (ignore_orientation_ && hw_states_position_.size() == 4) { // No orientation, skip! - } else if (dhdHasWrist(interface_ID_) && hw_states_position_.size() > 3) { + } else if (!ignore_orientation_ && hw_states_position_.size() > 3) { flag += dhdGetOrientationRad( &hw_states_position_[3], &hw_states_position_[4], &hw_states_position_[5], interface_ID_); + } else if (ignore_orientation_ && hw_states_position_.size() > 3) { + // Force orientation to zero + hw_states_position_[3] = 0.0; + hw_states_position_[4] = 0.0; + hw_states_position_[5] = 0.0; } // Get gripper angle @@ -311,10 +325,15 @@ hardware_interface::return_type FDEffortHardwareInterface::read( &hw_states_velocity_[2], interface_ID_); if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() == 4) { // No orientation, skip! - } else if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() > 3) { + } else if (!ignore_orientation_ && hw_states_velocity_.size() > 3) { flag += dhdGetAngularVelocityRad( &hw_states_velocity_[3], &hw_states_velocity_[4], &hw_states_velocity_[5], interface_ID_); + } else if (ignore_orientation_ && hw_states_velocity_.size() > 3) { + // Force angular velocity to zero + hw_states_velocity_[3] = 0.0; + hw_states_velocity_[4] = 0.0; + hw_states_velocity_[5] = 0.0; } // Get gripper angular velocity @@ -337,10 +356,15 @@ hardware_interface::return_type FDEffortHardwareInterface::read( // Get torques if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() == 4) { // No orientation, skip! - } else if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() > 3) { + } else if (!ignore_orientation_ && hw_states_effort_.size() > 3) { hw_states_effort_[3] = torque[0]; hw_states_effort_[4] = torque[1]; hw_states_effort_[5] = torque[2]; + } else if (ignore_orientation_ && hw_states_effort_.size() > 3) { + // Force angular forces to zero + hw_states_effort_[3] = 0.0; + hw_states_effort_[4] = 0.0; + hw_states_effort_[5] = 0.0; } // Get gripper force @@ -438,9 +462,9 @@ bool FDEffortHardwareInterface::connectToDevice() // Check if the device has 3 dof or more if (dhdHasWrist(interface_ID_)) { - RCLCPP_INFO(LOGGER, "dhd : Rotation enabled "); + RCLCPP_INFO(LOGGER, "dhd : Rotation supported"); } else { - RCLCPP_INFO(LOGGER, "dhd : Rotation disabled "); + RCLCPP_INFO(LOGGER, "dhd : Rotation not supported"); } // Retrieve the mass of the device @@ -510,6 +534,11 @@ bool FDEffortHardwareInterface::connectToDevice() return false; } + ignore_orientation_ &= !dhdHasWrist(interface_ID_); + if (ignore_orientation_) { + RCLCPP_INFO(LOGGER, "dhd : Orientation will be ignored !"); + } + // Sleep 100 ms dhdSleep(0.1); isConnected_ = true; From fe6fcec092d8c584e9a7d222b452d5dccd9dcaa1 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Wed, 28 Aug 2024 10:53:50 +0200 Subject: [PATCH 4/6] add "ignore_orientation_readings" to config --- fd_description/config/fd.config.xacro | 1 + 1 file changed, 1 insertion(+) diff --git a/fd_description/config/fd.config.xacro b/fd_description/config/fd.config.xacro index 452e931..312c8d1 100644 --- a/fd_description/config/fd.config.xacro +++ b/fd_description/config/fd.config.xacro @@ -22,6 +22,7 @@ use_fake_hardware="$(arg use_fake_hardware)" use_orientation="$(arg use_orientation)" orientation_is_actuated="$(arg use_orientation)" + ignore_orientation_readings="false" use_clutch="$(arg use_clutch)" emulate_button="true" /> From 211501b4af44b3ab30461359eaf53ed8fd34c296 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Wed, 28 Aug 2024 11:12:37 +0200 Subject: [PATCH 5/6] fix ignore_orientation_ --- fd_hardware/src/fd_effort_hi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp index c8173a3..70fd1b0 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/fd_hardware/src/fd_effort_hi.cpp @@ -534,7 +534,7 @@ bool FDEffortHardwareInterface::connectToDevice() return false; } - ignore_orientation_ &= !dhdHasWrist(interface_ID_); + ignore_orientation_ |= !dhdHasWrist(interface_ID_); if (ignore_orientation_) { RCLCPP_INFO(LOGGER, "dhd : Orientation will be ignored !"); } From 2f64985b025600a2e1720ee8cbdfc73c699e6996 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Wed, 28 Aug 2024 11:18:38 +0200 Subject: [PATCH 6/6] add default masses --- fd_description/config/fd.config.xacro | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/fd_description/config/fd.config.xacro b/fd_description/config/fd.config.xacro index 312c8d1..56da320 100644 --- a/fd_description/config/fd.config.xacro +++ b/fd_description/config/fd.config.xacro @@ -25,5 +25,11 @@ ignore_orientation_readings="false" use_clutch="$(arg use_clutch)" emulate_button="true" - /> + effector_mass="-1.0" + /> + +