Fit b spline se3

[myeatman-bdai]

Add functionality to fit a cubic-SE3-B-spline to data.

[myeatman-bdai]

Converted to draft because this uses scipy functionality not available in the version associated with the dependency list for 3.7 python.

[petercorke]

Not sure why this is needed. You would change the code pattern from:

x = SO3(R1)
x = SO3(R2)

to

x = SO3(R1)
x.R = R2

It's no shorter, clearer, or cheaper. The second line has a hidden SO3 constructor and like the first option leaves an object to be garbage collected. It also makes the check for matrix validity happen, which is in principle a good thing, but is expensive. If an SMTB module knows that the matrix belongs to SO(3) then that check can be circumvented by

x = SO3(R1, check=False)
x = SO3(R2, check=False)

This is a "feature" user's can also use but at their own peril. In your interpolator you know that the matrix you compute is in SO(3).

[petercorke]

Given the usefulness of the functionality, and their independence from the rest of the code, nothing in SMTB depends on these, the risk is low, and should be added. But see comments below.

[petercorke]

These are a really useful addition. The control poses

    def __init__(
         self,
         control_poses: List[SE3],
     ) -> None:

are passed as a list of SE3s not as a multi-element SE3. What's the thinking there?

There are multiple interpolator classes, each algorithm specific. Would it be better to have one interpolator class with a passed argument to say which one is used. This is kind of the approach that SciPy takes with optimisers and integrators that have a method parameter. Simplest approach would be a wrapper class that instantiates one of the specific classes, all of them with a common parent class.

[petercorke]

@myeatman-bdai a few more thoughts on this PR:

• be great to have some more user level documentation and runblock examples, and also to integrate it into the ReST documentation, I can't find the current spline classes by search or table of contents. I'm happy to help out here, but probably need a bit more to go on than what's there (actually the unit tests might be a start for me)
• there's a kinematic/dynamic constrained trajectory generator TOPPRA that might be useful to import, wrap or just play nicely with. I've not used it.