Resolve sampling interface for point TSRs about aikido

[cdellin]

Note: this issue arose out of a discussion on #13.

[jeking04]

I think I am also in favor of the separate PoseConstraint class for point TSRs. I agree with @cdellin we should be sure to document well for users who extensively use point TSRs. But I can imagine there will also be a large body of users who don't know anything about TSRs that may want to define a PoseConstraint. By having it separate, we can use an interface that doesn't require you learn TSRs to use. There is a bit of a usability drawback, in that often we may start with a point TSR and then realize we can/need to allow more freedom. This will require completely changing class types rather than just updating Bw bounds. But this is very minor.

[psigen]

I had some ideas in the past about a ConstraintBuilder helper class that would make it easier to construct constraints, like:

ConstraintBuilder builder
ProjectableConstraint constraint = builder.between(link1, link2)
                                          .enforcePose(T_l1_l2)
                                          .allowYaw(-pi/2, pi/2)
                                          .allowZ(-0.2, 0.2)
                                          .build()

If you were using something like that, you could have it automatically return the correct constraint type between PoseConstraint and TSRConstraint. But it's not fully baked yet.

[gilwoolee]

I agree with both @jeking04 and @cdellin 's comments above. My original implementation was built with the assumption that I (or someone) will implement a separate pointConstraint or PoseConstraint soon.

I have some questions:

1. PointTSR might have infinite number of IK solutions (at least theoretically), or may have only 1 IK solution. From my understanding, dart will return only 1 solution, and many other IKsolvers might also return only a finite number of solutions. How shall we address this in getNumSamples() and CanSample()? Should we solve IK once, generate all possible samples, and iteratively return the next one until we exhaust all the samples?

2. Do we have any other finite-set sampleable constraint class we should be considering?

[cdellin]

Thanks @jeking04! (-:

I agree with everything said, with one caveat. Like @lgw903 mentioned, I certainly agree that a separate PoseConstraint class is a good idea, and can be used by people who don't care about TSRs. My main argument here is that if someone does create a point TSR (since it is common for TSR people), it should behave correctly (i.e. as a finite sequence). Imagine a user who is translating a library of existing TSRs to use aikido -- it would be unfortunate if during the translation they have to discover which are point TSRs themselves and manually convert them to a different class. (And if we want to force this behavior as in the status quo, we should at least document it correctly).

@lgw903, your 1) is the question I was trying to get at in #23. A PoseConstraint (or a point TSR) should behave as a finite sequence of a single sample. But if such a pose sampleable is used in an IK adapter such as IkSampleableConstraint, we need to write those adapters so they also obey the sampleable API (i.e. behave finite if they are actually finite, and infinite if they are actually infinite).

In terms of other finite sampleables, I think it would be beneficial to have an explicit set-of-poses class (e.g. plan to any of these three equally-good grasps). This could even be a general-purpose combiner.

[mkoval]

And if we want to force this behavior as in the status quo, we should at least document it correctly.

The user will receive a warning when they attempt to sample from a TSR that alerts them to this. I think this is an acceptable compromise for now. We can revisit this decision if it becomes too burdensome to convert point TSRs to PoseConstraints.

A PoseConstraint (or a point TSR) should behave as a finite sequence of a single sample.

I agree. A PoseConstraint fundamentally contains one unique sample.

But if such a pose sampleable is used in an IK adapter such as IkSampleableConstraint, we need to write those adapters so they also obey the sampleable API (i.e. behave finite if they are actually finite, and infinite if they are actually infinite).

I'm not sure precisely what you mean by this. The only contract imposed by the API is that getNumSamples() should return an upper bound on the number of times sample() may return a non-empty value. The IkSampleableConstraint class implemented in #13 satisfies this requirement.

This does not mean that IkSampleableConstraint, which finds at most one solution for each pose sample, is the end of the story. We may want to have other adapters that implement more sophisticated logic. For example: we could use an analytic IK solver to enumerate all IK solutions for one sample before trying the next one. Or a hybrid approach that enumerates IK solutions when getNumSamples() != NO_LIMIT.

In terms of other finite sampleables, I think it would be beneficial to have an explicit set-of-poses class (e.g. plan to any of these three equally-good grasps). This could even be a general-purpose combiner.

👍 for a general-purpose combiner for Constraint<T>. Another obvious addition is a ConfigurationContraint class that contains one VectorXd sample. This gives us PlanToConfiguration and PlanToConfigurations (via the combiner) for free on planners that support sampleable goal sets.

[cdellin]

I'm of course fine with whatever the consensus decision is on these issues (this and #23) -- I'm just bringing up the aspects that initially confused me, given my background as someone who writes planners and used existing TSR-based planners.

I agree that your description of the Sampleable API/contract (that getNumSamples() must simply return a bound on the number of times sample() may return a value) is satisfied by the present implementation. But from my view such an API doesn't seem very useful to a planner. Here are some properties that I think might be useful:

1. Correctness: all samples lie on the constraint (this is obvious).
2. Coverage (in probability): given enough samples, you will eventually get within epsilon of any point on the constraint. This is important for things like probabilistic completeness.
3. Uniqueness: no samples will repeat (modulo machine precision issues). This is useful so a planner doesn't need to internally de-duplicate samples before e.g. adding them to a roadmap.
4. Measure. Some signal about the dimension and/or volume of constraint. This would help the planner to plan how it should sample. One such measure is whether the constraint is finite or infinite.

Note that the present implementation of IkSampleableConstraint breaks 2 when used with finite pose constraints (#23), and the current implementation of TSR breaks 3 and 4 when the TSR is a point TSR (warning message notwithstanding)

I understand that the current contact only guarantees 1, but I also think 2, 3, and 4 can also be important for planners, and I think it behooves us to attempt to address them. Furthermore, the fact that the current API enforces a finiteness distinction on the generator which is not related to the finiteness of the constraint itself bothers me (that is, it can return both a finite generator for an infinite constraint, and an infinite generator for a finite constraint).

[psigen]

Hmm, I'm starting to see some similarities here. In both PoseConstraint and TSRConstraint, we have the problem that there could be one or many solutions to the constraint depending on a few conditions:

PoseConstraint

• 6-DOF or fewer ➡️ zero or one solutions

• 6-DOF with aligned joints ➡️ zero, one, or infinite solutions*

• 6-DOF with non-aligned joints ➡️ zero or infinite solutions

TSRConstraint

• Point TSR ➡️ Above list
• Non-Point TSR ➡️ Above list for each TSR sample

Given that these can vary per robot, or even within a certain robot* you could easily end up having two TSR samples with one IK solution, then a TSR solution with infinite IK solutions, then a few more with one solution.

I am not sure how we want to handle this.

* A robot like the UR10 actually has a redundant DOF (infinite solutions) when the wrist aligns with the shoulder and elbow along a particular plane, but no redundancy (single solution) otherwise.

[egordon]

Addressed by #13