Revert "ergoCubV1_1: remove ft sensors of the arms" (#208)

icub-tech-iit · Jan 8, 2024 · 5dea0ea · 5dea0ea
2 parents 19e99a0 + 8f562f4
commit 5dea0ea
Show file tree

Hide file tree

Showing 8 changed files with 249 additions and 81 deletions.
diff --git a/tests/ergocub-model-test.cpp b/tests/ergocub-model-test.cpp
@@ -449,7 +449,7 @@ bool checkFTSensorsAreOddAndNotNull(iDynTree::ModelLoader & mdlLoader)
 }
 
 /**
- * All the ergoCub have a even and not null number of F/T sensors.
+ * All the iCub have a even and not null number of F/T sensors.
  */
 bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader)
 {
@@ -524,9 +524,9 @@ bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp)
                            0.5, 0.866025, 0,
                             0, 0, -1);
 
-    // bool ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft");
-    // ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft") && ok;
-    bool ok = checkFrameIsCorrectlyOriented(comp, rootLink_L_sensorFrameExpectedLeg, "l_leg_ft");
+    bool ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft");
+    ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft") && ok;
+    ok = checkFrameIsCorrectlyOriented(comp, rootLink_L_sensorFrameExpectedLeg, "l_leg_ft") && ok;
     ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedLeg, "r_leg_ft") && ok;
     ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_rear_ft") && ok;
     ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_rear_ft") && ok;
@@ -687,9 +687,9 @@ bool checkAllFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGi
     iDynTree::SensorsList sensors = mdlLoader.sensors();
 
 
-    // bool ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_arm_ft");
-    // ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_arm_ft") && ok;
-    bool ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_leg_ft");
+    bool ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_arm_ft");
+    ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_arm_ft") && ok;
+    ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_leg_ft") && ok;
     ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_leg_ft") && ok;
     ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_foot_rear_ft") && ok;
     ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_foot_rear_ft") && ok;

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml
@@ -702,30 +702,30 @@ exportedFrames:
 
 # Sensors options
 forceTorqueSensors:
-    # - jointName: l_arm_ft_sensor
-    #   directionChildToParent: Yes
-    #   exportFrameInURDF: Yes
-    #   frame: sensor
-    #   frameName: SCSYS_L_SHOULDER_2_FT
-    #   linkName: l_shoulder_2
-    #   sensorName: l_arm_ft
-    #   sensorBlobs:
-    #   - |
-    #       <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
-    #         <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
-    #       </plugin>
-    # - jointName: r_arm_ft_sensor
-    #   directionChildToParent: Yes
-    #   exportFrameInURDF: Yes
-    #   frame: sensor
-    #   frameName: SCSYS_R_SHOULDER_2_FT
-    #   linkName: r_shoulder_2
-    #   sensorName: r_arm_ft
-    #   sensorBlobs:
-    #   - |
-    #       <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
-    #         <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
-    #       </plugin>
+    - jointName: l_arm_ft_sensor
+      directionChildToParent: Yes
+      exportFrameInURDF: Yes
+      frame: sensor
+      frameName: SCSYS_L_SHOULDER_2_FT
+      linkName: l_shoulder_2
+      sensorName: l_arm_ft
+      sensorBlobs:
+      - |
+          <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+            <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+          </plugin>
+    - jointName: r_arm_ft_sensor
+      directionChildToParent: Yes
+      exportFrameInURDF: Yes
+      frame: sensor
+      frameName: SCSYS_R_SHOULDER_2_FT
+      linkName: r_shoulder_2
+      sensorName: r_arm_ft
+      sensorBlobs:
+      - |
+          <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+            <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+          </plugin>
     # left leg
     - jointName: l_leg_ft_sensor
       directionChildToParent: Yes

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml
@@ -919,30 +919,30 @@ assignedInertias:
 
 # Sensors options
 forceTorqueSensors:
-    # - jointName: l_arm_ft_sensor
-    #   directionChildToParent: Yes
-    #   exportFrameInURDF: Yes
-    #   frame: sensor
-    #   frameName: SCSYS_L_SHOULDER_2_FT
-    #   linkName: l_shoulder_2
-    #   sensorName: l_arm_ft
-    #   sensorBlobs:
-    #   - |
-    #       <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
-    #         <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
-    #       </plugin>
-    # - jointName: r_arm_ft_sensor
-    #   directionChildToParent: Yes
-    #   exportFrameInURDF: Yes
-    #   frame: sensor
-    #   frameName: SCSYS_R_SHOULDER_2_FT
-    #   linkName: r_shoulder_2
-    #   sensorName: r_arm_ft
-    #   sensorBlobs:
-    #   - |
-    #       <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
-    #         <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
-    #       </plugin>
+    - jointName: l_arm_ft_sensor
+      directionChildToParent: Yes
+      exportFrameInURDF: Yes
+      frame: sensor
+      frameName: SCSYS_L_SHOULDER_2_FT
+      linkName: l_shoulder_2
+      sensorName: l_arm_ft
+      sensorBlobs:
+      - |
+          <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+            <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+          </plugin>
+    - jointName: r_arm_ft_sensor
+      directionChildToParent: Yes
+      exportFrameInURDF: Yes
+      frame: sensor
+      frameName: SCSYS_R_SHOULDER_2_FT
+      linkName: r_shoulder_2
+      sensorName: r_arm_ft
+      sensorBlobs:
+      - |
+          <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+            <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+          </plugin>
     # left leg
     - jointName: l_leg_ft_sensor
       directionChildToParent: Yes

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml
@@ -1032,30 +1032,30 @@ assignedCollisionGeometry:
 
 # Sensors options
 forceTorqueSensors:
