[ergoCubSN001] Update the configuration files to use WBD (#584)

robotology · Nov 2, 2023 · eea8666 · eea8666
1 parent a248cc3
commit eea8666
Show file tree

Hide file tree

Showing 2 changed files with 85 additions and 79 deletions.
diff --git a/ergoCubSN001/ergocub_wbd.xml b/ergoCubSN001/ergocub_wbd.xml
@@ -9,17 +9,17 @@
 
     <devices>
 
-    <!-- POS4 -->
+    <!-- POS4
     <xi:include href="wrappers/POS/right_hand-pos_wrapper4.xml" />
     <xi:include href="hardware/POS/right_hand-pos4.xml" />
     <xi:include href="wrappers/POS/left_hand-pos_wrapper4.xml" />
-    <xi:include href="hardware/POS/left_hand-pos4.xml" />
+    <xi:include href="hardware/POS/left_hand-pos4.xml" /> -->
 
-    <!-- POS2 -->
+    <!-- POS2
     <xi:include href="wrappers/POS/right_hand-pos_wrapper2.xml" />
     <xi:include href="hardware/POS/right_hand-pos2.xml" />
     <xi:include href="wrappers/POS/left_hand-pos_wrapper2.xml" />
-    <xi:include href="hardware/POS/left_hand-pos2.xml" />
+    <xi:include href="hardware/POS/left_hand-pos2.xml" /> -->
 
     <!-- motor controllers wrappers -->
     <xi:include href="wrappers/motorControl/left_leg-mc_wrapper.xml" />
@@ -51,16 +51,16 @@
     <!-- LEFT ARM -->
     <xi:include href="hardware/motorControl/left_arm-eb2-j0_1-mc.xml" />
     <xi:include href="hardware/motorControl/left_arm-eb4-j2_3-mc.xml" />
-    <xi:include href="hardware/motorControl/left_arm-eb31-j4_6-mc.xml" />
+   <!-- <xi:include href="hardware/motorControl/left_arm-eb31-j4_6-mc.xml" />
     <xi:include href="hardware/motorControl/left_arm-eb23-j7_10-mc.xml" />
-    <xi:include href="hardware/motorControl/left_arm-eb25-j11_12-mc.xml" />
+    <xi:include href="hardware/motorControl/left_arm-eb25-j11_12-mc.xml" /> -->
 
     <!-- RIGHT ARM -->
     <xi:include href="hardware/motorControl/right_arm-eb1-j0_1-mc.xml" />
     <xi:include href="hardware/motorControl/right_arm-eb3-j2_3-mc.xml" />
-    <xi:include href="hardware/motorControl/right_arm-eb30-j4_6-mc.xml" />
+    <!--<xi:include href="hardware/motorControl/right_arm-eb30-j4_6-mc.xml" />
     <xi:include href="hardware/motorControl/right_arm-eb22-j7_10-mc.xml" />
-    <xi:include href="hardware/motorControl/right_arm-eb24-j11_12-mc.xml" />
+    <xi:include href="hardware/motorControl/right_arm-eb24-j11_12-mc.xml" /> -->
 
     <!-- LEFT LEG -->
     <xi:include href="hardware/motorControl/left_leg-eb8-j0_3-mc.xml" />
@@ -88,6 +88,7 @@
     <xi:include href="wrappers/FT/left_leg-FT_wrapper.xml" />
     <xi:include href="wrappers/FT/right_leg-FT_wrapper.xml" />
 
+
     <!-- INERTIAL SENSOR -->
     <xi:include href="wrappers/inertials/head-imuFilter_wrapper.xml" />
     <xi:include href="wrappers/inertials/head-imuFilter.xml" />
@@ -96,6 +97,12 @@
     <xi:include href="hardware/inertials/waist-inertial.xml" />
     <xi:include href="wrappers/inertials/waist-inertials_wrapper.xml" />
 
+    <!-- IMU - MTB4 BOARDS
+    <xi:include href="hardware/inertials/left_arm-eb4-j2_3-inertial.xml" />
+    <xi:include href="wrappers/inertials/left_arm-inertials_wrapper.xml" />
+    <xi:include href="hardware/inertials/right_arm-eb3-j2_3-inertial.xml" />
+    <xi:include href="wrappers/inertials/right_arm-inertials_wrapper.xml" /> -->
+
     <!-- FT SENSORS - IMU -->
     <xi:include href="hardware/inertials/left_arm-eb2-j0_1-inertial.xml" />
     <xi:include href="hardware/inertials/right_arm-eb1-j0_1-inertial.xml" />
@@ -128,7 +135,6 @@
     <xi:include href="calibrators/right_arm-calib.xml" />
     <xi:include href="calibrators/head-calib.xml" />
 
-    <!-- estimators -->
     <xi:include href="estimators/wholebodydynamics.xml" />
 
   </devices>

diff --git a/ergoCubSN001/estimators/wholebodydynamics.xml b/ergoCubSN001/estimators/wholebodydynamics.xml
@@ -2,82 +2,82 @@
 <!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="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_roll,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_roll,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
-        <param name="modelFile">model.urdf</param>
-        <param name="fixedFrameGravity">(0,0,-9.81)</param>
-	<param name="defaultContactFrames">(l_hand_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)</param>
-        <param name="imuFrameName">head_imu_0</param>
-        <param name="publishOnROS">true</param>
-	<param name="forceTorqueEstimateConfidence">2</param>
-        <param name="useJointVelocity">true</param>
-        <param name="useJointAcceleration">false</param>
-	<param name="streamFilteredFT">true</param>
-        <param name="imuFilterCutoffInHz">3.0</param>
-        <param name="forceTorqueFilterCutoffInHz">3.0</param>
-        <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
-        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
-        <param name="devicePeriodInSeconds">0.002</param>
-        <param name="useSkinForContacts">false</param>
-        <param name="publishNetExternalWrenches">true</param>
-	<!--<param name="assume_fixed">root_link</param>-->
-	<group name="HW_USE_MAS_IMU">
-	    <param name="accelerometer">rfeimu_acc</param>
-	    <param name="gyroscope">rfeimu_gyro</param>
-	</group>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="wholebodydynamics" type="wholebodydynamics">
+  <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+  <param name="modelFile">model.urdf</param>
+  <param name="fixedFrameGravity">(0,0,-9.81)</param>
+  <param name="defaultContactFrames">(l_hand_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)</param>
+  <param name="imuFrameName">head_imu_0</param>
+  <param name="publishOnROS">true</param>
+  <param name="forceTorqueEstimateConfidence">2</param>
+  <param name="useJointVelocity">true</param>
+  <param name="useJointAcceleration">false</param>
+  <param name="streamFilteredFT">true</param>
+  <param name="imuFilterCutoffInHz">3.0</param>
+  <param name="forceTorqueFilterCutoffInHz">3.0</param>
+  <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
+  <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
+  <param name="devicePeriodInSeconds">0.002</param>
+  <param name="useSkinForContacts">false</param>
+  <param name="publishNetExternalWrenches">true</param>
+  <!--<param name="assume_fixed">root_link</param>-->
+  <group name="HW_USE_MAS_IMU">
+    <param name="accelerometer">sensor_imu_xsensmt</param>
+    <param name="gyroscope">sensor_imu_xsensmt</param>
+  </group>
 
-        <group name="multipleAnalogSensorsNames">
-            <param name="SixAxisForceTorqueSensorsNames">(l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)</param>
-        </group>
+  <group name="multipleAnalogSensorsNames">
+    <param name="SixAxisForceTorqueSensorsNames">(l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)</param>
+  </group>
 
 
-        <group name="GRAVITY_COMPENSATION">
-            <param name="enableGravityCompensation">true</param>
-            <param name="gravityCompensationBaseLink">root_link</param>
-            <param name="gravityCompensationAxesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
-        </group>
+  <group name="GRAVITY_COMPENSATION">
+    <param name="enableGravityCompensation">true</param>
+    <param name="gravityCompensationBaseLink">root_link</param>
+    <param name="gravityCompensationAxesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+  </group>
 
 
-        <group name="WBD_OUTPUT_EXTERNAL_WRENCH_PORTS">
-	     <param name="/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o">(l_hand_palm,l_hand_palm,root_link)</param>
-            <param name="/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o">(r_hand_palm,r_hand_palm,root_link)</param>
-            <!-- <param name="/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o">(l_lower_leg,l_lower_leg,root_link)</param> -->
-            <!-- <param name="/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o">(r_lower_leg,r_lower_leg,root_link)</param> -->
-            <param name="/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o">(l_foot_front,l_sole,l_sole)</param>
-            <param name="/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o">(l_foot_rear,l_sole,l_sole)</param>
-            <param name="/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o">(r_foot_front,r_sole,r_sole)</param>
-            <param name="/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o">(r_foot_rear,r_sole,r_sole)</param>
-        </group>
+  <group name="WBD_OUTPUT_EXTERNAL_WRENCH_PORTS">
+    <!-- <param name="/wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o">(l_hand_palm,l_hand_palm,root_link)</param> -->
+    <!-- <param name="/wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o">(r_hand_palm,r_hand_palm,root_link)</param> -->
+    <!-- <param name="/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o">(l_lower_leg,l_lower_leg,root_link)</param> -->
+    <!-- <param name="/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o">(r_lower_leg,r_lower_leg,root_link)</param> -->
+    <param name="/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o">(l_foot_front,l_sole,l_sole)</param>
+    <param name="/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o">(l_foot_rear,l_sole,l_sole)</param>
+    <param name="/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o">(r_foot_front,r_sole,r_sole)</param>
+    <param name="/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o">(r_foot_rear,r_sole,r_sole)</param>
+  </group>
 
-        <action phase="startup" level="15" type="attach">
-            <paramlist name="networks">
-                <!-- motorcontrol and virtual torque sensors -->
-		<elem name="head-j0">head-eb20-j0_1-mc</elem>
-                <elem name="head-j2">head-eb21-j2_3-mc</elem>
-                <elem name="torso">torso-eb5-j0_2-mc</elem>
-                <elem name="left_upper_arm-j0">left_arm-eb2-j0_1-mc</elem>
-                <elem name="left_upper_arm-j2">left_arm-eb4-j2_3-mc</elem>
-                <elem name="left_lower_arm">left_arm-eb31-j4_6-mc</elem>
-                <elem name="right_upper_arm-j0">right_arm-eb1-j0_1-mc</elem>
-                <elem name="right_upper_arm-j2">right_arm-eb3-j2_3-mc</elem>
-                <elem name="right_lower_arm">right_arm-eb30-j4_6-mc</elem>
-                <elem name="left_upper_leg-j0">left_leg-eb8-j0_3-mc</elem>
-                <elem name="left_lower_leg-j2">left_leg-eb9-j4_5-mc</elem>
-                <elem name="right_upper_leg-j0">right_leg-eb6-j0_3-mc</elem>
-                <elem name="right_lower_leg-j2">right_leg-eb7-j4_5-mc</elem>
+  <action phase="startup" level="15" type="attach">
+    <paramlist name="networks">
+      <!-- motorcontrol and virtual torque sensors -->
+      <elem name="head-j0">head-eb20-j0_1-mc</elem>
+      <elem name="head-j2">head-eb21-j2_3-mc</elem>
+      <elem name="torso">torso-eb5-j0_2-mc</elem>
+      <elem name="left_upper_arm-j0">left_arm-eb2-j0_1-mc</elem>
+      <elem name="left_upper_arm-j2">left_arm-eb4-j2_3-mc</elem>
+      <!-- <elem name="left_lower_arm">left_arm-eb31-j4_6-mc</elem> -->
+      <elem name="right_upper_arm-j0">right_arm-eb1-j0_1-mc</elem>
+      <elem name="right_upper_arm-j2">right_arm-eb3-j2_3-mc</elem>
+      <!-- <elem name="right_lower_arm">right_arm-eb30-j4_6-mc</elem> -->
+      <elem name="left_upper_leg-j0">left_leg-eb8-j0_3-mc</elem>
+      <elem name="left_lower_leg-j2">left_leg-eb9-j4_5-mc</elem>
+      <elem name="right_upper_leg-j0">right_leg-eb6-j0_3-mc</elem>
+      <elem name="right_lower_leg-j2">right_leg-eb7-j4_5-mc</elem>
 
-                <!-- imu -->
-                <elem name="imu">head-inertial</elem>
-                <!-- imu_waist -->
-<!--            <elem name="imu">xsensmt-inertial</elem> -->
-                <!-- ft -->
-                <elem name="l_leg_ft_sensors">left_leg-FT_remapper</elem>
-                <elem name="r_leg_ft_sensors">right_leg-FT_remapper</elem>
-                <elem name="l_arm_ft_sensors">left_arm-eb2-j0_1-strain</elem>
-                <elem name="r_arm_ft_sensors">right_arm-eb1-j0_1-strain</elem>
-            </paramlist>
-        </action>
+      <!-- imu -->
+      <elem name="imu">waist-inertial</elem>
+      <!-- imu_waist -->
+      <!--            <elem name="imu">xsensmt-inertial</elem> -->
+      <!-- ft -->
+      <elem name="l_leg_ft_sensors">left_leg-FT_remapper</elem>
+      <elem name="r_leg_ft_sensors">right_leg-FT_remapper</elem>
+      <elem name="l_arm_ft_sensors">left_arm-eb2-j0_1-strain</elem>
+      <elem name="r_arm_ft_sensors">right_arm-eb1-j0_1-strain</elem>
+    </paramlist>
+  </action>
 
-	<action phase="shutdown" level="2" type="detach" />
+  <action phase="shutdown" level="2" type="detach" />
 
-    </device>
+</device>