Add p_BQ as a free variable in PositionConstraint

[hongkai-dai]

The users can search over the point p_BQ as a decision variable, expressed in the frame B.

Resolves #22793

---

This change is 

[hongkai-dai]

+a:@cohnt for feature review please, thanks!

@hongkai-dai made 1 comment.
Reviewable status: LGTM missing from assignee cohnt, needs platform reviewer assigned, needs at least two assigned reviewers, missing label for release notes (waiting on @hongkai-dai).

[hongkai-dai]

-a:@cohnt since he is busy working towards paper deadlines.

+a:@SeanCurtis-TRI for feature review please, thanks!

@hongkai-dai made 1 comment.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers.

[SeanCurtis-TRI]

I've got some notes on some specific things in the implementation. I haven't reviewed the code in detail, but before I do so, I find myself confronted with a fundamental confusion.

I do not remotely understand what this change achieves. Here's how I see it, and you can let me know what I've misapprehended.

The "traditional" constraint worked because the constraint point Q is affixed to a (presumably robot frame) B. B's pose depends on the qs. For the fixed point Q to lie in an arbitrary bounding box, that limits the poses on frame B (and, consequently, the entire kinematic chain). So, it has reduced the space of acceptable qs, because not all configurations will keep Q in the box.

By making the pose of Q in B a free variable, it seems like it's no longer rigidly affixed. I can pose the arm arbitrarily and simply pick a Q that is in the bounding box and measure and express it in B. In other words, as a free variable, I can't see how this constraint is still a constraint. I feel this disables the constraint.

Presumably, I've missed something. It seems that the class documentation should also be clear on why one would want the constraint point to be a free variable.

@SeanCurtis-TRI reviewed 2 files and all commit messages, and made 6 comments.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers.

---

multibody/inverse_kinematics/position_constraint.h line 55 at r1 (raw file):

      const Eigen::Ref<const Eigen::Vector3d>& p_AQ_upper,
      const Frame<double>& frameB,
      const std::optional<Eigen::Ref<const Eigen::Vector3d>>& p_BQ,

nit: An "optional reference" has a special type in C++. It's called a pointer. However, you can't just change this parameter from a reference type to a pointer type, because it will change the signature in ways that are not backwards compatible.

Here's my proposal.

1. Leave the current constructors alone. They have the same semantics as always; specifying p_BQ.
2. Create a second set of overloaded constructors that omit the parameter entirely.

We'll then rely on overloading to decide whether p_BQ is free or not and we don't have to take optional references.

I know it leads to an explosion of constructors, and it may require some rethinking about constructor delegation..but optional references are really something we want to stay away from.

One solution may be to eliminate the explicit double/AutoDiffXd spellings of the constructor. Instead, declare it templated but requires the desired types and then explicitly instantiate them in the .cc file. You'd also have to do some type trait games in the constructor to make sure the plane and context end up assigned to the right members.

Let me know if you'd like me to take a stab at that.

---

