From dbd3190760ff86299942a23721240a90939daf2f Mon Sep 17 00:00:00 2001
From: Martina Gloria <114698424+martinaxgloria@users.noreply.github.com>
Date: Fri, 5 May 2023 17:57:36 +0200
Subject: [PATCH] Add waist_imu in urdf file (#99)

---
 urdf/ergoCub/conf/estimators/wbd_ecub_sim.xml |  1 +
 .../conf/launch_wholebodydynamics_ecub.xml    |  8 +++-
 .../data/ergocub/ERGOCUB_all_options.yaml     | 45 ++++++++++++++++++-
 .../ERGOCUB_all_options_gazebo.yaml.in        | 38 ++++++++++++++++
 .../data/ergocub/conf/ergocub.xml             |  1 +
 .../conf/gazebo_ergocub_waist_inertial.ini    |  2 +
 .../inertials/waist-inertials_wrapper.xml     | 17 +++++++
 7 files changed, 108 insertions(+), 4 deletions(-)
 create mode 100644 urdf/simmechanics/data/ergocub/conf/gazebo_ergocub_waist_inertial.ini
 create mode 100644 urdf/simmechanics/data/ergocub/conf/wrappers/inertials/waist-inertials_wrapper.xml

diff --git a/urdf/ergoCub/conf/estimators/wbd_ecub_sim.xml b/urdf/ergoCub/conf/estimators/wbd_ecub_sim.xml
index 5975bda9..75a19224 100644
--- 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/launch_wholebodydynamics_ecub.xml b/urdf/ergoCub/conf/launch_wholebodydynamics_ecub.xml
index d5389ca5..ebf5b78c 100644
--- a/urdf/ergoCub/conf/launch_wholebodydynamics_ecub.xml
+++ b/urdf/ergoCub/conf/launch_wholebodydynamics_ecub.xml
@@ -39,8 +39,12 @@
         <param name="remote"> /ergocubSim/head/inertials </param>
         <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/simmechanics/data/ergocub/ERGOCUB_all_options.yaml b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml
index b6a931c2..2fb139d9 100644
--- a/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml
+++ b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml
@@ -808,8 +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>
-
+        </plugin>     
   - sensorName: default
     exportFrameInURDF: No
     sensorType: "depth"
@@ -937,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]
@@ -1018,6 +1027,38 @@ 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>
+    - | 
+            <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>
     # 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
index 70899f37..6b967edd 100644
--- a/urdf/simmechanics/data/ergocub/ERGOCUB_all_options_gazebo.yaml.in
+++ b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options_gazebo.yaml.in
@@ -936,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
@@ -1216,6 +1222,38 @@ 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>
+    - | 
+            <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>
     # upperbody
     - |
             <link name="base_link" />
diff --git a/urdf/simmechanics/data/ergocub/conf/ergocub.xml b/urdf/simmechanics/data/ergocub/conf/ergocub.xml
index 252e8706..28a89503 100644
--- 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
new file mode 100644
index 00000000..1c03fc88
--- /dev/null
+++ b/urdf/simmechanics/data/ergocub/conf/gazebo_ergocub_waist_inertial.ini
@@ -0,0 +1,2 @@
+disableImplicitNetworkWrapper
+yarpDeviceName waist_inertial_hardware_device
\ No newline at end of file
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
new file mode 100644
index 00000000..38101a19
--- /dev/null
+++ 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> 
\ No newline at end of file