#fix correct calculation of the center of cuboid_hull

[crush0441]

When the corner points of link_cuboid_hulls have the same sign, this error will occur.

Visual model;

raw code collision model :

modified code collision model:

[kjyv]

I'm not sure this is the right fix (it might give problems with other models). The question is why is one of your bounding boxes oriented wrong while the others are the way they should be. I think the link geometry in the URDF file might be defined wrongly which results in a wrong bounding box (maybe using rotation or position tags? These are not used at the moment, so they should always be defined from the link origin - there is a TODO in the code about that). The sign is thrown away because the bounding box is supposed to always extend in positive direction as a chain is built up.

[crush0441]

@kjyv
Some of my links are in the right places because their coordinate are in the bounding box.

This problem happens ONLY when the link coordinate is out of the bounding box. So the edge points have the same sign.

whole robot

coordinate in the bound box

coordinate out of the bound box

[kjyv]

I see, that's a bit unusual to have the link origin away from the actual body. Is there a reason to do that?
Your patch might work for your case, but it incorrectly calculates the center point as (b[1][0])+(b[0][0]) + p[0], .... p is the link geometry position, b are the min and max coordinate values for each axis of the bounding box. I'm not sure why I added the abs calls. But it should be more like 0.5*(b[1][0]-b[0][0])+p[0]. I don't see why adding the min and max would be correct, also halving the geometry position is probably wrong. Also it is possible that the geometry position or bounding box is not determined correctly in your case. I can't really test this at the moment to fix it myself, sorry.