Simon-Steinmann-nested-Transform-fix

[Simon-Steinmann]

Fixes the issue detailed in #77

[DavidMansolino]

Looks good thank you.

[jgueldenstein]

@Simon-Steinmann have you tested this with translated collision objects only or also with rotated collision objects?
We had some issues with bounding boxes for rotated collision objects.

[Simon-Steinmann]

@jgueldenstein looking at the code, it should include the translation and rotation. Make sure you have the absolute newest version of urdf2webots installed. There have been many updates last year.

[jgueldenstein]

@Simon-Steinmann Thank you for your quick response, I had some issues when I worked on it in November (I am not sure if I used the newest version back then to be honest). I can't reproduce these issues and it seems to be working correctly. All bounding boxes are calculated correctly. Sorry to bother you.

[jgueldenstein]

@Simon-Steinmann I was able to reproduce the issue, it occurs when there are multiple collision models in a single link. I have a solution that works for this case but I would need to test further if it breaks other cases.

[Simon-Steinmann]

@jgueldenstein And these multiple collision models have different translations and rotations? Maybe you can share your urdf file.

[jgueldenstein]

@Simon-Steinmann Surely: https://github.com/bit-bots/wolfgang_robot/blob/f4e18ccf2e2b6596a13cead013e56b54c89f3361/wolfgang_description/urdf/robot.urdf
Please excuse the lack of indentation, it is auto generated.

An example link with multiple collision models that is probably easier to talk about than the torso link (~40 collision models) is this: https://github.com/bit-bots/wolfgang_robot/blob/f4e18ccf2e2b6596a13cead013e56b54c89f3361/wolfgang_description/urdf/robot.urdf#L327

When running the urdf conversion script:
python urdf2webots/demo.py --input wolfgang_old.urdf --output protos --multi-file --box-collision --multi-file --disable-mesh-optimization

I get the following result in the simulator

The only Bounding box that is drawn correctly is the one around the "shoulder_connector.stl" which has a rotation of basically zero <origin xyz="0 0 2.77556e-17" rpy="2.4048e-34 -1.72563e-31 -1.5783e-33" /> in the URDF

And the corresponding part in the proto file:

name "r_shoulder"
boundingObject Group {
  children [
    Transform {
      translation -0.025950 0.042000 0.000000
      rotation -0.577350 -0.577352 0.577350 2.094399
      children [
        Transform {
          translation -0.025950 0.042000 0.002250
          rotation -0.577350 -0.577352 0.577350 2.094399
          children [
            Box {
              size 0.050000 0.050000 0.004500
            }
          ]
        }
      ]
    }
    Transform {
      translation -0.016950 0.042000 0.000000
      rotation -0.577350 -0.577352 0.577350 2.094399
      children [
        Transform {
          translation -0.016950 0.042000 0.004500
          rotation -0.577350 -0.577352 0.577350 2.094399
          children [
            Box {
              size 0.050000 0.050000 0.009000
            }
          ]
        }
      ]
    }
    Transform {
      translation 0.000000 0.022500 0.000000
      rotation 0.000000 1.000000 0.000000 0.000000
      children [
        Box {
          size 0.063900 0.045000 0.034000
        }
      ]
    }
  ]
}

Webots also displays the following info:
INFO: DEF Robot Wolfgang (PROTO) > HingeJoint > Solid: A child to the Transform placed in 'boundingObject' is expected.

[Simon-Steinmann]

The nested transforms seem wrong. Seems only to affect meshes.

[Simon-Steinmann]

Try changing line 186 of writeProto.py to
if not boxCollision and (boundingObject.position != [0.0, 0.0, 0.0] or boundingObject.rotation[3] != 0.0):

[jgueldenstein]

When only changing this line, the bounding boxes are generated but incorrectly as seen here

I have a solution where I also applied this but also adapted the calculations for the bounding boxes.
jgueldenstein@f246831

The bounding boxes seem to be calculated correctly:

[jgueldenstein]

@Simon-Steinmann i have put this in a new issue for further discussion.