multibody/inverse_kinematics/position_constraint.cc line 15 at r1 (raw file):

    const std::optional<Eigen::Ref<const Eigen::Vector3d>>& p_BQ) {
  if (p_BQ.has_value()) {
    return std::make_optional(

BTW std::make_optional is unnecessary. If you simply return Vector3d, std::optional has implicit constructors that will automatically wrap it.

Although, based on my recommendation in the header, the "optional reference" should be going away.

---

multibody/inverse_kinematics/position_constraint.cc line 147 at r1 (raw file):

  Eigen::Vector3d p_BQ_value;
  const bool p_BQ_is_decision_variable = !p_BQ.has_value();
  if (p_BQ.has_value()) {

BTW Given the variable you just created, this might read better as:

if (!p_BQ_is_decision_variable) {
...

Or, better yet, you can use the positive variable and reverse the order of your branches.

---

multibody/inverse_kinematics/position_constraint.cc line 149 at r1 (raw file):

  if (p_BQ.has_value()) {
    p_BQ_value = *p_BQ;
    p_BQ_T = p_BQ_value.cast<T>();

nit: I wouldn't think casting is necessary. p_BQ_value is always double valued. And p_BQ_T is either double or AutoDiffXd valued. We can assign double to either of those without adornment. Or am I missing something?

---

multibody/inverse_kinematics/position_constraint.cc line 165 at r1 (raw file):

      p_BQ_T = x.template tail<3>();
    } else {
      // S = AutoDiffXd, T = double

nit; This comment doesn't seem helpful. AutoDiffXd doesn't enter into this, because p_BQ_value is double and p_BQ_T is likewise double. So, if anything, I'd say:

// S != T --> T is double. Assign double to double.
p_BQ_T = p_BQ_value;

[cohnt]

Overall, the desire for this feature is to be able to use the world position of a part of the robot as if it were a decision variable in an IK problem. For example, I can enforce stability of a humanoid robot by enforcing the center of mass is over the support polytope -- the support polytope can be approximated as the convex hull of several points on the boundaries of the robot's feet. The best way I've been able to do this so far is to create new decision variables representing the world position of those foot points, add a constraint rather like the PositionConstraint enforcing the agreement w.r.t. kinematics, and then add the convex hull constraint. But currently, the way Drake's InverseKinematics is set up makes it very hard to add auxiliary variables that have a semantic meaning in task space (as opposed to joint space).

I'm very happy to help try and come up with the best way of accomplishing this if this seems like not the best approach. I'm also happy to leave this be (I have a local fix, albeit in Python) if you think it's too niche a problem to include in Drake, or if Hongkai is not able to work on this. (Which it sounds like might be the case?)

@cohnt reviewed all commit messages and made 1 comment.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers.

[HongkaiDai-LBM]

@cohnt what constraint would you like to enforce in the IK? Is it the quasi-static constraint (CoM's projection in the world xy plane is within the convex hull of some foot contact points)? If yes, then I don't this this PR will help you much; we can implement the quasi-static constraint instead (we did that during the DRC days).

[cohnt]

It's one example of a constraint I would like to enforce. I already have an implementation of that constraint in Python, and I could transfer it over to C++ if I need to.

The common trend on all of the constraints I'm making is I want to reason about points welded to the robot as if they were decision variables.

[SeanCurtis-TRI]

It seems like there's still something to hash out here. But the conversation seems to me heading towards "not this change". If that's the case, let's go ahead and close this PR.

@SeanCurtis-TRI made 1 comment.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers.

[HongkaiDai-LBM]

Sorry for the delay. I addressed your comments. PTAL

@HongkaiDai-LBM made 6 comments and resolved 5 discussions.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers.

---

multibody/inverse_kinematics/position_constraint.h line 55 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: An "optional reference" has a special type in C++. It's called a pointer. However, you can't just change this parameter from a reference type to a pointer type, because it will change the signature in ways that are not backwards compatible.

Here's my proposal.

1. Leave the current constructors alone. They have the same semantics as always; specifying p_BQ.
2. Create a second set of overloaded constructors that omit the parameter entirely.

We'll then rely on overloading to decide whether p_BQ is free or not and we don't have to take optional references.

I know it leads to an explosion of constructors, and it may require some rethinking about constructor delegation..but optional references are really something we want to stay away from.

One solution may be to eliminate the explicit double/AutoDiffXd spellings of the constructor. Instead, declare it templated but requires the desired types and then explicitly instantiate them in the .cc file. You'd also have to do some type trait games in the constructor to make sure the plane and context end up assigned to the right members.

Let me know if you'd like me to take a stab at that.

Done. I added the overloaded

---

multibody/inverse_kinematics/position_constraint.cc line 15 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW std::make_optional is unnecessary. If you simply return Vector3d, std::optional has implicit constructors that will automatically wrap it.

Although, based on my recommendation in the header, the "optional reference" should be going away.

Done.

---

multibody/inverse_kinematics/position_constraint.cc line 147 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Given the variable you just created, this might read better as:

if (!p_BQ_is_decision_variable) {
...

Or, better yet, you can use the positive variable and reverse the order of your branches.

Done.

---

multibody/inverse_kinematics/position_constraint.cc line 149 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: I wouldn't think casting is necessary. p_BQ_value is always double valued. And p_BQ_T is either double or AutoDiffXd valued. We can assign double to either of those without adornment. Or am I missing something?

Done.

---

multibody/inverse_kinematics/position_constraint.cc line 165 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit; This comment doesn't seem helpful. AutoDiffXd doesn't enter into this, because p_BQ_value is double and p_BQ_T is likewise double. So, if anything, I'd say:

// S != T --> T is double. Assign double to double.
p_BQ_T = p_BQ_value;

Done.

[SeanCurtis-TRI]

@SeanCurtis-TRI partially reviewed 6 files and made 4 comments.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @hongkai-dai).

---

multibody/inverse_kinematics/position_constraint.h line 55 at r1 (raw file):

Previously, HongkaiDai-LBM (Hongkai Dai) wrote…

Done. I added the overloaded

I'm going to push a commit to your PR. We can handle all of the constructors with a single implementation. This also makes the AutoDiff versions consistent with the double versions (in which they all throw if either plant or plant_context is null -- something currently missing.

PTAL.

---

a discussion (no related file):
One last topic of conversation.

To avoid the optional reference, I recommended overloading the constructors. There's a lot of cruft involved in that (as can be seen in this PR).

Furthermore, the fact that the overload comes from simply omitting a parameter seems like it could lead to problems. If someone forgets to specify p_BQ they won't have any secondary constraint on those decision variables and they'll have effectively disabled the constraint. And it may not be immediately apparent if the constraint proves to be redundant in portions of configuration space. Could that be a pernicious trap?

If we're worried about that at all, then I have an alternative thought:

The overloaded APIs shouldn't omit p_BQ but, instead, declare it as EigenPtr so that a value must either be passed or explicitly nulled out (you can't forget by accident). This might have the additional benefit that we could eventually deprecate the original API, because the EigenPtr API would do it all.

Thoughts?

---

bindings/pydrake/multibody/inverse_kinematics_py.cc line 690 at r2 (raw file):

            py::keep_alive<1, 2>(),
            // Keep alive, reference: `self` keeps `plant_context` alive.
            py::keep_alive<1, 7>(), cls_doc.ctor.doc_double)

nit: I'm unsure about this documentation tag. This references the overload that takes p_BQ. Is that what we want to do?

I suspect to keep parity between C++ and python, we want to add custom mkdoc tags in the header and reference those unique tags for the new constructors here.

---

multibody/inverse_kinematics/position_constraint.h line 20 at r2 (raw file):

 * when p_BQ is a decision variable, the constraint is evaluated on the vector x
 * = [q, p_BQ]. when p_BQ is specified, then it is not a decision variable, the
 * constraint is evaluated on the vector x = q.

nit: Based on the various conversations (in review and in person), it seems there needs to be some additional guidance regarding making p_BQ a decision variable. Specifically, I think it's worth calling out why someone would do this (and possibly even include that warning that doing so without further constraints on p_BQ would be considered a misuse).

As I see it, the "standard" instantiation of PositionConstraint constrains p_BQ to be fixed to body B. Simply removing the constraint (by making p_BQ a decision variable) is pointless. The user is obliged to replace the constraint with one of their own.

[HongkaiDai-LBM]

@HongkaiDai-LBM made 1 comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @hongkai-dai).

---

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

One last topic of conversation.

To avoid the optional reference, I recommended overloading the constructors. There's a lot of cruft involved in that (as can be seen in this PR).

Furthermore, the fact that the overload comes from simply omitting a parameter seems like it could lead to problems. If someone forgets to specify p_BQ they won't have any secondary constraint on those decision variables and they'll have effectively disabled the constraint. And it may not be immediately apparent if the constraint proves to be redundant in portions of configuration space. Could that be a pernicious trap?

If we're worried about that at all, then I have an alternative thought:

The overloaded APIs shouldn't omit p_BQ but, instead, declare it as EigenPtr so that a value must either be passed or explicitly nulled out (you can't forget by accident). This might have the additional benefit that we could eventually deprecate the original API, because the EigenPtr API would do it all.

Thoughts?

Sorry for my belated reply.

So you would like the API to pass in

PositionConstraint(
...,
EigenPtr<Eigen::Vector3d> p_BQ,
...
)

?
So the call site would be either

Eigen::Vector3d p_BQ;
PositionConstraint(plant, ..., &p_BQ, ...);

or

PositionConstraint(plant, ..., nullptr, ...);

?
This means that the current code which passes p_BQ must be all changed to passing &p_BQ. This change on the call site feels less preferable?

[SeanCurtis-TRI]

@SeanCurtis-TRI made 1 comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @hongkai-dai).

---

a discussion (no related file):

Previously, HongkaiDai-LBM (Hongkai Dai) wrote…

Sorry for my belated reply.

So you would like the API to pass in

PositionConstraint(
...,
EigenPtr<Eigen::Vector3d> p_BQ,
...
)

?
So the call site would be either

Eigen::Vector3d p_BQ;
PositionConstraint(plant, ..., &p_BQ, ...);

or

PositionConstraint(plant, ..., nullptr, ...);

?
This means that the current code which passes p_BQ must be all changed to passing &p_BQ. This change on the call site feels less preferable?

That's almost what I'm saying. You've taken it a step farther than I want. I'm not advocating changing the the pre-existing API (or eliminating it). I'm saying, as long as we're overloading, maybe it would be better to overload it with something else.

So, given the current state of the PR we'd have the following valid invocations:

// Definitely not decision variable.
PositionConstraint(plant, ..., frame_B, p_BQ, context);
// p_BQ is a decision variable, but it's not obvious that the caller intends that.
PositionConstraint(plant, ..., frame_B, context);

With my proposal, we'd have the following:

// Definitely not decision variable.
PositionConstraint(plant, ..., frame_B, p_BQ, context);
// Definitely not decision variable.
PositionConstraint(plant, ..., frame_B, &p_BQ, context);
// p_BQ is definitely a decision variable; we assume people don't pass nullptr by accident.
PositionConstraint(plant, ..., frame_B, nullptr, context);

So, there's no way to accidentally omit p_BQ. The old API still works and, here's the beauty, you only have to bind the new pointer-based API in python.

Finally, if at any time in the future we choose to deprecate the pass-by-reference API, we already have a fully functional API we can redirect people towards (although, we may never bother).

[hongkai-dai]

@hongkai-dai made 1 comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits).

---

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

That's almost what I'm saying. You've taken it a step farther than I want. I'm not advocating changing the the pre-existing API (or eliminating it). I'm saying, as long as we're overloading, maybe it would be better to overload it with something else.

So, given the current state of the PR we'd have the following valid invocations:

// Definitely not decision variable.
PositionConstraint(plant, ..., frame_B, p_BQ, context);
// p_BQ is a decision variable, but it's not obvious that the caller intends that.
PositionConstraint(plant, ..., frame_B, context);

With my proposal, we'd have the following:

// Definitely not decision variable.
PositionConstraint(plant, ..., frame_B, p_BQ, context);
// Definitely not decision variable.
PositionConstraint(plant, ..., frame_B, &p_BQ, context);
// p_BQ is definitely a decision variable; we assume people don't pass nullptr by accident.
PositionConstraint(plant, ..., frame_B, nullptr, context);

So, there's no way to accidentally omit p_BQ. The old API still works and, here's the beauty, you only have to bind the new pointer-based API in python.

Finally, if at any time in the future we choose to deprecate the pass-by-reference API, we already have a fully functional API we can redirect people towards (although, we may never bother).

So these two calls do the same thing?

// Pass p_BQ by reference
PositionConstraint(plant, ..., frame_B, p_BQ, context);
// Pass p_BQ by pointer
PositionConstraint(plant, ..., frame_B, &p_BQ, context);

I am not sure if I like it, for two reasons

1. We provide two slightly different API to do the same thing.
2. We often pass a pointer for an output variable, a variable whose value could change. Here we pass the pointer &p_BQ but don't actually change it. I think this is different from most of our code.

[SeanCurtis-TRI]

@SeanCurtis-TRI made 1 comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @hongkai-dai).

