Allow the scenario with clutch, but w/o the orientation

ICube-Robotics · Aug 21, 2024 · b1ad2bd · b1ad2bd
1 parent fb5e545
commit b1ad2bd
Showing 1 changed file with 38 additions and 9 deletions.
diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp
@@ -282,40 +282,63 @@ 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() > 3) {
+  if (dhdHasWrist(interface_ID_) && hw_states_position_.size() == 4) {
+    // No orientation, skip!
+  } else if (dhdHasWrist(interface_ID_) && hw_states_position_.size() > 3) {
     flag += dhdGetOrientationRad(
       &hw_states_position_[3], &hw_states_position_[4],
       &hw_states_position_[5], interface_ID_);
   }
-  if (dhdHasGripper(interface_ID_) && hw_states_position_.size() > 6) {
+
+  // Get gripper angle
+  if (dhdHasGripper(interface_ID_) && hw_states_position_.size() == 4) {
+    flag += dhdGetGripperAngleRad(&hw_states_position_[3], interface_ID_);
+  } else if (dhdHasGripper(interface_ID_) && hw_states_position_.size() > 6) {
     flag += dhdGetGripperAngleRad(&hw_states_position_[6], interface_ID_);
   }
 
   // Get velocity
   flag += dhdGetLinearVelocity(
     &hw_states_velocity_[0], &hw_states_velocity_[1],
     &hw_states_velocity_[2], interface_ID_);
-  if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() > 3) {
+  if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() == 4) {
+    // No orientation, skip!
+  } else if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() > 3) {
     flag += dhdGetAngularVelocityRad(
       &hw_states_velocity_[3], &hw_states_velocity_[4],
       &hw_states_velocity_[5], interface_ID_);
   }
-  if (dhdHasGripper(interface_ID_) && hw_states_velocity_.size() > 6) {
+
+  // Get gripper angular velocity
+  if (dhdHasGripper(interface_ID_) && hw_states_velocity_.size() == 4) {
+    flag += dhdGetGripperAngularVelocityRad(&hw_states_velocity_[3], interface_ID_);
+  } else if (dhdHasGripper(interface_ID_) && hw_states_velocity_.size() > 6) {
     flag += dhdGetGripperAngularVelocityRad(&hw_states_velocity_[6], interface_ID_);
   }
+
   // Get forces
   double torque[3];
   double gripper_force;
   flag += dhdGetForceAndTorqueAndGripperForce(
-    &hw_states_effort_[0], &hw_states_effort_[1],
-    &hw_states_effort_[2], &torque[0], &torque[1],
-    &torque[2], &gripper_force, interface_ID_);
-  if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() > 3) {
+    &hw_states_effort_[0], &hw_states_effort_[1], &hw_states_effort_[2],
+    &torque[0], &torque[1], &torque[2],
+    &gripper_force,
+    interface_ID_
+  );
+
+  // Get torques
+  if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() == 4) {
+    // No orientation, skip!
+  } else if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() > 3) {
     hw_states_effort_[3] = torque[0];
     hw_states_effort_[4] = torque[1];
     hw_states_effort_[5] = torque[2];
   }
-  if (dhdHasGripper(interface_ID_) && hw_states_effort_.size() > 6) {
+
+  // Get gripper force
+  if (dhdHasGripper(interface_ID_) && hw_states_effort_.size() == 4) {
+    hw_states_effort_[3] = gripper_force;
+  } else if (dhdHasGripper(interface_ID_) && hw_states_effort_.size() > 6) {
     hw_states_effort_[6] = gripper_force;
   }
   // Get inertia
@@ -372,12 +395,18 @@ hardware_interface::return_type FDEffortHardwareInterface::write(
         hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2],
         hw_commands_effort_[3], hw_commands_effort_[4], hw_commands_effort_[5],
         hw_commands_effort_[6], interface_ID_);
+    } else if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() == 4) {
+      dhdSetForceAndTorqueAndGripperForce(
+        hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2],
+        0.0, 0.0, 0.0, hw_commands_effort_[3], interface_ID_);
     } else if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() > 3) {
+      // No clutch joint
       dhdSetForceAndTorqueAndGripperForce(
         hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2],
         hw_commands_effort_[3], hw_commands_effort_[4], hw_commands_effort_[5],
         0, interface_ID_);
     } else {
+      // Only translation is actuated
       dhdSetForceAndTorqueAndGripperForce(
         hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2],
         0, 0, 0, 0, interface_ID_);