WBV Planning with repulsive forces produces "spiky" behavior

[Sharwin24]

Spiky Behavior

Some odd spiky paths when planning with WBV with a goal pose that the path needs to go through repulsive influence zones

Another example

---

Notes after meeting Dan Lynch

• Plot or visualize the time series data for the repulsive forces acting on each link.
• Verify that propagating a force on the outer surface of a link is correctly mapped to a joint torque "felt" from the force.
• Consider using ellipsoids to model links instead of using meshes since mesh quality may impact distance queries.
• Verify distance queries using meshes are smoothly transitioning as a mesh is in motion

This Pull Request: #31 implements the visualization of the individual forces and clearances on the links

Next Steps

A promising idea could be to change the robot representation from meshes to simple shapes that will have smooth transitions in distance to obstacles. There are many options but here are a few ideas:

• Ellipsoids
• Capsule (Spherocylinder)
• Cylinders
• 3D Oriented Bounding Box (OBB)