---

a discussion (no related file):

Previously, hongkai-dai (Hongkai Dai) wrote…

So these two calls do the same thing?

// Pass p_BQ by reference
PositionConstraint(plant, ..., frame_B, p_BQ, context);
// Pass p_BQ by pointer
PositionConstraint(plant, ..., frame_B, &p_BQ, context);

I am not sure if I like it, for two reasons

1. We provide two slightly different API to do the same thing.
2. We often pass a pointer for an output variable, a variable whose value could change. Here we pass the pointer &p_BQ but don't actually change it. I think this is different from most of our code.

Yep, you got it.

(1) Correct.
(2) Well, we do have APIs that take EigenPtr in the sense of "this is an optional reference". You'd apply consts properly. But you couldn't tell that from the call site.

Those are perfectly good reasons to leaving this PR as is.

On the python side, it might make sense for the user to be able to pass p_BQ=None. That's very pythonic. We've got other APIs where we don't do a 100% mapping of C++ to python because the full API doesn't make as much sense in python. In that spirit, it might make sense to create a single binding that accepts a nullable p_BQ so the user can pass None. THoughts?

[hongkai-dai]

@hongkai-dai made 1 comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits).

---

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Yep, you got it.

(1) Correct.
(2) Well, we do have APIs that take EigenPtr in the sense of "this is an optional reference". You'd apply consts properly. But you couldn't tell that from the call site.

