ergoCubGazeboV1: reenable torso_pitch

icub-tech-iit · Jul 20, 2023 · a579047 · a579047
1 parent 6f01cb9
commit a579047
Show file tree

Hide file tree

Showing 6 changed files with 49 additions and 54 deletions.
diff --git a/tests/ergocub-model-test.cpp b/tests/ergocub-model-test.cpp
@@ -518,11 +518,10 @@ int main(int argc, char ** argv)
     comp.setRobotState(qj,dqj,grav);
 
     // Check axis
-    // Skipping for now because we had to lock torso_pitch to 10 deg see https://github.com/icub-tech-iit/ergocub-software/issues/102
-    // if( !checkAxisDirections(comp) )
-    // {
-    //     return EXIT_FAILURE;
-    // }
+    if( !checkAxisDirections(comp) )
+    {
+        return EXIT_FAILURE;
+    }
     // Check if base_link exist, and check that is a frame attached to root_link and if its
     // transform is the idyn
     if( !checkBaseLink(comp) )
@@ -543,11 +542,11 @@ int main(int argc, char ** argv)
     {
         return EXIT_FAILURE;
     }
-    // Skipping for now because we had to lock torso_pitch to 10 deg see https://github.com/icub-tech-iit/ergocub-software/issues/102
-    //if (!checkFTSensorsAreCorrectlyOriented(comp))
-    //{
-    //    return EXIT_FAILURE;
-    //}
+
+    if (!checkFTSensorsAreCorrectlyOriented(comp))
+    {
+       return EXIT_FAILURE;
+    }
 
 
     std::cerr << "Check for model " << modelPath << " concluded correctly!" << std::endl;

diff --git a/urdf/simmechanics/CMakeLists.txt b/urdf/simmechanics/CMakeLists.txt
@@ -570,14 +570,10 @@ macro(generate_ergocub_simmechanics)
                        DEPENDS  "${GENERATED_YAML_LOCATION}"
                                 "${GENERATED_CSV_LOCATION}")
 
-    # Set torso pitch fixed to 10 deg see https://github.com/icub-tech-iit/ergocub-software/issues/102
     add_custom_command(OUTPUT "${CMAKE_BINARY_DIR}/${BUILD_PREFIX}/robots/${GIVTWO_YARP_ROBOT_NAME}/model.urdf"
-                      MAIN_DEPENDENCY "${GIVTWO_YARP_ROBOT_NAME}.urdf"
-                      COMMENT "I AM COPYING TO THE BUILD!"
-                      COMMAND sed -i 's/<joint name=\"torso_pitch\" type=\"revolute\">/<joint name=\"torso_pitch\" type=\"fixed\">/g' ${CMAKE_BINARY_DIR}/urdf/simmechanics/${GIVTWO_YARP_ROBOT_NAME}.urdf
-                      COMMAND sed -i 's/<origin xyz=\"0 0 0.11800000000000005\" rpy=\"0 0 0\"\\/>/<origin xyz=\"0 0 0.11800000000000005\" rpy=\"0 0.174533 0\"\\/>/g' ${CMAKE_BINARY_DIR}/urdf/simmechanics/${GIVTWO_YARP_ROBOT_NAME}.urdf
-                      COMMAND  ${CMAKE_COMMAND} -E copy "${GIVTWO_YARP_ROBOT_NAME}.urdf" "${CMAKE_BINARY_DIR}/${BUILD_PREFIX}/robots/${GIVTWO_YARP_ROBOT_NAME}/model.urdf"
-)
+                       MAIN_DEPENDENCY "${GIVTWO_YARP_ROBOT_NAME}.urdf"
+                       COMMAND ${CMAKE_COMMAND} -E
+                               copy "${GIVTWO_YARP_ROBOT_NAME}.urdf" "${CMAKE_BINARY_DIR}/${BUILD_PREFIX}/robots/${GIVTWO_YARP_ROBOT_NAME}/model.urdf")
 
     list(APPEND model-simmechanics-generated-models "${CMAKE_BINARY_DIR}/${BUILD_PREFIX}/robots/${GIVTWO_YARP_ROBOT_NAME}/model.urdf")
 endmacro()

diff --git a/urdf/simmechanics/data/ergocub/conf/estimators/wbd_ecub_sim.xml b/urdf/simmechanics/data/ergocub/conf/estimators/wbd_ecub_sim.xml
@@ -3,7 +3,7 @@
 
 <devices robot="ergoCubGazeboV1" build="1">
    <device name="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_roll,torso_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,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="axesNames">(torso_roll,torso_pitch,torso_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,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>
@@ -22,22 +22,22 @@
         <group name="GRAVITY_COMPENSATION">
             <param name="enableGravityCompensation">true</param>
             <param name="gravityCompensationBaseLink">root_link</param>
-            <param name="gravityCompensationAxesNames">(torso_roll,torso_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow)</param>
+            <param name="gravityCompensationAxesNames">(torso_roll,torso_pitch,torso_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow)</param>
         </group>
 
         <group name="HW_USE_MAS_IMU">
             <param name="accelerometer">head_imu_0</param>
             <param name="gyroscope">head_imu_0</param>
         </group>
