icub-tech-iit · Nicogene · Sep 12, 2023 · Sep 11, 2023 · Sep 11, 2023 · Sep 12, 2023
diff --git a/README.md b/README.md
@@ -11,7 +11,7 @@ cmake -DCMAKE_INSTALL_PREFIX=<install-prefix> ..
 make
 (make install)
 ```
-`ergocub` model needs [Gazebo YARP Plugins v4.7.0](https://github.com/robotology/gazebo-yarp-plugins/releases/tag/v4.7.0) or higher in order to be used.
+`ergocub` model needs a Gazebo YARP Plugins version that includes https://github.com/robotology/gazebo-yarp-plugins/pull/662.
 
 In order to use the model, the following env variables must be configured:
 ```sh

diff --git a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options.yaml b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options.yaml
@@ -1340,7 +1340,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |
@@ -1382,7 +1382,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |

diff --git a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml
@@ -1549,7 +1549,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |
@@ -1591,7 +1591,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |

diff --git a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml
@@ -1859,7 +1859,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |
@@ -1901,7 +1901,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml
@@ -1317,7 +1317,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |
@@ -1359,7 +1359,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml
@@ -1534,7 +1534,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |
@@ -1576,7 +1576,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml
@@ -1834,7 +1834,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |
@@ -1876,7 +1876,7 @@ XMLBlobs:
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand.ini</yarpConfigurationFile>
+            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
     - |

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_joint_all_parameters.csv b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_joint_all_parameters.csv
@@ -28,31 +28,31 @@ l_elbow,-3,75,2,0,120,
 l_wrist_roll,-62,52,2,0,120,
 l_wrist_pitch,-32,32,2,0,120,
 l_wrist_yaw,-92,92,2,0,120,
-l_thumb_add,0,90,2,0,100,
-l_thumb_prox,0,82.1,2,0,100,
-l_thumb_dist,0,53.6,2,0,120,
+l_thumb_add,0,100,2,0,100,
+l_thumb_prox,0,90,2,0,100,
+l_thumb_dist,0,101.4,2,0,120,
 l_index_add,0,15,2,0,100,
 l_index_prox,0,90,2,0,100,
-l_index_dist,0,99.2,2,0,100,
+l_index_dist,0,103.1,2,0,100,
 l_middle_prox,0,90,2,0,100,
-l_middle_dist,0,99.2,2,0,100,
+l_middle_dist,0,103.1,2,0,100,
 l_ring_prox,0,90,2,0,100,
-l_ring_dist,0,99.2,2,0,100,
+l_ring_dist,0,103.1,2,0,100,
 l_pinkie_prox,0,90,2,0,100,
-l_pinkie_dist,0,93.3,2,0,100,
+l_pinkie_dist,0,98.7,2,0,100,
 r_elbow,-3,75,2,0,120,
 r_wrist_roll,-62,52,2,0,120,
 r_wrist_pitch,-32,32,2,0,120,
 r_wrist_yaw,-92,92,2,0,120,
-r_thumb_add,0,90,2,0,100,
-r_thumb_prox,0,82.1,2,0,100,
-r_thumb_dist,0,53.6,2,0,120,
+r_thumb_add,0,100,2,0,100,
+r_thumb_prox,0,90,2,0,100,
+r_thumb_dist,0,101.4,2,0,120,
 r_index_add,0,15,2,0,100,
 r_index_prox,0,90,2,0,100,
-r_index_dist,0,99.2,2,0,100,
+r_index_dist,0,103.1,2,0,100,
 r_middle_prox,0,90,2,0,100,
-r_middle_dist,0,99.2,2,0,100,
+r_middle_dist,0,103.1,2,0,100,
 r_ring_prox,0,90,2,0,100,
-r_ring_dist,0,99.2,2,0,100,
+r_ring_dist,0,103.1,2,0,100,
 r_pinkie_prox,0,90,2,0,100,
-r_pinkie_dist,0,93.3,2,0,100,
+r_pinkie_dist,0,98.7,2,0,100,
diff --git a/...ergoCub/conf/gazebo_ergocub_left_hand.ini → ...b/conf/gazebo_ergocub_left_hand_mk5_0.ini b/...ergoCub/conf/gazebo_ergocub_left_hand.ini → ...b/conf/gazebo_ergocub_left_hand_mk5_0.ini
@@ -5,6 +5,16 @@ jointNames l_thumb_add l_thumb_prox l_thumb_dist l_index_add l_index_prox l_inde
 [COUPLING]
 icub_hand_mk5 (0 1 2 3 4 5 6 7 8 9 10 11) (l_thumb_add l_thumb_oc l_index_add l_index_oc l_middle_oc l_ring_pinky_oc reserved reserved reserved reserved reserved reserved) (0.0 0.0 0.0 0.0 0.0 0.0) (90.0 82.1 15.0 90.0 90.0 90.0)
 
