From 5d98d9ac0029243c9afbe1b4bd62dc16ba9d5ae9 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 28 Aug 2024 12:30:54 +0200 Subject: [PATCH] allow change effector mass and ignore rotations (#31) (#32) * add effector mass management * fix effector mass parameter reading * add option to ignore orientation readings (i.e., exporting the interfaces, but setting them to zero) * add "ignore_orientation_readings" to config * add default masses (cherry picked from commit a6799550ce2f9785b8a7c12fc1b9d3ae108ed608) Co-authored-by: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> --- fd_description/config/fd.config.xacro | 9 ++- .../ros2_control/fd.r2c_hardware.xacro | 4 +- .../include/fd_hardware/fd_effort_hi.hpp | 6 ++ fd_hardware/src/fd_effort_hi.cpp | 70 ++++++++++++++++--- 4 files changed, 77 insertions(+), 12 deletions(-) diff --git a/fd_description/config/fd.config.xacro b/fd_description/config/fd.config.xacro index 452e931..56da320 100644 --- a/fd_description/config/fd.config.xacro +++ b/fd_description/config/fd.config.xacro @@ -22,7 +22,14 @@ 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" - /> + effector_mass="-1.0" + /> + + diff --git a/fd_description/ros2_control/fd.r2c_hardware.xacro b/fd_description/ros2_control/fd.r2c_hardware.xacro index 40e269c..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'"> @@ -13,6 +13,8 @@ ${interface_id} ${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 4c8c90c..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,12 @@ 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; + 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..70fd1b0 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/fd_hardware/src/fd_effort_hi.cpp @@ -176,6 +176,23 @@ 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_mass->second); + RCLCPP_INFO(LOGGER, "Interface mass parameter found: %lf Kg", effector_mass_); + } else { + 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_ && @@ -282,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 @@ -303,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 @@ -329,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 @@ -430,15 +462,15 @@ 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 - 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 +489,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 { @@ -489,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;