-	
+
         <group name="WBD_OUTPUT_EXTERNAL_WRENCH_PORTS">
             <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="multipleAnalogSensorsNames">
             <param name="SixAxisForceTorqueSensorsNames">("l_arm_ft_sensor", "r_arm_ft_sensor", "l_leg_ft_sensor", "r_leg_ft_sensor", "l_foot_front_ft_sensor", "r_foot_front_ft_sensor", "l_foot_rear_ft_sensor", "r_foot_rear_ft_sensor")</param>

diff --git a/urdf/simmechanics/data/ergocub/conf/gazebo_ergocub_torso.ini b/urdf/simmechanics/data/ergocub/conf/gazebo_ergocub_torso.ini
@@ -1,11 +1,11 @@
 disableImplicitNetworkWrapper
 yarpDeviceName torso_hardware_device
-jointNames torso_roll torso_yaw
+jointNames torso_roll torso_pitch torso_yaw
 
-min_stiffness 0.0    0.0
-max_stiffness 1000.0 1000.0
-min_damping   0.0    0.0
-max_damping   100.0  100.0
+min_stiffness 0.0    0.0    0.0
+max_stiffness 1000.0 1000.0 1000.0
+min_damping   0.0    0.0    0.0
+max_damping   100.0  100.0  100.0
 
 
 [TRAJECTORY_GENERATION]
@@ -16,37 +16,37 @@ trajectory_type minimum_jerk
 [POSITION_CONTROL]
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
-kp            70.0  70.0
-kd            0.15  0.15
-ki            0.17  0.17
-maxInt        9999  9999
-maxOutput     9999  9999
-shift         0.0   0.0
-ko            0.0   0.0
-stictionUp    0.0   0.0
-stictionDwn   0.0   0.0
+kp            70.0  70.0  70.0
+kd            0.15  0.15  0.15
+ki            0.17  0.17  0.17
+maxInt        9999  9999  9999
+maxOutput     9999  9999  9999
+shift         0.0   0.0   0.0
+ko            0.0   0.0   0.0
+stictionUp    0.0   0.0   0.0
+stictionDwn   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
 velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
-kp            8.726 8.726
-kd            0.035 0.035
-ki            0.002 0.002
-maxInt        9999  9999
-maxOutput     9999  9999
-shift         0.0   0.0
-ko            0.0   0.0
-stictionUp    0.0   0.0
-stictionDwn   0.0   0.0
+kp            8.726 8.726  8.726
+kd            0.035 0.035  0.035
+ki            0.002 0.002  0.002
+maxInt        9999  9999   9999
+maxOutput     9999  9999   9999
+shift         0.0   0.0    0.0
+ko            0.0   0.0    0.0
+stictionUp    0.0   0.0    0.0
+stictionDwn   0.0   0.0    0.0
 
 [IMPEDANCE_CONTROL]
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
-stiffness     0.0 0.0
-damping       0.0 0.0
+stiffness     0.0 0.0 0.0
+damping       0.0 0.0 0.0
 
 [LIMITS]
-jntPosMax  43.0 23.0
-jntPosMin -43.0 -23.0
-jntVelMax 100.0 100.0
+jntPosMax  43.0 45.0  23.0
+jntPosMin -43.0 -18.0 -23.0
+jntVelMax 100.0 100.0 100.0
diff --git a/urdf/simmechanics/data/ergocub/conf/wrappers/motorControl/all-mc_remapper.xml b/urdf/simmechanics/data/ergocub/conf/wrappers/motorControl/all-mc_remapper.xml
@@ -3,9 +3,9 @@
 
 <device xmlns:xi="http://www.w3.org/2001/XInclude" name="alljoints_mc" type="controlboardremapper">
 <!-- These are the parameters with torso_pitch removed -->
-    <param name="axesNames">(neck_pitch,neck_roll,neck_yaw,camera_tilt,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch,l_thumb_add,l_thumb_oc,l_index_add,l_index_oc,l_middle_oc,l_ring_pinky_oc,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch,r_thumb_add,r_thumb_oc,r_index_add,r_index_oc,r_middle_oc,r_ring_pinky_oc,torso_roll,torso_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="axesNames">(neck_pitch,neck_roll,neck_yaw,camera_tilt,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch,l_thumb_add,l_thumb_oc,l_index_add,l_index_oc,l_middle_oc,l_ring_pinky_oc,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch,r_thumb_add,r_thumb_oc,r_index_add,r_index_oc,r_middle_oc,r_ring_pinky_oc,torso_roll,torso_pitch,torso_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="period"> 0.010 </param>
-    <param name="joints"> 44 </param>
+    <param name="joints"> 45 </param>
     <action phase="startup" level="6" type="attach">
         <paramlist name="networks">
             <elem name="head_joints">       head-mc_remapper </elem>

diff --git a/urdf/simmechanics/data/ergocub/conf/wrappers/motorControl/torso-mc_remapper.xml b/urdf/simmechanics/data/ergocub/conf/wrappers/motorControl/torso-mc_remapper.xml
@@ -3,9 +3,9 @@
 
 <device xmlns:xi="http://www.w3.org/2001/XInclude" name="torso-mc_remapper" type="controlboardremapper">
     <paramlist name="networks">
-        <elem name="torso_joints">(  0 1 0 1 )</elem>
+        <elem name="torso_joints">(  0 2 0 2 )</elem>
     </paramlist>
-    <param name="joints"> 2			</param>
+    <param name="joints"> 3		</param>
     <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
             <elem name="torso_joints">  torso_hardware_device </elem>