icub-tech-iit · Nicogene · May 5, 2023 · Apr 27, 2023 · Apr 27, 2023 · Apr 28, 2023
diff --git a/urdf/ergoCub/conf/ergocub.xml b/urdf/ergoCub/conf/ergocub.xml
@@ -31,6 +31,7 @@
 
     <!-- INERTIAL SENSOR-->
     <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
+    <xi:include href="wrappers/inertials/waist-inertials_wrapper.xml" />
 
     <!--  HEAD SENSORS -->
     <xi:include href="sensors/rgbd_camera_wrapper.xml" />

diff --git a/urdf/ergoCub/conf/ergocub_ros2.xml b/urdf/ergoCub/conf/ergocub_ros2.xml
@@ -34,6 +34,7 @@
 
     <!-- INERTIAL SENSOR-->
     <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
+    <xi:include href="wrappers/inertials/waist-inertials_wrapper.xml" />
 
     <!--  HEAD SENSORS -->
     <xi:include href="sensors/rgbd_camera_wrapper.xml" />

diff --git a/urdf/ergoCub/conf/estimators/wbd_ecub_sim.xml b/urdf/ergoCub/conf/estimators/wbd_ecub_sim.xml
@@ -57,6 +57,7 @@
                 <elem name="left_lower_arm">left_arm-mc_wrapper</elem-->
                 <!-- imu -->
                 <elem name="imu">head_inertial_hardware_device</elem>
+                <elem name="waist_imu">waist_inertial_hardware_device</elem>
                 <!-- ft -->
                 <elem name="l_arm_ft_sensor">ergocub_left_arm_ft</elem>
                 <elem name="r_arm_ft_sensor">ergocub_right_arm_ft</elem>

diff --git a/urdf/ergoCub/conf/gazebo_ergocub_waist_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_waist_inertial.ini
@@ -0,0 +1,2 @@
+disableImplicitNetworkWrapper
+yarpDeviceName waist_inertial_hardware_device
diff --git a/urdf/ergoCub/conf/launch_wholebodydynamics_ecub.xml b/urdf/ergoCub/conf/launch_wholebodydynamics_ecub.xml
@@ -40,7 +40,11 @@
         <param name="local"> /wholeBodyDynamics/imu </param>
     </device>
 
-
+    <device name="waist_inertial_hardware_device" type="multipleanalogsensorsclient">
+        <param name="remote"> /ergocubSim/waist/inertials </param>
+        <param name="local"> /wholeBodyDynamics/waist_imu </param>
+    </device>
+
     <!-- six axis force torque sensors -->
     <device name="ergocub_left_arm_ft" type="multipleanalogsensorsclient">
         <param name="remote"> /ergocubSim/left_arm </param>

diff --git a/urdf/ergoCub/conf/wrappers/inertials/waist-inertials_wrapper.xml b/urdf/ergoCub/conf/wrappers/inertials/waist-inertials_wrapper.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="waist-inertials_wrapper" type="multipleanalogsensorsserver">
+        <param name="period">       10                  </param>
+        <param name="name">     ${portprefix}/waist/inertials       </param>
+
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+                <elem name="SetOfIMUs"> waist_inertial_hardware_device </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device> 
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf
@@ -566,8 +566,8 @@
   <link name="torso_1">
     <inertial>
       <origin xyz="-0.00963997 0.0006168500000000001 0.08244230000000001" rpy="0 0 1.5707963267948966"/>
-      <mass value="2.01685"/>
-      <inertia ixx="0.00709474" ixy="6.97521e-06" ixz="-1.25869e-05" iyy="0.00581692" iyz="0.000722773" izz="0.00295365"/>
+      <mass value="2.0645667"/>
+      <inertia ixx="0.0074060860" ixy="5.5916149e-06" ixz="-2.2492681e-05" iyy="0.0059470733" iyz="0.00055043756" izz="0.0031780111"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0.11800000000000006" rpy="0 0 1.5707963267948966"/>
@@ -585,6 +585,13 @@
       </geometry>
     </collision>
   </link>
+  <link name="waist_imu_0"/>
+  <joint name="waist_imu_0_fixed_joint" type="fixed">
+    <origin xyz="0.0354497 0.00639688 0.060600" rpy="-1.5707322 1.5707963 0"/>
+    <parent link="torso_1"/>
+    <child link="waist_imu_0"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <joint name="torso_roll" type="revolute">
     <origin xyz="0 0 0.060499999999999984" rpy="0 0 0"/>
     <axis xyz="-1.0 0.0 0.0"/>