Those are perfectly good reasons to leaving this PR as is.

On the python side, it might make sense for the user to be able to pass p_BQ=None. That's very pythonic. We've got other APIs where we don't do a 100% mapping of C++ to python because the full API doesn't make as much sense in python. In that spirit, it might make sense to create a single binding that accepts a nullable p_BQ so the user can pass None. THoughts?

I like passing p_BQ=None in python.

Maybe another way to avoid "user accidentally passing by p_BQ", is to change the order of the variables when p_BQ is a decision variable, so we have

// Specify the value of p_BQ
PositionConstraint(plant, frame_A, p_AQ_lower, p_AQ_upper, frame_B, p_BQ, context);
// Use p_BQ as a decision variable.
PositionConstraint(plant, frame_A, frame_B, p_AQ_lower, p_AQ_upper, context);

WDYT?

[SeanCurtis-TRI]

@SeanCurtis-TRI made 1 comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @hongkai-dai).

---

a discussion (no related file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I like passing p_BQ=None in python.

Maybe another way to avoid "user accidentally passing by p_BQ", is to change the order of the variables when p_BQ is a decision variable, so we have

// Specify the value of p_BQ
PositionConstraint(plant, frame_A, p_AQ_lower, p_AQ_upper, frame_B, p_BQ, context);
// Use p_BQ as a decision variable.
PositionConstraint(plant, frame_A, frame_B, p_AQ_lower, p_AQ_upper, context);

WDYT?

I can't believe how stupid I've been. In retrospect, the answer has been staring us in the face all along.

We don't need the extra overloads to gain the new functionality and maintain backward compatibility. Dur.

The original code declared type of the p_BQ parameter was:

const Eigen::Ref<const Eigen::Vector3d>& p_BQ

We simply change it to:

std::optional<Eigen::Vector3d> p_BQ

We've removed all reference shenanigans. No Eigen::Ref and no &. We're simply passing the point by value. This makes perfect sense because, in the current code, we immediately copy it into a Vector3 anyways. So, we'll copy it at the call instead of inside the constructor. Now std::nullopt is a perfectly valid argument for p_BQ, but so were all of the previous non-null values. We're not doing any extra copying.

It does mean it's important to use std::move in the delegating constructors to make sure things get to the right place.

It's worthwhile to keep the refactoring of the constructors so they all delegate to a single constructor (so we get consistent error checking at the like) among the constructors that we will keep.

This gives us all the wins:

1. No family of overloads.
2. A user can't accidentally omit p_BQ in invocations.
3. The python bindings accept p_BQ=None as a natural consequence of the parameter being optional<FooL.

Let me know if you want any assistance in making this change.

(P.S. the member declaration would change to const std::optional<Eigen::Vector3<T>> p_BQ_;. You won't need the const on the inside of the template parameter. A const optional will only return const references to its contained value.)

[hongkai-dai]

@hongkai-dai made 1 comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers.

---

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I can't believe how stupid I've been. In retrospect, the answer has been staring us in the face all along.

We don't need the extra overloads to gain the new functionality and maintain backward compatibility. Dur.

The original code declared type of the p_BQ parameter was:

const Eigen::Ref<const Eigen::Vector3d>& p_BQ

We simply change it to:

std::optional<Eigen::Vector3d> p_BQ

We've removed all reference shenanigans. No Eigen::Ref and no &. We're simply passing the point by value. This makes perfect sense because, in the current code, we immediately copy it into a Vector3 anyways. So, we'll copy it at the call instead of inside the constructor. Now std::nullopt is a perfectly valid argument for p_BQ, but so were all of the previous non-null values. We're not doing any extra copying.

It does mean it's important to use std::move in the delegating constructors to make sure things get to the right place.

It's worthwhile to keep the refactoring of the constructors so they all delegate to a single constructor (so we get consistent error checking at the like) among the constructors that we will keep.

This gives us all the wins:

1. No family of overloads.
2. A user can't accidentally omit p_BQ in invocations.
3. The python bindings accept p_BQ=None as a natural consequence of the parameter being optional<FooL.

Let me know if you want any assistance in making this change.

(P.S. the member declaration would change to const std::optional<Eigen::Vector3<T>> p_BQ_;. You won't need the const on the inside of the template parameter. A const optional will only return const references to its contained value.)

Done. Good idea, I changed to std::optional<Eigen::Vector3d> p_BQ.

[SeanCurtis-TRI]

@SeanCurtis-TRI reviewed 6 files and all commit messages, made 8 comments, and resolved 2 discussions.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers.

---

a discussion (no related file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Done. Good idea, I changed to std::optional<Eigen::Vector3d> p_BQ.

Resolving this discussion -- the new interface checks all the boxes. I've just got local comments at the implementation.

---

multibody/inverse_kinematics/position_constraint.h line 20 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: Based on the various conversations (in review and in person), it seems there needs to be some additional guidance regarding making p_BQ a decision variable. Specifically, I think it's worth calling out why someone would do this (and possibly even include that warning that doing so without further constraints on p_BQ would be considered a misuse).

As I see it, the "standard" instantiation of PositionConstraint constrains p_BQ to be fixed to body B. Simply removing the constraint (by making p_BQ a decision variable) is pointless. The user is obliged to replace the constraint with one of their own.

Just a reminder -- we're also waiting on this.

---

bindings/pydrake/multibody/inverse_kinematics_py.cc line 690 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: I'm unsure about this documentation tag. This references the overload that takes p_BQ. Is that what we want to do?

I suspect to keep parity between C++ and python, we want to add custom mkdoc tags in the header and reference those unique tags for the new constructors here.

No longer relevant.

---

multibody/inverse_kinematics/position_constraint.h line 18 at r4 (raw file):

 *
 * Note that p_BQ may or may not be a decision variable.
 * when p_BQ is a decision variable, the constraint is evaluated on the vector x

nit: Typo

Suggestion:

When p_BQ is

---

multibody/inverse_kinematics/position_constraint.cc line 23 at r4 (raw file):

    systems::Context<double>* plant_context)
    : PositionConstraint(plant, frameA, std::nullopt, p_AQ_lower, p_AQ_upper,
                         frameB, p_BQ, plant_context) {}

nit: You want to avoid an unnecessary copy here.

Suggestion:

    : PositionConstraint(plant, frameA, std::nullopt, p_AQ_lower, p_AQ_upper,
                         frameB, std::move(p_BQ), plant_context) {}

---

multibody/inverse_kinematics/position_constraint.cc line 33 at r4 (raw file):

    const Frame<double>& frameB, std::optional<Eigen::Vector3d> p_BQ,
    systems::Context<double>* plant_context)
    : solvers::Constraint(

nit: You backed this out a bit too far.

The previous revision had only a single private constructor implementation to which all public constructors delegated. That was definitely an improvement.

For example, right now the autodiff-valued constructor doesn't validate that plant is non-null like the double-valued constructor does.

---

multibody/inverse_kinematics/position_constraint.cc line 90 at r4 (raw file):

    systems::Context<AutoDiffXd>* plant_context)
    : PositionConstraint(plant, frameA, std::nullopt, p_AQ_lower, p_AQ_upper,
                         frameB, p_BQ, plant_context) {}

nit: Avoid unnecessary copy.

Suggestion:

    : PositionConstraint(plant, frameA, std::nullopt, p_AQ_lower, p_AQ_upper,
                         frameB, std::move(p_BQ), plant_context) {}

---

bindings/pydrake/multibody/inverse_kinematics_py.cc line 711 at r4 (raw file):

            // Keep alive, reference: `self` keeps `plant_context` alive.
            py::keep_alive<1, 8>(), cls_doc.ctor.doc_autodiff)

nit: Stray empty line.

[hongkai-dai]

@hongkai-dai made 2 comments and resolved 5 discussions.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers.

---

multibody/inverse_kinematics/position_constraint.h line 20 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Just a reminder -- we're also waiting on this.

Done.

---

multibody/inverse_kinematics/position_constraint.cc line 33 at r4 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: You backed this out a bit too far.

The previous revision had only a single private constructor implementation to which all public constructors delegated. That was definitely an improvement.

For example, right now the autodiff-valued constructor doesn't validate that plant is non-null like the double-valued constructor does.

Thanks! I must did a force push before I pull in your commit. I reverted it now.

[SeanCurtis-TRI]

I've updated your PR: I've merged from master and ran the pybind doc string regeneration. The latter should make CI happy. The former was necessary so I could do the latter.

+a:@ggould-tri for platform review.

@SeanCurtis-TRI partially reviewed 3 files, made 1 comment, and resolved 1 discussion.
Reviewable status: LGTM missing from assignee ggould-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @hongkai-dai).

