3D Obstacle Avoidance for Convex Shapes

[augustsb]

Hi,

I'm wondering if anyone knows any libraries that support casadi that can be used for 3D collision detection and obstacle avoidance? What I want to do is to represent the swept volume of my robot as a box and I would like to use convex polytopes as obstacles, but it seems this can't be done easily in casadi?
Alternatively, does anyone know any other optimization tools other than casadi that will be better for this?

[charleskhazoom]

There is The Flexible Collision Library:
https://github.com/flexible-collision-library/fcl

There is also an extension to it which seems to be improved and still maintained :

https://github.com/humanoid-path-planner/hpp-fcl

For obstacle avoidance, depending on your chosen method, you may need the gradients of the signed-distance functions. This is often not readily differentiable except for spheres. Thus, it does not necessarily work well with Casadi. But approximations such as the ones used here work well in practice : https://arxiv.org/abs/2207.00692

[augustsb]

Ok, but is it possible to use FCL to compute the distances as external function and then somehow approximate the derivatives using finite differences or something?

[charleskhazoom]

FCL can give you the distances between two shapes as well as the closest points on those shapes. Once you have the closest points, the approximations for computing J_AB in https://arxiv.org/abs/2207.00692 can be applied. To compute J_A, you could use Casadi's jacobian function to differentiate the closest point value on the object with respect to the pose of the object.