@@ -3291,6 +3298,20 @@
     <parent link="head"/>
     <origin rpy="3.141592653589793 -0.0 1.5707963267948966" xyz="-0.004299998 -0.0055 0.03823499999999996"/>
   </sensor>
+  <gazebo reference="torso_1">
+    <sensor name="waist_imu_0" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.0354497 0.00639688 0.060600 -1.5707322 1.5707963 0</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+    <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+</plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="waist_imu_0" type="accelerometer">
+    <parent link="torso_1"/>
+    <origin rpy="-1.5707322 1.5707963 0" xyz="0.0354497 0.00639688 0.060600"/>
+  </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>

diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf
@@ -566,8 +566,8 @@
   <link name="torso_1">
     <inertial>
       <origin xyz="-0.00963997 0.0006168500000000001 0.08244230000000001" rpy="0 0 1.5707963267948966"/>
-      <mass value="2.01685"/>
-      <inertia ixx="0.00709474" ixy="6.97521e-06" ixz="-1.25869e-05" iyy="0.00581692" iyz="0.000722773" izz="0.00295365"/>
+      <mass value="2.0645667"/>
+      <inertia ixx="0.0074060860" ixy="5.5916149e-06" ixz="-2.2492681e-05" iyy="0.0059470733" iyz="0.00055043756" izz="0.0031780111"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0.11800000000000006" rpy="0 0 1.5707963267948966"/>
@@ -585,6 +585,13 @@
       </geometry>
     </collision>
   </link>
+  <link name="waist_imu_0"/>
+  <joint name="waist_imu_0_fixed_joint" type="fixed">
+    <origin xyz="0.0354497 0.00639688 0.060600" rpy="-1.5707322 1.5707963 0"/>
+    <parent link="torso_1"/>
+    <child link="waist_imu_0"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <joint name="torso_roll" type="revolute">
     <origin xyz="0 0 0.060499999999999984" rpy="0 0 0"/>
     <axis xyz="-1.0 0.0 0.0"/>
@@ -3291,6 +3298,20 @@
     <parent link="head"/>
     <origin rpy="3.141592653589793 -0.0 1.5707963267948966" xyz="-0.004299998 -0.0055 0.03823499999999996"/>
   </sensor>
+  <gazebo reference="torso_1">
+    <sensor name="waist_imu_0" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.0354497 0.00639688 0.060600 -1.5707322 1.5707963 0</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+    <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+</plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="waist_imu_0" type="accelerometer">
+    <parent link="torso_1"/>
+    <origin rpy="-1.5707322 1.5707963 0" xyz="0.0354497 0.00639688 0.060600"/>
+  </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>

diff --git a/urdf/ergoCub/robots/ergoCubSN000/model.urdf b/urdf/ergoCub/robots/ergoCubSN000/model.urdf
@@ -566,8 +566,8 @@
   <link name="torso_1">
     <inertial>
       <origin xyz="-0.00963997 0.0006168500000000001 0.08244230000000001" rpy="0 0 1.5707963267948966"/>
-      <mass value="2.01685"/>
-      <inertia ixx="0.00709474" ixy="6.97521e-06" ixz="-1.25869e-05" iyy="0.00581692" iyz="0.000722773" izz="0.00295365"/>
+      <mass value="2.0645667"/>
+      <inertia ixx="0.0074060860" ixy="5.5916149e-06" ixz="-2.2492681e-05" iyy="0.0059470733" iyz="0.00055043756" izz="0.0031780111"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0.11800000000000006" rpy="0 0 1.5707963267948966"/>
@@ -585,6 +585,13 @@
       </geometry>
     </collision>
   </link>
+  <link name="waist_imu_0"/>
+  <joint name="waist_imu_0_fixed_joint" type="fixed">
+    <origin xyz="0.0354497 0.00639688 0.060600" rpy="-1.5707322 1.5707963 0"/>
+    <parent link="torso_1"/>
+    <child link="waist_imu_0"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <joint name="torso_roll" type="revolute">
     <origin xyz="0 0 0.060499999999999984" rpy="0 0 0"/>
     <axis xyz="-1.0 0.0 0.0"/>
@@ -3291,6 +3298,20 @@
     <parent link="head"/>
     <origin rpy="3.141592653589793 -0.0 1.5707963267948966" xyz="-0.004299998 -0.0055 0.03823499999999996"/>
   </sensor>
