ergoCubV1*: fix alignment of the depth (#188)

icub-tech-iit · Nov 9, 2023 · 91dfd1a · 91dfd1a
2 parents f7ba46b + a422848
commit 91dfd1a
Show file tree

Hide file tree

Showing 14 changed files with 59 additions and 58 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### urdf
 - Fix location of `l/r_sole` frames(https://github.com/icub-tech-iit/ergocub-software/issues/186)
+- Fix alignment of depth to the RGB frame(https://github.com/icub-tech-iit/ergocub-software/issues/183)
 
 ## [0.5.0] - 2023-10-26
 

diff --git a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options.yaml b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options.yaml
@@ -816,7 +816,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: SCSYS_HEAD_DEPTH
+  - frameName: SCSYS_HEAD_RGB
     linkName: realsense
     sensorName: realsense_head_depth
     sensorType: "depth"
@@ -825,7 +825,7 @@ sensors:
     - |
         <camera name="intel_realsense_depth_camera">
           <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-          <horizontal_fov>1.57079</horizontal_fov>
+          <horizontal_fov>1.2217</horizontal_fov>
           <distortion>
             <k1>0</k1>
             <k2>0</k2>

diff --git a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml
@@ -812,7 +812,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: SCSYS_HEAD_DEPTH
+  - frameName: SCSYS_HEAD_RGB
     linkName: realsense
     sensorName: realsense_head_depth
     sensorType: "depth"
@@ -821,7 +821,7 @@ sensors:
     - |
         <camera name="intel_realsense_depth_camera">
           <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-          <horizontal_fov>1.57079</horizontal_fov>
+          <horizontal_fov>1.2217</horizontal_fov>
           <distortion>
             <k1>0</k1>
             <k2>0</k2>

diff --git a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml
@@ -812,7 +812,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: SCSYS_HEAD_DEPTH
+  - frameName: SCSYS_HEAD_RGB
     linkName: realsense
     sensorName: realsense_head_depth
     sensorType: "depth"
@@ -821,7 +821,7 @@ sensors:
     - |
         <camera name="intel_realsense_depth_camera">
           <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-          <horizontal_fov>1.57079</horizontal_fov>
+          <horizontal_fov>1.2217</horizontal_fov>
           <distortion>
             <k1>0</k1>
             <k2>0</k2>

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml
@@ -814,7 +814,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: SCSYS_HEAD_DEPTH
+  - frameName: SCSYS_HEAD_RGB
     linkName: realsense
     sensorName: realsense_head_depth
     sensorType: "depth"
@@ -823,7 +823,7 @@ sensors:
     - |
         <camera name="intel_realsense_depth_camera">
           <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-          <horizontal_fov>1.57079</horizontal_fov>
+          <horizontal_fov>1.2217</horizontal_fov>
           <distortion>
             <k1>0</k1>
             <k2>0</k2>

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml
@@ -1031,7 +1031,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: SCSYS_HEAD_DEPTH
+  - frameName: SCSYS_HEAD_RGB
     linkName: realsense
     sensorName: realsense_head_depth
     sensorType: "depth"
@@ -1040,7 +1040,7 @@ sensors:
     - |
         <camera name="intel_realsense_depth_camera">
           <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-          <horizontal_fov>1.57079</horizontal_fov>
+          <horizontal_fov>1.2217</horizontal_fov>
           <distortion>
             <k1>0</k1>
             <k2>0</k2>

diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml
@@ -1144,7 +1144,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: SCSYS_HEAD_DEPTH
+  - frameName: SCSYS_HEAD_RGB
     linkName: realsense
     sensorName: realsense_head_depth
     sensorType: "depth"
@@ -1153,7 +1153,7 @@ sensors:
     - |
         <camera name="intel_realsense_depth_camera">
           <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-          <horizontal_fov>1.57079</horizontal_fov>
+          <horizontal_fov>1.2217</horizontal_fov>
           <distortion>
             <k1>0</k1>
             <k2>0</k2>

diff --git a/urdf/ergoCub/conf/gazebo_ergocub_depth_camera.ini b/urdf/ergoCub/conf/gazebo_ergocub_depth_camera.ini
@@ -2,7 +2,7 @@ disableImplicitNetworkWrapper
 yarpDeviceName ergocub_depth_camera
 
 [CAMERA_PARAM]
-focalLengthX 570.3422241210938
-focalLengthY 570.3422241210938
+focalLengthX 389.453
+focalLengthY 389.453
 tangentialPointX 0.0
 tangentialPointY 0.0
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf
@@ -3377,10 +3377,10 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30</update_rate>
-      <pose>0.00751550026595621 0.010000000000000005 -4.911144457775407e-08 -1.5707962931078618 8.881784197001252e-16 -1.5707963267948954</pose>
+      <pose>0.00751550026595621 -0.0115 -4.911144457775407e-08 -1.5707962931078618 8.881784197001252e-16 -1.5707963267948954</pose>
       <camera name="intel_realsense_depth_camera">
   <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-  <horizontal_fov>1.57079</horizontal_fov>
+  <horizontal_fov>1.2217</horizontal_fov>
   <distortion>
     <k1>0</k1>
     <k2>0</k2>
@@ -3407,7 +3407,7 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5707962931078618 8.881784197001252e-16 -1.5707963267948954" xyz="0.00751550026595621 0.010000000000000005 -4.911144457775407e-08"/>
+    <origin rpy="-1.5707962931078618 8.881784197001252e-16 -1.5707963267948954" xyz="0.00751550026595621 -0.0115 -4.911144457775407e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">

diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
@@ -1241,8 +1241,8 @@
     </collision>
   </link>
   <joint name="l_index_add" type="revolute">
-    <origin xyz="0.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
-    <axis xyz="1.1895336853461886e-15 0.9999999999999999 -1.9812040713213337e-15"/>
+    <origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
+    <axis xyz="1.3005559878087045e-15 1.0000000000000002 -1.953448495705706e-15"/>
     <parent link="l_hand_palm"/>
     <child link="l_hand_index_1"/>
     <limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
@@ -1251,7 +1251,7 @@
   <link name="l_hand_index_1">
     <inertial>
       <mass value="0.01923264"/>
-      <origin xyz="0.0046807100979840005 0.0038510231744863237 -0.018526526203731575" rpy="0 -0 0"/>
+      <origin xyz="0.0046807100979840005 0.0038510231744862677 -0.018526526203731464" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="1.4548370851701976e-7" ixz="1.4373172964405972e-7" iyy="0.01" iyz="-6.425403568227921e-7" izz="0.01"/>
     </inertial>
     <visual>
@@ -1571,8 +1571,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
-    <axis xyz="-1 8.326672684688522e-17 7.023654435833748e-10"/>
+    <origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
+    <axis xyz="-1.0000000000000002 2.7755575615627613e-17 7.023654366444809e-10"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1604,8 +1604,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
-    <axis xyz="-1 -2.359224176539984e-16 4.163337069211178e-17"/>
+    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
+    <axis xyz="-1 -1.804112664227406e-16 9.020562801946248e-17"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
@@ -1614,7 +1614,7 @@
   <link name="l_hand_index_3">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="-0.018787329483201345 0.0027546711703011484 -0.018853294932697662" rpy="0 -0 0"/>
+      <origin xyz="-0.018787329483201373 0.002754671170301093 -0.018853294932697606" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="4.65463304290381e-8" ixz="2.5624287052698965e-7" iyy="0.01" iyz="-2.891156821380605e-7" izz="0.01"/>
     </inertial>
     <visual>
@@ -3453,10 +3453,10 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-        <horizontal_fov>1.57079</horizontal_fov>
+        <horizontal_fov>1.2217</horizontal_fov>
         <distortion>
           <k1>0</k1>
           <k2>0</k2>
@@ -3483,7 +3483,7 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">

diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
@@ -1133,8 +1133,8 @@
     </collision>
   </link>
   <joint name="l_index_add" type="revolute">
-    <origin xyz="0.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
-    <axis xyz="1.1895336853461886e-15 0.9999999999999999 -1.9812040713213337e-15"/>
+    <origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
+    <axis xyz="1.3005559878087045e-15 1.0000000000000002 -1.953448495705706e-15"/>
     <parent link="l_hand_palm"/>
     <child link="l_hand_index_1"/>
     <limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
@@ -1143,7 +1143,7 @@
   <link name="l_hand_index_1">
     <inertial>
       <mass value="0.01923264"/>
-      <origin xyz="0.0046807100979840005 0.0038510231744863237 -0.018526526203731575" rpy="0 -0 0"/>
+      <origin xyz="0.0046807100979840005 0.0038510231744862677 -0.018526526203731464" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="1.4548370851701976e-7" ixz="1.4373172964405972e-7" iyy="0.01" iyz="-6.425403568227921e-7" izz="0.01"/>
     </inertial>
     <visual>
@@ -1433,8 +1433,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
-    <axis xyz="-1 8.326672684688522e-17 7.023654435833748e-10"/>
+    <origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
+    <axis xyz="-1.0000000000000002 2.7755575615627613e-17 7.023654366444809e-10"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1463,8 +1463,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
-    <axis xyz="-1 -2.359224176539984e-16 4.163337069211178e-17"/>
+    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
+    <axis xyz="-1 -1.804112664227406e-16 9.020562801946248e-17"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
@@ -1473,7 +1473,7 @@
   <link name="l_hand_index_3">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="-0.018787329483201345 0.0027546711703011484 -0.018853294932697662" rpy="0 -0 0"/>
+      <origin xyz="-0.018787329483201373 0.002754671170301093 -0.018853294932697606" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="4.65463304290381e-8" ixz="2.5624287052698965e-7" iyy="0.01" iyz="-2.891156821380605e-7" izz="0.01"/>
     </inertial>
     <visual>
@@ -3273,10 +3273,10 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30.000000</update_rate>
-      <pose>0.00751548 0.01 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
+      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
         <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-        <horizontal_fov>1.57079</horizontal_fov>
+        <horizontal_fov>1.2217</horizontal_fov>
         <distortion>
           <k1>0</k1>
           <k2>0</k2>
@@ -3303,7 +3303,7 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 0.01 1.73521e-08"/>
+    <origin rpy="-1.5708 3.75968e-16 -1.5708 " xyz="0.00751548 -0.0115 1.73521e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">

diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf
@@ -3377,10 +3377,10 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30</update_rate>
-      <pose>0.00751550026595621 0.010000000000000005 -4.911144457775407e-08 -1.5707962931078618 8.881784197001252e-16 -1.5707963267948954</pose>
+      <pose>0.00751550026595621 -0.0115 -4.911144457775407e-08 -1.5707962931078618 8.881784197001252e-16 -1.5707963267948954</pose>
       <camera name="intel_realsense_depth_camera">
   <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-  <horizontal_fov>1.57079</horizontal_fov>
+  <horizontal_fov>1.2217</horizontal_fov>
   <distortion>
     <k1>0</k1>
     <k2>0</k2>
@@ -3407,7 +3407,7 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5707962931078618 8.881784197001252e-16 -1.5707963267948954" xyz="0.00751550026595621 0.010000000000000005 -4.911144457775407e-08"/>
+    <origin rpy="-1.5707962931078618 8.881784197001252e-16 -1.5707963267948954" xyz="0.00751550026595621 -0.0115 -4.911144457775407e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">

diff --git a/urdf/ergoCub/robots/ergoCubSN000/model.urdf b/urdf/ergoCub/robots/ergoCubSN000/model.urdf
@@ -3377,10 +3377,10 @@
     <sensor name="realsense_head_depth" type="depth">
       <always_on>1</always_on>
       <update_rate>30</update_rate>
-      <pose>0.00751550026595621 0.010000000000000005 -4.911144457775407e-08 -1.5707962931078618 8.881784197001252e-16 -1.5707963267948954</pose>
+      <pose>0.00751548 -0.0115 1.73521e-08 -1.5708 3.75968e-16 -1.5708 </pose>
       <camera name="intel_realsense_depth_camera">
   <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
-  <horizontal_fov>1.57079</horizontal_fov>
+  <horizontal_fov>1.2217</horizontal_fov>
   <distortion>
     <k1>0</k1>
     <k2>0</k2>
@@ -3407,7 +3407,7 @@
   </gazebo>
   <sensor name="realsense_head_depth" type="depth">
     <parent link="realsense"/>
-    <origin rpy="-1.5707962931078618 8.881784197001252e-16 -1.5707963267948954" xyz="0.00751550026595621 0.010000000000000005 -4.911144457775407e-08"/>
+    <origin rpy="-1.5707962931078618 8.881784197001252e-16 -1.5707963267948954" xyz="0.00751550026595621  -0.0115 -4.911144457775407e-08"/>
   </sensor>
   <gazebo reference="realsense">
     <sensor name="realsense_head_rgb" type="camera">