Clean up robot/util.

[sniyaz]

This PR cleans up robot/util by deleting all of the planTo* methods. This requires us to rewrite the corresponding methods in ConcreteRobot and ConcreteManipulator to reroute through the new Planner API.

This PR also removes all CRRT functionality in both ConcreteRobot and ConcreteManipulator. This will be added back later once we actually decide how to integrate CRRT and AIKIDO. None of our demo code should rely on CRRT, so this is fine.

NOTE: This PR depends on #587 by @brianhou, due to the new planner adapter.

---

Before creating a pull request

• Document new methods and classes
• Format code with make format

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md
• Add unit test(s) for this change

[brianhou]

Found some time for a quick review! Broad strokes seem solid, although I'd also like to see a corresponding libada PR before merging.

[brianhou]

For some reason, I thought that we take the goal state as an Eigen::VectorXd instead. Is there a reason we take a State? What happens if this isn't actually a MetaSkeletonStateSpace::State?

[sniyaz]

Yeah, this is something I noticed as well. I'm not sure why we decided to take StateSpace::State* instead of MetaSkeletonStateSpace::State*. Since we're working with ConcreteRobot we must be working with MetaSkeletonStateSpace::State* (unless the user is doing something very stupid).

I agree that this design decision is a little funky (especially considering that we take a metaSkeleton as another argument), but changing it would break a lot of things just because of type issues. Maybe we can file an issue to clean this up later?

[brianhou]

@aditya-vk does this seem ok for now?

[sniyaz]

@brianhou I tried to handle #447 by enforcing that any DART planner (and only a DART planner) has to satisfy these two invariants when plan() is called:

1. The MetaSkeleton is locked during planning.
2. The MetaSkeleton is reset after planning.

The only DART planner class that didn't need to be updated was the VFP offset planner (since the core VFP code does both of these anyway). LMK what you think of this solution: if we're happy with it, I can document this new invariant for DART planners somewhere.

This also means we don't have to worry about locking or saving state anywhere in ConcreteRobot or ConcreteManipulator, which is nice. I've taken that out.

Also, for what it's worth I tested simple_trajectories with all of these changes and things seem to work fine (so at least planToConfiguration isn't broken).

[brianhou]

Seems generally fine to me, but would like @aditya-vk's eyes on this too.

[aditya-vk]

Looks good to me. Has it been tested through libherb and herb3_demos to ensure everything works fine?

[sniyaz]

@aditya-vk Yup! The only other thing we're waiting on are the corresponding ADA changes. I'll chat with @egordon about those.

[aditya-vk]

LGTM

[aditya-vk]

Thanks for the checks with ADA and HERB!