[Feature]: Motion planning for rigid entities with free joints.

[Bingkun-Huang]

What feature or enhancement are you proposing?

The plan_path() is not yet supported for robots with free joints (like Tiago with moving based and manipulator)

Potential Benefit

So that in Genesis could simulate with more diverse robots

What is the expected outcome of the implementation work?

Use existed libraries for planning