It seems that the origin of the first collision box of the torso is not correct. I expect it to be ```xml <origin rpy="0 0 0" xyz="0 0 0.25" /> ``` https://github.com/pal-robotics/tiago_robot/blob/31790ca2cd84b4f591ff22b497127e0129bfc4ca/tiago_description/urdf/torso/torso.urdf.xacro#L46 This is related to https://github.com/pal-robotics/pmb2_robot/issues/6