Add the XMLBlobs, the assigned mass and inertia for waist_imu

icub-tech-iit · May 5, 2023 · 47900b1 · 47900b1
1 parent 72a2fda
commit 47900b1
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 93 deletions.
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf
@@ -585,13 +585,6 @@
       </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"/>
@@ -3298,20 +3291,6 @@
     <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
@@ -585,13 +585,6 @@
       </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"/>
@@ -3298,20 +3291,6 @@
     <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
@@ -585,13 +585,6 @@
       </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"/>
@@ -3298,20 +3291,6 @@
     <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/data/ergocub/ERGOCUB_all_options.yaml b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml
@@ -310,9 +310,6 @@ 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
@@ -811,19 +808,7 @@ sensors:
     - |
         <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
             <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>
-     
+        </plugin>     
   - sensorName: default
     exportFrameInURDF: No
     sensorType: "depth"
@@ -951,6 +936,16 @@ sensors:
           <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
         </plugin>
 
+assignedMasses:
+ torso_1   : 2.0645667
+
+assignedInertias:
+  # This is due to https://github.com/robotology/icub-models/issues/33
+  - linkName: torso_1
+    xx: 0.0074060860
+    yy: 0.0059470733
+    zz: 0.0031780111
+
 assignedColors:
   root_link   : [0.70, 0.70, 0.70, 1]
   torso_1     : [0.70, 0.70, 0.70, 1]
@@ -1032,6 +1027,22 @@ reverseRotationAxis:
 #    neck_yaw
 
 XMLBlobs:
+    # imu waist (workaround since simmechanics was not updated)
+    - |
+            <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>
+
+            <gazebo>
+            <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     # upperbody
     - |
             <link name="base_link" />

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,9 +310,6 @@ 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
@@ -809,17 +806,6 @@ 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"
@@ -950,9 +936,15 @@ sensors:
 
 
 @ASSIGNED_COLLISION_GEOMETRIES@
+assignedMasses:
+ torso_1   : 2.0645667
 
 assignedInertias:
   # This is due to https://github.com/robotology/icub-models/issues/33
+  - linkName: torso_1
+    xx: 0.0074060860
+    yy: 0.0059470733
+    zz: 0.0031780111
   - linkName: realsense
     xx: 0.01
     yy: 0.01
@@ -1230,6 +1222,22 @@ reverseRotationAxis:
 #   neck_yaw
 
 XMLBlobs:
+    # imu waist (workaround since simmechanics was not updated)
+    - |
+            <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>
+
+            <gazebo>
+            <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     # upperbody
     - |
             <link name="base_link" />