+[COUPLING_PARAMS]
+L0x     -0.00555 -0.0050 -0.0050 -0.0050  -0.0050
+L0y      0.00285  0.0040  0.0040  0.0040   0.0040
+q2bias     -180.0  -173.35  -173.35  -173.35  -170.54
+q1off       4.29    2.86    2.86    2.86     3.43
+k         0.0171 0.02918 0.02918 0.02918  0.02425
+d        0.02006 0.03004 0.03004 0.03004  0.02504
+l         0.0085 0.00604 0.00604 0.00604  0.00608
+b        0.00624  0.0064  0.0064  0.0064   0.0064
+
 [TRAJECTORY_GENERATION]
 trajectory_type minimum_jerk
 

diff --git a/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini b/urdf/ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini
@@ -0,0 +1,57 @@
+disableImplicitNetworkWrapper
+yarpDeviceName left_hand_hardware_device
+jointNames l_thumb_add l_thumb_prox l_thumb_dist l_index_add l_index_prox l_index_dist l_middle_prox l_middle_dist l_ring_prox l_ring_dist l_pinkie_prox l_pinkie_dist
+
+[COUPLING]
+icub_hand_mk5 (0 1 2 3 4 5 6 7 8 9 10 11) (l_thumb_add l_thumb_oc l_index_add l_index_oc l_middle_oc l_ring_pinky_oc reserved reserved reserved reserved reserved reserved) (0.0 0.0 0.0 0.0 0.0 0.0) (100.0 90.0 15.0 90.0 90.0 90.0)
+
+[COUPLING_PARAMS]
+L0x      -0.0050 -0.0050 -0.0050 -0.0050  -0.0050
+L0y       0.0040  0.0040  0.0040  0.0040   0.0040
+q2bias   -173.35 -173.35 -173.35 -173.35  -170.53
+q1off       2.45    2.15    2.15    2.15     2.46
+k        0.03415 0.03913 0.03913 0.03913  0.03418
+d        0.03503 0.04003 0.04003 0.04003  0.03503
+l        0.00604 0.00604 0.00604 0.00604  0.00608
+b        0.00640 0.00640 0.00640 0.00640  0.00640
+
+[TRAJECTORY_GENERATION]
+trajectory_type minimum_jerk
+
+[LIMITS]
+jntVelMax  100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0
+jntPosMax   100.0  90.0  101.4  15.0  90.0  103.1  90.0  103.1  90.0  103.1  90.0  98.7
+jntPosMin    0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+
+[POSITION_CONTROL]
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+kp           5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0
+kd           0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+ki           0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00
+maxInt       9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
+maxOutput    9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
+shift        0     0     0     0     0     0     0     0     0     0     0     0
+ko           0     0     0     0     0     0     0     0     0     0     0     0
+stictionUp   0     0     0     0     0     0     0     0     0     0     0     0
+stictionDwn  0     0     0     0     0     0     0     0     0     0     0     0
+
+[VELOCITY_CONTROL]
+controlUnits metric_units
+controlLaw   joint_pid_gazebo_v1
+velocityControlImplementationType integrator_and_position_pid
+kp           0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01
+kd           0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+ki           0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+maxInt       9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
+maxOutput    9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
+shift        0     0     0     0     0     0     0     0     0     0     0     0
+ko           0     0     0     0     0     0     0     0     0     0     0     0
+stictionUp   0     0     0     0     0     0     0     0     0     0     0     0
+stictionDwn  0     0     0     0     0     0     0     0     0     0     0     0
+
+[IMPEDANCE_CONTROL]
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+stiffness    0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+damping      0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
diff --git a/...rgoCub/conf/gazebo_ergocub_right_hand.ini → .../conf/gazebo_ergocub_right_hand_mk5_0.ini b/...rgoCub/conf/gazebo_ergocub_right_hand.ini → .../conf/gazebo_ergocub_right_hand_mk5_0.ini
@@ -5,6 +5,16 @@ jointNames r_thumb_add r_thumb_prox r_thumb_dist r_index_add r_index_prox r_inde
 [COUPLING]
 icub_hand_mk5 (0 1 2 3 4 5 6 7 8 9 10 11) (r_thumb_add r_thumb_oc r_index_add r_index_oc r_middle_oc r_ring_pinky_oc reserved reserved reserved reserved reserved reserved) (0.0 0.0 0.0 0.0 0.0 0.0) (90.0 82.1 15.0 90.0 90.0 90.0)
 