-    # - jointName: l_arm_ft_sensor
-    #   directionChildToParent: Yes
-    #   exportFrameInURDF: Yes
-    #   frame: sensor
-    #   frameName: SCSYS_L_SHOULDER_2_FT
-    #   linkName: l_shoulder_2
-    #   sensorName: l_arm_ft
-    #   sensorBlobs:
-    #   - |
-    #       <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
-    #         <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
-    #       </plugin>
-    # - jointName: r_arm_ft_sensor
-    #   directionChildToParent: Yes
-    #   exportFrameInURDF: Yes
-    #   frame: sensor
-    #   frameName: SCSYS_R_SHOULDER_2_FT
-    #   linkName: r_shoulder_2
-    #   sensorName: r_arm_ft
-    #   sensorBlobs:
-    #   - |
-    #       <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
-    #         <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
-    #       </plugin>
+    - jointName: l_arm_ft_sensor
+      directionChildToParent: Yes
+      exportFrameInURDF: Yes
+      frame: sensor
+      frameName: SCSYS_L_SHOULDER_2_FT
+      linkName: l_shoulder_2
+      sensorName: l_arm_ft
+      sensorBlobs:
+      - |
+          <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+            <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+          </plugin>
+    - jointName: r_arm_ft_sensor
+      directionChildToParent: Yes
+      exportFrameInURDF: Yes
+      frame: sensor
+      frameName: SCSYS_R_SHOULDER_2_FT
+      linkName: r_shoulder_2
+      sensorName: r_arm_ft
+      sensorBlobs:
+      - |
+          <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+            <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+          </plugin>
     # left leg
     - jointName: l_leg_ft_sensor
       directionChildToParent: Yes

diff --git a/urdf/ergoCub/conf/ergocub.xml b/urdf/ergoCub/conf/ergocub.xml
@@ -24,8 +24,8 @@
     <xi:include href="wrappers/FT/right_leg-FT_remapper.xml" />
     <xi:include href="wrappers/FT/left_leg-FT_wrapper.xml" />
     <xi:include href="wrappers/FT/right_leg-FT_wrapper.xml" />
-    <!-- <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
-    <xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" /> -->
+    <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" />
 
     <!-- INERTIAL SENSOR-->
     <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />

diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
@@ -2146,6 +2146,12 @@
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.004349999999999996 -2.7755575615628914e-17 -0.054300000000000015" rpy="3.141592653589793 2.498001805406602e-16 -1.5707963267948952"/>
+    <parent link="l_shoulder_2"/>
+    <child link="l_arm_ft"/>
+  </joint>
   <link name="l_foot_front_ft"/>
   <joint name="l_foot_front_ft_fixed_joint" type="fixed">
     <origin xyz="0.13525 -4.163336342344337e-17 -0.047899999999999054" rpy="-4.259189482954547e-17 -7.513330770412076e-17 -2.0943951023931966"/>
@@ -2164,6 +2170,12 @@
     <parent link="l_hip_2"/>
     <child link="l_leg_ft"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.006849999999999986 -2.7755575615628914e-17 -0.05429999999999999" rpy="3.141592653589793 1.6653345369377348e-16 1.5707963267948981"/>
+    <parent link="r_shoulder_2"/>
+    <child link="r_arm_ft"/>
+  </joint>
   <link name="r_foot_front_ft"/>
   <joint name="r_foot_front_ft_fixed_joint" type="fixed">
     <origin xyz="0.13525 1.3877787807814457e-17 -0.04789999999999983" rpy="-4.2591894829555776e-17 -7.513330770412949e-17 -2.094395102393197"/>
@@ -3247,6 +3259,28 @@
   <gazebo>
     <pose>0.000000 0.000000 0.800000 0.000000 0.000000 0.000000</pose>
   </gazebo>
+  <gazebo reference="l_arm_ft_sensor">
+    <sensor name="l_arm_ft" type="force_torque">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <force_torque>
+        <frame>sensor</frame>
+        <measure_direction>child_to_parent</measure_direction>
+      </force_torque>
+      <pose>-3.46945e-18 0 0 3.14159 1.94289e-16 -1.5708 </pose>
+      <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+        <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_arm_ft" type="force_torque">
+    <parent joint="l_arm_ft_sensor"/>
+    <force_torque>
+      <frame>sensor</frame>
+      <measure_direction>child_to_parent</measure_direction>
+    </force_torque>
+    <origin rpy="3.14159 1.94289e-16 -1.5708 " xyz="-3.46945e-18 0 0"/>
+  </sensor>
   <gazebo reference="l_foot_front_ft_sensor">
     <sensor name="l_foot_front_ft" type="force_torque">
       <always_on>1</always_on>
