one-model-at-a-time IK is common, very easy to get wrong

[ggould-tri]

In TRI's Anzu project, I have found a very large number of reimplementations of multibody plant IK of one model at a time; more interestingly, each implementation has had one or more bugs, so I think putting it in one canonical, correct form in Drake would have a big payoff.

The pattern is: From a MBP, perform IK on a single model while treating all other models as fixed (not adustable, but also not enforcing their joint limits), then return an MBP state with that single model adjusted and the other models' states unchanged.

This one-model-at-a-time IK is not generally used in live robot control, but is implemented in perhaps half a dozen unit tests and calibration systems, usually cases that do not initialize the other models in the scene. The particular implementations have bugs like:

• Enforcing other models' joint limits but not initializing them to limit-obedient states
• Initializing other models' joints to limit-obedient states and then copying out those values in the result or asserting that the joints were unchanged
• Hardcoding the set of joints to adjust (or the size of that set) rather than reading it out from model data

[ggould-tri]

In fact in the case of inactive joints Drake IK will bounding-box them to their joint limits regardless of whether their current position lies outside the joint limits (which since joint limits are quite soft they often will), leading to infeasibility (the caller sets q_inactive to q0_inactive; IK constrains q_inactive to its joint limits; the two constraints are contradictory)

[sherm1]

@ggould-tri does "model" in your description above correspond to "ModelInstance" in MBP parlance or is that a different concept?

[ggould-tri]

Yes, ModelInstance is what I mean.

[ggould-tri]

Here is a typical example of an incorrect pydrake IK usage (and its correction) that should be taken care of on the Drake side instead And here is an example of a unit test that has overconfidently assumed that pydrake IK will leave inactive DoFs unchanged

[ggould-tri]

(This is not to say that pydrake IK was wrong to do what it did, merely that it should provide an API that does what such common cases expect.)

[ggould-tri]

Temporarily assigning this issue to Dynamics/Sherm per platform review checklist.

[ggould-tri]

Whoops, caught another one today

A caller assuming that IK is not already imposing constraints on these joints, and so constraining them in a way that will prove contradictory. The effect of this, btw, was to teleport mugs all around the kitchen because the dishwasher had contradictory constraints and so the solution for mug position was everywhere infeasible, allowing SNOPT to random-walk the unconstrained dofs. Genuinely kind of funny, actually.

[ggould-tri]

A temporary solution to this would be to make Drake IK's BoundingBox constraints optional and/or warn before solver dispatch when a variable has contradictory BoundingBoxes. A better intermediate solution would be to not overload joint limits in this way (a thing that can be violated in plant state but cannot be violated in IK). Or to make DoF adjustability and limits opt-in rather than opt-out-but-you-won't-figure-out-how.

[sherm1]

@ggould-tri: I'm trying to understand this problem in terms of code changes to Drake that would fix it. There is a class drake::multibody::InverseKinematics. Would this issue be resolved if that object could be instantiated with a single ModelInstance and would then operate only on the dofs that belong to that instance? Do you have a concrete proposal in mind that would resolve the problems you encountered?

[ggould-tri]

The most common immediate problem I've encountered is that drake::multibody::InverseKinematics automatically sets BoundingBox constraints on its joint limits; because most real contexts have joint limit violations (eg a context of any system with gravity will always lie outside any vertical joint limits), callers who constrain "inactive" joints to their current positions will get failures and crazy results.

The pattern I'm seeing reimplemented a lot of is IK starting from a particular Context, where the caller sets equality constraints for most joints to their values in that context, solves on the remaining joints, and then updates the context on only the joints that they solved on. A more Context-aware wrapper around drake IK could provide this feature, but only if drake::multibody::InverseKinematics stopped introducing the joint limit constraints.

I think a method drake::multibody::InverseKinematics::FixToCurrentPositionsAndAmendConstraints(set<ModelInstanceIndex>, set<Body>, set<Joint)) (yeah, not that name, but I haven't had coffee yet...) that replaced the bounding box constraints on those elements with equality constraints would go a long way.

The other big area of confusion I see in our calling code is how to write back the Solve(prog()) results into the Context; it would be good if there were a method that did this. A first step to that would be clarifying how the plant context passed in to InverseKinematics is updated would also be helpful -- you'd be hard pressed to understand from https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_inverse_kinematics.html#a7084cbb71ccf87400b3e546ddf5b2af1 why it takes a non-const Context and under what circumstances it mutates it, and I suspect some caller code is confusing because it is trying to avoid having multiple mutable pointers to the same context at once.

It would be nice eg if I could replace most of our callers with something more like

ik = drake::multibody::InverseKinematics(plant, context);
ik.FixToCurrentPositionsAndAmendConstraints(plant.GetModelInstances() - the_only_model_i_care_about);
auto ik_prog_result = Solve(ik,prog());
ik.WriteBackUnfixedDofs(ik_prog_result);

(or its python equivalent, of course)

This would leave the opportunity to muck with the prog as needed while denying callers the need to risk fouling up the constraint semantics, and it would let callers take care that the mutable Context passed into the ik is the only source of Context mutation for the lifetime of that IK.

[hongkai-dai]

Sorry I am late in the discussion. We also had the joint limit problem back to the DRC days, that IK thinks the joint limits are hard constraints, but in reality they are soft (the robot actual joint can violate the joint limit a bit).

I think adding an option to IK to ignore the joint limit is reasonable. I can add that option if both @ggould-tri and @avalenzu agree.

[sherm1]

Thanks, @hongkai-dai. Reassigning to you for now.

[hongkai-dai]

@ggould-tri , @avalenzu @calderpg-tri and I had a discussion, here are the action items needed to address this issue

• [x] InverseKinematics has an option to ignore the joint limit. The joint limits are active by default, but in the constructor of InverseKinematics we can turn it off.
• [ ] When converting MathematicalProgram to snopt problem in snopt_solver.cc, we check if the constraint lower bound is greater than the constraint upper bound. If yes, we throw a warning.
• [ ] Add a new constructor to InverseKinematics, that takes in a set of model instance, whose q will be fixed to the values in context. The joint limits are ignored for these model instances.
• [x] Add the documentation to the constructor of InverseKinematics that takes in a Context*, that this context value will be modified. At the end of Solve, this context pointer stores the value as in the Solve result q. If the user wants to perform any kinematic computation using the IK result, they could use this context pointer directly.
• [ ] Add a new constructor to InverseKinematics, that takes in a const Context& instead of a Context*

[jwnimmer-tri]

Add a new constructor to InverseKinematics, that takes in a set of model instance, whose q will be fixed to the values in context. The joint limits are ignored for these model instances.

Does it need to be a constructor argument?

I could imagine a function like AddLockedModelsConstraint(set<ModelInstanceIndex> model_instances) that adds a "constraint" that certain models are locked (fixed at current posture, joint limits unconditionally ignored; in essence switching the bbox constraint from the joint limits min/max to the current posture exactly, instead). Similarly we could offer a function that locks everything except the models given as arguments.

Both of those functions could be both sugar for locking specific joints, so that the user could also lock any subset of joints they wanted (in a kind of advanced mode). Remember, not every set of bodies being postured is covered exactly by some set of model instances. Model instances are a function of parsing administrivia, not of robot modeling.

Similarly, I can only imagine that that IK ignores the "joint locking" feature of MbP's dynamics. If it obeyed that, it would be another way for the user to express what should be locked (by locking some joints in the context before passing it in).

[jwnimmer-tri]

=> #19442 for a baby step (obeying joint locking).