RSDK-8852 migrate motionplan to work on framesystems rather than individual frames

[biotinker]

This PR migrates motionplan to remove solverFrames, and in general have all algorithms operate on entire framesystems rather than single frames.

This allows new features such as enabling planning for multiple independent frames simultaneously, such as several arms moving near but independently from one another.

Most of the frame-specific motionplan functions and structures, such as constraints, now have a framesystem-specific counterpart.

The entire file for solver frames has been removed, and what little remains are now known as motionChains, of which a planner may have many and which are used to inform which frames may move but do not limit planning. This PR does not change the external public Plan() function, and motion calls should continue to work the same way as they have, with few exceptions. An example of such an exception would be that since all components can now move, one may move out of the way of another if need be, something not possible before now.

[raybjork]

From a high level this makes sense. I think the biggest question is how we want to talk about Segment vs SegmentFS. I guess I'm leaning to having both now?

[raybjork]

Not a fan of adding this here. This should be just for the constraints we expose through proto. You can always append to the Constraints with custom constraints after the fact

[biotinker]

Fair, I think I did this as a quick one-off to get some tests working. Will update before final review

[biotinker]

Actually, not sure on this.

Since this is the structure we use for our own consumption and passing around of constraints and initialization of actual constraint functions, these structures should I think be the union of what we support in proto, and what we support via things like extra. This allows us to better prototype what will eventually be added into proto. The alternative is creating a parallel data structure that we pass around to do the same thing, but which adds parameters, masks the putative new API, and makes it easier to forget things.

[raybjork]

Could consider making these interfaces to cut down on the need for duplication. But if the vision is we're moving away from the original versions then probably not necessary

[biotinker]

I think they would need to be generics, not just interfaces.
But yes this is a stepping stone towards the eventual removal of the originals

[raybjork]

Is this the solverframe code?

[biotinker]

No. This simply allows things to call nlopt, which requires a vector of floats.

What little code survives from solverFrame, is now motionChain

[nfranczak]

[q]: will this be capitalized in future work?
[q]: why not make this a slice in case there is are specific poses we would like to move through?

[biotinker]

It will indeed be eventually capitalized.

I don't think there's a need for this to take an array; when pre-planning is fully operational, calling pre-plans iteratively will yield the same result while giving the caller extra control.