@@ -3313,6 +3347,28 @@
     </force_torque>
     <origin rpy="3.14159 -2.02132e-15 2.61799 " xyz="5.20417e-18 0 0"/>
   </sensor>
+  <gazebo reference="r_arm_ft_sensor">
+    <sensor name="r_arm_ft" type="force_torque">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <force_torque>
+        <frame>sensor</frame>
+        <measure_direction>child_to_parent</measure_direction>
+      </force_torque>
+      <pose>0 0 0 -3.14159 -2.77556e-17 1.5708 </pose>
+      <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+        <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_arm_ft" type="force_torque">
+    <parent joint="r_arm_ft_sensor"/>
+    <force_torque>
+      <frame>sensor</frame>
+      <measure_direction>child_to_parent</measure_direction>
+    </force_torque>
+    <origin rpy="-3.14159 -2.77556e-17 1.5708 " xyz="0 0 0"/>
+  </sensor>
   <gazebo reference="r_foot_front_ft_sensor">
     <sensor name="r_foot_front_ft" type="force_torque">
       <always_on>1</always_on>

diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
@@ -1960,6 +1960,12 @@
     <parent link="realsense"/>
     <child link="realsense_imu_0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.004349999999999996 -2.7755575615628914e-17 -0.054300000000000015" rpy="3.141592653589793 2.498001805406602e-16 -1.5707963267948952"/>
+    <parent link="l_shoulder_2"/>
+    <child link="l_arm_ft"/>
+  </joint>
   <link name="l_foot_front_ft"/>
   <joint name="l_foot_front_ft_fixed_joint" type="fixed">
     <origin xyz="0.13525 -4.163336342344337e-17 -0.047899999999999054" rpy="-4.259189482954547e-17 -7.513330770412076e-17 -2.0943951023931966"/>
@@ -1978,6 +1984,12 @@
     <parent link="l_hip_2"/>
     <child link="l_leg_ft"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.006849999999999986 -2.7755575615628914e-17 -0.05429999999999999" rpy="3.141592653589793 1.6653345369377348e-16 1.5707963267948981"/>
+    <parent link="r_shoulder_2"/>
+    <child link="r_arm_ft"/>
+  </joint>
   <link name="r_foot_front_ft"/>
   <joint name="r_foot_front_ft_fixed_joint" type="fixed">
     <origin xyz="0.13525 1.3877787807814457e-17 -0.04789999999999983" rpy="-4.2591894829555776e-17 -7.513330770412949e-17 -2.094395102393197"/>
@@ -3061,6 +3073,28 @@
   <gazebo>
     <pose>0.000000 0.000000 0.800000 0.000000 0.000000 0.000000</pose>
   </gazebo>
+  <gazebo reference="l_arm_ft_sensor">
+    <sensor name="l_arm_ft" type="force_torque">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <force_torque>
+        <frame>sensor</frame>
+        <measure_direction>child_to_parent</measure_direction>
+      </force_torque>
+      <pose>-3.46945e-18 0 0 3.14159 1.94289e-16 -1.5708 </pose>
+      <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+        <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_arm_ft" type="force_torque">
+    <parent joint="l_arm_ft_sensor"/>
+    <force_torque>
+      <frame>sensor</frame>
+      <measure_direction>child_to_parent</measure_direction>
+    </force_torque>
+    <origin rpy="3.14159 1.94289e-16 -1.5708 " xyz="-3.46945e-18 0 0"/>
+  </sensor>
   <gazebo reference="l_foot_front_ft_sensor">
     <sensor name="l_foot_front_ft" type="force_torque">
       <always_on>1</always_on>
@@ -3127,6 +3161,28 @@
     </force_torque>
     <origin rpy="3.14159 -2.02132e-15 2.61799 " xyz="5.20417e-18 0 0"/>
   </sensor>
+  <gazebo reference="r_arm_ft_sensor">
+    <sensor name="r_arm_ft" type="force_torque">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <force_torque>
+        <frame>sensor</frame>
+        <measure_direction>child_to_parent</measure_direction>
+      </force_torque>
+      <pose>0 0 0 -3.14159 -2.77556e-17 1.5708 </pose>
+      <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
+        <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_arm_ft" type="force_torque">
+    <parent joint="r_arm_ft_sensor"/>
+    <force_torque>
+      <frame>sensor</frame>
+      <measure_direction>child_to_parent</measure_direction>
+    </force_torque>
+    <origin rpy="-3.14159 -2.77556e-17 1.5708 " xyz="0 0 0"/>
+  </sensor>
   <gazebo reference="r_foot_front_ft_sensor">
     <sensor name="r_foot_front_ft" type="force_torque">
       <always_on>1</always_on>