+[COUPLING_PARAMS]
+L0x     -0.00555 -0.0050 -0.0050 -0.0050  -0.0050
+L0y      0.00285  0.0040  0.0040  0.0040   0.0040
+q2bias     -180.0  -173.35  -173.35  -173.35  -170.54
+q1off       4.29    2.86    2.86    2.86     3.43
+k         0.0171 0.02918 0.02918 0.02918  0.02425
+d        0.02006 0.03004 0.03004 0.03004  0.02504
+l         0.0085 0.00604 0.00604 0.00604  0.00608
+b        0.00624  0.0064  0.0064  0.0064   0.0064
+
 [TRAJECTORY_GENERATION]
 trajectory_type minimum_jerk
 

diff --git a/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini b/urdf/ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini
@@ -0,0 +1,57 @@
+disableImplicitNetworkWrapper
+yarpDeviceName right_hand_hardware_device
+jointNames r_thumb_add r_thumb_prox r_thumb_dist r_index_add r_index_prox r_index_dist r_middle_prox r_middle_dist r_ring_prox r_ring_dist r_pinkie_prox r_pinkie_dist
+
+[COUPLING]
+icub_hand_mk5 (0 1 2 3 4 5 6 7 8 9 10 11) (r_thumb_add r_thumb_oc r_index_add r_index_oc r_middle_oc r_ring_pinky_oc reserved reserved reserved reserved reserved reserved) (0.0 0.0 0.0 0.0 0.0 0.0) (100.0 90.0 15.0 90.0 90.0 90.0)
+
+[COUPLING_PARAMS]
+L0x      -0.0050 -0.0050 -0.0050 -0.0050  -0.0050
+L0y       0.0040  0.0040  0.0040  0.0040   0.0040
+q2bias   -173.35 -173.35 -173.35 -173.35  -170.53
+q1off       2.45    2.15    2.15    2.15     2.46
+k        0.03415 0.03913 0.03913 0.03913  0.03418
+d        0.03503 0.04003 0.04003 0.04003  0.03503
+l        0.00604 0.00604 0.00604 0.00604  0.00608
+b        0.00640 0.00640 0.00640 0.00640  0.00640
+
+[TRAJECTORY_GENERATION]
+trajectory_type minimum_jerk
+
+[LIMITS]
+jntVelMax  100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.0
+jntPosMax   100.0  90.0  101.4  15.0  90.0  103.1  90.0  103.1  90.0  103.1  90.0  98.7
+jntPosMin    0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+
+[POSITION_CONTROL]
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+kp           5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0   5.0
+kd           0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+ki           0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00  0.00
+maxInt       9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
+maxOutput    9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
+shift        0     0     0     0     0     0     0     0     0     0     0     0
+ko           0     0     0     0     0     0     0     0     0     0     0     0
+stictionUp   0     0     0     0     0     0     0     0     0     0     0     0
+stictionDwn  0     0     0     0     0     0     0     0     0     0     0     0
+
+[VELOCITY_CONTROL]
+controlUnits metric_units
+controlLaw   joint_pid_gazebo_v1
+velocityControlImplementationType integrator_and_position_pid
+kp           0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01  0.01
+kd           0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+ki           0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+maxInt       9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
+maxOutput    9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
+shift        0     0     0     0     0     0     0     0     0     0     0     0
+ko           0     0     0     0     0     0     0     0     0     0     0     0
+stictionUp   0     0     0     0     0     0     0     0     0     0     0     0
+stictionDwn  0     0     0     0     0     0     0     0     0     0     0     0
+
+[IMPEDANCE_CONTROL]
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+stiffness    0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
+damping      0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf
@@ -3122,7 +3122,7 @@
 </gazebo>
   <gazebo>
 <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
-<yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand.ini</yarpConfigurationFile>
+<yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
 </plugin>
 </gazebo>
   <gazebo reference="l_shoulder_pitch">
@@ -3154,7 +3154,7 @@
 </gazebo>
   <gazebo>
 <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
-<yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand.ini</yarpConfigurationFile>
+<yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
 </plugin>
 </gazebo>
   <gazebo reference="r_shoulder_pitch">