Implement RRT* planner

[villekuosmanen]

🚀 Feature

RRT* is an optimal motion planning algorithm. It is much slower than RRTConnect but creates better paths.

I have a version working in a fork, will be happy to share it. But the agent code needs to be adjusted to choose what planner to use - RRT* should only be used for operations that run less frequently. For example:

# Create two motion planners: RRTConnect is quick but suboptimal, RRTStar is slow but optimal
self.quick_motion_planner = SimplifyXYT(
    Shortcut(
        RRTConnect(self.planning_space, self.planning_space.is_valid)
    ),
    min_step=0.05, max_step=0.2, num_steps=4,
)
self.optimal_motion_planner = SimplifyXYT(
    Shortcut(
        RRTStar(self.planning_space, self.planning_space.is_valid, max_iter=100)
    ),
    min_step=0.05, max_step=0.2, num_steps=4,
)

Assigning to myself, will get a PR up when I have time.

Motivation

Additional context

[villekuosmanen]

@cpaxton following up from the Twitter convo

[villekuosmanen]

#504

Would one of the maintainers mind testing this with the home robot agents? have tested with my separate agents, but not here.