[SeanCurtis-TRI]

@SeanCurtis-TRI reviewed 1 file and all commit messages.
Reviewable status: LGTM missing from assignee ggould-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @hongkai-dai).

[ggould-tri]

with some readability concerns.

@ggould-tri reviewed 6 files and all commit messages, and made 8 comments.
Reviewable status: 7 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on hongkai-dai).

---

multibody/inverse_kinematics/position_constraint.h line 29 at r6 (raw file):

 * When p_BQ is a decision variable, the constraint is evaluated on the vector x
 * = [q, p_BQ]. When p_BQ is specified, then it is not a decision variable, the
 * constraint is evaluated on the vector x = q.

BTW: I found this grammar a bit confusing; I think the second sentence is meant to be compound and isn't. Suggestion for slightly more parallel structure below:

Suggestion:

 * When p_BQ is a decision variable (i.e., it is _not_ specified in the ctor),
 * the constraint is evaluated on the vector x = [q, p_BQ].
 * When p_BQ is not a decision variable (i.e. it _is_ specified non-`nullopt`in the ctor),
 * the constraint is evaluated on the vector x = q.

---

multibody/inverse_kinematics/position_constraint.h line 108 at r6 (raw file):

   */
  PositionConstraint(const MultibodyPlant<double>* plant,
                     const Frame<double>& frameAbar,

BTW: I am astonished that our styleguide doesn't prohibit overloading on one random argument in the middle of a vast forest of arguments. Clearly this is a longstanding practice, but it is a readbility landmine. I had a heck of a time understanding what was up with the tests because of this.
Pre-existing and baked into the public API, so no action required here, but this seems bad, and we should consider arg-commenting callsites -- and/or have named factories or a config struct or use an optional for in-band signaling or something better here.

---

multibody/inverse_kinematics/position_constraint.cc line 66 at r6 (raw file):

    const math::RigidTransformd& X_AAbar, const Frame<double>& frameB,
    const Eigen::Vector3d& p_AQ, const Eigen::Vector3d& p_BQ,
    bool p_BQ_is_decision_variable, const Eigen::Ref<const AutoDiffVecXd>& x,

The switch from optional to bool-plus-constref at this layer is both surprising and brittle (since the constref p_BQ will be uninitialized on one path).
At minimum there should be an explanation of why this representation switch was made, and was made at this level.

Code quote:

    const Eigen::Vector3d& p_AQ, const Eigen::Vector3d& p_BQ,
    bool p_BQ_is_decision_variable, const Eigen::Ref<const AutoDiffVecXd>& x,

---

multibody/inverse_kinematics/position_constraint.cc line 124 at r6 (raw file):

    }
  } else {
    p_BQ_value = *p_BQ;

nit: .value() has better error output behaviour and is more consistent with the surrounding code. Alternatively, value_or, or restructure the args as mentioned above.

Suggestion:

p_BQ.value()

---

multibody/inverse_kinematics/test/position_constraint_test.cc line 208 at r6 (raw file):

  };
  // Constructs constraint with non-empty X_AbarA.
  PositionConstraint constraint(plant_, frameAbar, X_AAbar.inverse(),

BTW, author's discretion, IMO this comment would be slightly clearer with an intermediate line to align with both the comment and the argument name, particularly because the PositionConstraint ctor has so many easily confused args. And because the subsequent comments phrase it the inverse way.

Suggestion:

  // Constructs constraint with non-empty X_AbarA.
  const math::RigidTransformd X_AbarA = X_AAbar.inverse();
  PositionConstraint constraint(plant_, frameAbar, X_AbarA,

---

multibody/inverse_kinematics/test/position_constraint_test.cc line 216 at r6 (raw file):

  PositionConstraint constraint_empty_X_AAbar(plant_, frameAbar, p_AQ_lower,
                                              p_AQ_upper, frameB, std::nullopt,
                                              plant_context_);

minor: "empty X_AAbar" is confusing since X_AAbar is not empty but wholly absent -- double confusing since there's a std::nullopt denoting something empty in the call but it isn't X_AAbar the thing you just said is empty.

Suggestion:

  // Constructs constraint without X_AAbar.
  PositionConstraint constraint_without_X_AAbar(plant_, frameAbar, p_AQ_lower,
                                              p_AQ_upper, frameB, std::nullopt,
                                              plant_context_);

---

multibody/inverse_kinematics/test/position_constraint_test.cc line 234 at r6 (raw file):

  }
  /* tolerance for checking numerical gradient vs analytical gradient. The
   * numerical gradient is only accurate up to 4E-7 */

nit cappunc

Suggestion:

  /* Tolerance for checking numerical gradient vs analytical gradient. The
   * numerical gradient is only accurate up to 4E-7. */

[hongkai-dai]

@hongkai-dai made 1 comment and resolved 6 discussions.
Reviewable status: 1 unresolved discussion.

---

multibody/inverse_kinematics/position_constraint.cc line 66 at r6 (raw file):

Previously, ggould-tri wrote…

The switch from optional to bool-plus-constref at this layer is both surprising and brittle (since the constref p_BQ will be uninitialized on one path).
At minimum there should be an explanation of why this representation switch was made, and was made at this level.

There might be a confusion here, the const Eigen::Ref<const AutoDiffVecXd>& x is always initialized. It contains all the decision variables. When p_BQ_is_decision_variable = True, then x = [q, p_BQ]; when p_BQ_is_decision_variable=False, then x = q.

[SeanCurtis-TRI]

@SeanCurtis-TRI reviewed 4 files and all commit messages.
Reviewable status: 1 unresolved discussion.

[ggould-tri]

@ggould-tri resolved 1 discussion.
Reviewable status: complete! all discussions resolved, LGTM from assignees ggould-tri(platform),SeanCurtis-TRI(platform).