+  <gazebo reference="torso_1">
+    <sensor name="waist_imu_0" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.0354497 0.00639688 0.060600 -1.5707322 1.5707963 0</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+    <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+</plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="waist_imu_0" type="accelerometer">
+    <parent link="torso_1"/>
+    <origin rpy="-1.5707322 1.5707963 0" xyz="0.0354497 0.00639688 0.060600"/>
+  </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>

diff --git a/urdf/simmechanics/CMakeLists.txt b/urdf/simmechanics/CMakeLists.txt
@@ -481,7 +481,14 @@ macro(generate_ergocub_simmechanics)
     set(MESH_FILE_FORMAT "filenameformatchangeext: \"package://iCub/meshes/simmechanics/%s-binary.stl\"")
     set(ANKLE_PITCH_ROM "-35,35")
     set(CUSTOM_EPSILON "")
-    set(XSENS_IMU_FRAME "")
+    set(XSENS_IMU_FRAME 
+"
+- frameName: XSENS_MTI_630_IMU_CSYS
+    linkName: torso_1
+    sensorName: waist_imu_0
+    sensorType: \"accelerometer\"
+    sensorBlobs: \"\"
+")
     set(FT_IMU_SENSORS
 "  - frameName: SCSYS_R_UPPER_LEG_FT-IMU_3B11
     linkName: r_hip_2

diff --git a/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml
@@ -310,6 +310,9 @@ exportedFrames:
   - frameName: SCSYS_HEAD_IMU
     frameReferenceLink: head
     exportedFrameName: imu_frame
+  - frameName: XSENS_MTI_630_IMU_CSYS
+    frameReferenceLink: torso_1
+    exportedFrameName: waist_imu_frame
   - frameName: SCSYS_HEAD_DEPTH
     frameReferenceLink: realsense
     exportedFrameName: realsense_depth_frame
@@ -810,6 +813,17 @@ sensors:
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
         </plugin>
 
+  - frameName: XSENS_MTI_630_IMU_CSYS
+    linkName: torso_1
+    sensorName: waist_imu_0
+    exportFrameInURDF: Yes
+    sensorType: "accelerometer"
+    sensorBlobs:
+    - |
+        <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+        </plugin>
+
   - sensorName: default
     exportFrameInURDF: No
     sensorType: "depth"

diff --git a/urdf/simmechanics/data/ergocub/ERGOCUB_all_options_gazebo.yaml.in b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options_gazebo.yaml.in
@@ -310,6 +310,9 @@ exportedFrames:
   - frameName: SCSYS_HEAD_IMU
     frameReferenceLink: head
     exportedFrameName: imu_frame
+  - frameName: XSENS_MTI_630_IMU_CSYS
+    frameReferenceLink: torso_1
+    exportedFrameName: waist_imu_frame
   - frameName: SCSYS_HEAD_DEPTH
     frameReferenceLink: realsense
     exportedFrameName: realsense_depth_frame
@@ -806,6 +809,17 @@ sensors:
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
         </plugin>
 
+  - frameName: XSENS_MTI_630_IMU_CSYS
+    linkName: torso_1
+    sensorName: waist_imu_0
+    exportFrameInURDF: Yes
+    sensorType: "accelerometer"
+    sensorBlobs:
+    - |
+        <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+        </plugin>
+
   - sensorName: default
     exportFrameInURDF: No
     sensorType: "depth"

diff --git a/urdf/simmechanics/data/ergocub/conf/ergocub.xml b/urdf/simmechanics/data/ergocub/conf/ergocub.xml
@@ -31,6 +31,7 @@
 
     <!-- INERTIAL SENSOR-->
     <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
+    <xi:include href="wrappers/inertials/waist-inertials_wrapper.xml" />
 
     <!--  HEAD SENSORS -->
     <xi:include href="sensors/rgbd_camera_wrapper.xml" />

diff --git a/urdf/simmechanics/data/ergocub/conf/gazebo_ergocub_waist_inertial.ini b/urdf/simmechanics/data/ergocub/conf/gazebo_ergocub_waist_inertial.ini
@@ -0,0 +1,2 @@
+disableImplicitNetworkWrapper
+yarpDeviceName waist_inertial_hardware_device
diff --git a/urdf/simmechanics/data/ergocub/conf/wrappers/inertials/waist-inertials_wrapper.xml b/urdf/simmechanics/data/ergocub/conf/wrappers/inertials/waist-inertials_wrapper.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="waist-inertials_wrapper" type="multipleanalogsensorsserver">
+        <param name="period">       10                  </param>
+        <param name="name">     ${portprefix}/waist/inertials       </param>
+
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+                <elem name="SetOfIMUs"> waist_inertial_hardware_device </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>