search

robotology / icub-models

Official URDF and SDF models of the iCub humanoid robot.
Creative Commons Attribution Share Alike 4.0 International
33 stars 33 forks source link

Misalignment between soles frames and feet collisions (iCubGazeboV2_5) #31

Open lrapetti opened 4 years ago

lrapetti commented 4 years ago

Putting the robot on the ground on a dynamical simulator, I would expect the soles frames to be in contact with the ground (coordinate z equal to zero). However, this is not happening due to a misalignment between the collision and the feet frames

e.g.

feetConstraints

so an offset of $\approx 1cm$ is observed

lrapetti commented 4 years ago

This misalignment was observed also by @GiulioRomualdi, he probably can give more info about his experience.

traversaro commented 4 years ago

Are you sure that this is not due to the compliance of the contact model in Gazebo, rather then to a misalignment of the frames and collision shape of the model?

GiulioRomualdi commented 4 years ago

I think that the collision is placed under the sole (on the real iCub is the plastic part mounted under the foot) while the frame is placed on the top of the sole (on iCub the metallic part)

traversaro commented 4 years ago

I think that the collision is placed under the sole (on the real iCub is the plastic part mounted under the foot) while the frame is placed on the top of the sole (on iCub the metallic part)

If that is confirmed (but I think it would be useful to visualize the frames for example in RViz or https://github.com/OTL/urdf-viz to confirm), we have two options:

I think the preferred option depends on which configuration we use nowadays more on the real robot, if the one with the plastic part or without.

lrapetti commented 4 years ago

From the model we can also observe:

so we have 0.014-0.004=0.01 (1cm) distance on z-axis (plus the thickness of the collsion)

GiulioRomualdi commented 4 years ago

For instance in this commit I added a new frame in the center of the collision https://github.com/GiulioRomualdi/icub-models/commit/9f5bfe6e6fdac2672741f0c06d8e49b260c530d6

GiulioRomualdi commented 4 years ago

Hi @traversaro and @lrapetti do you think we can try to fix this problem?

Option A: Move the *_sole below the simplified collision box Option B: Move the collision up to align it with the meshes extracted from Creo (where there is just the metallic part, no plastic sole is currently model in Creo as far as I remember) and to align it with the plastic sole I think the preferred option depends on which configuration we use nowadays more on the real robot, if the one with the plastic part or without.

Regarding this, we are currently using Option A on our real hardware (we are using the plastic soles). Do you think we may try to move the _sole frame on the fake collision rectangle? I don't know how to do it but if it doesn't involve the usage of Creo I can try

traversaro commented 4 years ago

so we have 0.014-0.004=0.01 (1cm) distance on z-axis (plus the thickness of the collsion)

To clarify, that is not completely correct, as 0.014 refers to the center of the box, while to compute the location of the lower side of the box you need to also account for the box tickness, so the real distance is 0.014+0.001/2-0.004 .

GiulioRomualdi commented 4 years ago

This is just for me: 0.014+0.001/2-0.004 = 0.0105

traversaro commented 4 years ago

Regarding this, we are currently using Option A on our real hardware (we are using the plastic soles). Do you think we may try to move the _sole frame on the fake collision rectangle? I don't know how to do it but if it doesn't involve the usage of Creo I can try

Ok, I think as an initial step you can try to modify the l_sole location in your model and then we can look into how to handle that as part of the generation process. Just to understand, are we sure that the collision element sdf in Gazebo for the sole is correctly aligned with the plastic sole on the real robot?

Furthermore, are we always using the plastic/skin soles all the times on iCubGenova04 or it depends on the demo/experiments (for example, YOGA++ is using the plastic/skin sole?) If the usage of the sole depend from experiment to experiment, we should have have either two frames, or some kind of l_sole frame that changes its location.

lrapetti commented 4 years ago

Just to mention, I don't know which one is currently the "reliable" documentation, but for example in http://wiki.icub.org/wiki/ICub_Model_naming_conventions it is mentioned that the sole frame is placed on the metal sole so it should be updated accordingly.

GiulioRomualdi commented 4 years ago

Another possibility is to add a new frame.

@traversaro

For example, YOGA++ is using the plastic/skin sole?

Yes also there

GiulioRomualdi commented 4 years ago
Current Situation What I'm proposing
l_sole new_lsole

Note that the new l_sole is placed under in the lower side of the box

new_lsole_bottom

In order to change position of l_sole as presented in the figure, I applied the following patch to the urdf model

--- a/iCub/robots/iCubGazeboV2_5/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5/model.urdf
@@ -1998,7 +1998,7 @@
   </joint>
   <link name="l_sole"/>
   <joint name="l_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.015000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="l_foot"/>
     <child link="l_sole"/>
     <dynamics damping="0.1"/>

Here the z position of the new frame moves from 0.004 to 0.015, indeed The position of the box w.r.t. the origin of l_foot is: https://github.com/robotology/icub-models/blob/bd7eeb19ed781dc55a6189f47247d621301603c4/iCub/robots/iCubGazeboV2_5/model.urdf#L528 while the size of the box is: https://github.com/robotology/icub-models/blob/bd7eeb19ed781dc55a6189f47247d621301603c4/iCub/robots/iCubGazeboV2_5/model.urdf#L530

Here I assumed (differently from what @traversaro wrote in https://github.com/robotology/icub-models/issues/31#issuecomment-696799028) that the the frame of the box is placed on the upper side of the box. So the new z-position of the frame is 0.014 + 0.01 = 0.015.

I also tried to follow @traversaro suggestions and put the frame in 0.014 + 0.005 = 0.0145 however from the gazebo visualization there seems that the frame is placed inside the box.

I think that the answer of where the frame of the collision is placed in written somewhere here,

GiulioRomualdi commented 4 years ago

My fault as written in the documentation

size attribute contains the three side lengths of the box. The origin of the box is in its center

This is the right patch

--- a/iCub/robots/iCubGazeboV2_5/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5/model.urdf
@@ -1998,7 +1998,7 @@
   </joint>
   <link name="l_sole"/>
   <joint name="l_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.014500000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="l_foot"/>
     <child link="l_sole"/>
     <dynamics damping="0.1"/>
GiulioRomualdi commented 4 years ago

Here I copy the ROS documentation:

The coordinate frame called l_sole defines the position and orientation of the left leg support body. The (X,Y) plane of this frame must be coplanar with the support body contact surface. It is recommended to choose the origin as the projection of the support body origin on the contact surface. The Z coordinate of this frame should be on the ground level when standing.

The coordinate frame called r_sole defines the equivalent frame for the right leg.

There seems that the frame should be on the ground level when standing and right now it is not like this.

traversaro commented 4 years ago

For example, YOGA++ is using the plastic/skin sole?

Yes also there

To clarify, the DIC group is not using the foot skin patch and its plastic holder, but it is using a different plastic sole without any skin, did I understood correctly this?

@prashanthr05 this could explain some strange results on the floating base estimation with odometry of the past.

traversaro commented 4 years ago

As mentioned by @lrapetti in http://wiki.icub.org/wiki/ICub_Model_naming_conventions the definition of the l_sole frame is indeed clear and based on that it is implemented (I hope correctly) in the CAD model. For that regards the REP-120 document linked by @GiulioRomualdi, indeed the l_sole frame was aligned with the contact plane back in 2015 when the frame was defined, but it stopped being aligned with the contact frame when we started using an additional plastic sole for doing experiments. However, changing the l_sole definition is not backward compatible and may be technically tricky, as it requires changing the CAD model and regenerate the model, and actually people can still do experiments with the other soles. For this reason, even if it is not follow exactly REP-120, I think it could be a good idea to define a frame *_sole_contact that is actually aligned with any additional plastic or other material sole is actually mounted in the robot, that we can easily add in icub-model-generator as Z-offset w.r.t. to the l_sole frame defined in the CAD, that will continue to refer to the metal sole. To simplify, I will refer in the following as metal-sole as the raw metal sole, soft-skin-sole with the sole covered with soft sensorized skin and rubber-sole with the sole that is currently used in most experiments on iCubGenova04 .

traversaro commented 4 years ago

An additional problem is that the rubber-sole variant is the only one for which we are actually testing all the balancing and walking software, and so we need to make sure that the iCub production group is fully aware of this variant, otherwise new iCub may be produced with the wrong assumption that they can be used to reproduce the results as seen on iCubGenova04 .

traversaro commented 4 years ago

To avoid mixing stuff up, I think in this issue we should only address P2, as defined in the previous comment.