allow change effector mass and ignore rotations

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"
-    /> <!-- orientation_is_actuated = use_orientation for now (see HW impl.) -->
+        effector_mass="-1.0"
+    />
+
+    <!-- Default effector masses:
+        - 0.148 kg for the Omega 3 device
+        - 0.5 kg for the Omega 6 device
+    -->
 </robot>

fd_description/ros2_control/fd.r2c_hardware.xacro

@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro
         name="fd_ros2_control"
-        params="robot_id plugin_name:='FDHapticInterface' interface_id:='-1' use_fake_hardware:='false' use_orientation:='false' orientation_is_actuated:='false' use_clutch:='false' emulate_button:='false'">
+        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'">
         <ros2_control name="${plugin_name}" type="system">
             <hardware>
                 <xacro:if value="${use_fake_hardware}">
@@ -13,6 +13,8 @@
                     <param name="interface_id">${interface_id}</param>
                     <param name="emulate_button">${emulate_button}</param>
                     <param name="inertia_interface_name">fd_inertia</param>
+                    <param name="effector_mass">${effector_mass}</param>
+                    <param name="ignore_orientation_readings">${ignore_orientation_readings}</param>
                 </xacro:unless>
             </hardware>
 

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

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(&current_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;