Add an Approximate Joint Centering Cost to GlobalInerseKinematics

[cohnt]

This is roughly what I was thinking such a cost might look like. It seems to be a lot faster than AddPostureCost, with Gurobi yielding a solution almost instantly.

cc @hongkai-dai

---

This change is 

[cohnt]

As a follow-up, we can add an approximate "L2-norm squared cost" by using some more explicit parsing. But this performs worse than the existing cost on the test case (17 seconds vs 7 seconds). The "L1-norm cost" is basically instantaneous, with Gurobi guessing the solution heuristically on the first try. The "L2 cost" (with and without using the Lorentz cone trick) takes 4 seconds.

(I put the costs in quotes, since they're all really just approximations.)

[hongkai-dai]

Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 369 at r1 (raw file):

   * @param body_index The joint connecting the parent link to this body will have a cost applied.
   * @param nominal_value The cost is minimized when the joint is equal to this value.
   * @param weight The weight applied to this cost.

Could you add the mathematical formulation of this cost in the documentation?

[tyler-yankee]

@drake-jenkins-bot retest this please

Attention contributor: in order for Jenkins to pass, you will need to rebase (or merge) your PR up to the latest master branch, and re-push. If you need help with that, let us know. You can push that change separately, or you can combine it with a push of review fixes.

[cohnt]

Alright, I think this one is ready for feature review.

+@assignee:hongkai-dai for feature review

Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 369 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Could you add the mathematical formulation of this cost in the documentation?

Done.

---

a discussion (no related file):
Not sure why there are a bunch of changes to files in bindings/generated_docstrings/... but they resulted from running

bazel run //bindings/generated_docstrings:regenerate

(Which was needed to make CI happy.)

[jwnimmer-tri]

Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

a discussion (no related file):

Previously, cohnt (Thomas Cohn) wrote…

Not sure why there are a bunch of changes to files in bindings/generated_docstrings/... but they resulted from running

bazel run //bindings/generated_docstrings:regenerate

(Which was needed to make CI happy.)

Are you running on Jammy? We don't support docstring regeneration on Jammy (clang is too old). It must be on Noble or macOS.

[cohnt]

Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Are you running on Jammy? We don't support docstring regeneration on Jammy (clang is too old). It must be on Noble or macOS.

Yes, I'm running jammy. What is the appropriate fix?

[jwnimmer-tri]

Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

a discussion (no related file):

Previously, cohnt (Thomas Cohn) wrote…

Yes, I'm running jammy. What is the appropriate fix?

The platform reviewer can push the docstrings at the end.

[jwnimmer-tri]

Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The platform reviewer can push the docstrings at the end.

It also looks like maybe just reverting all of the generated_docstrings change other than multibody might work.

[cohnt]

Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

It also looks like maybe just reverting all of the generated_docstrings change other than multibody might work.

Looks like it's good now?

[hongkai-dai]

@hongkai-dai reviewed 4 of 4 files at r5, 1 of 6 files at r6.
Reviewable status: 4 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 409 at r7 (raw file):

   * use AddPostureCost() instead.
   */
  void AddJointCenteringCost(BodyIndex body_index, double nominal_value,

BTW Consider renaming body_index to child_body_index? Each joint has two bodies, the parent and the child. It is better to be more specific.

---

multibody/inverse_kinematics/test/global_inverse_kinematics_test.cc line 212 at r7 (raw file):

    result = gurobi_solver.Solve(global_ik_.prog(), {}, options);
    EXPECT_TRUE(result.is_success());

I think we should check if the cost value is the expected value, for each form of cost, namely add a check

EXPECT_NEAR(result.get_optimal_cost(), cost_expected);

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 881 at r7 (raw file):

      joint->frame_on_child().GetFixedPoseInBodyFrame();

  switch (joint->num_velocities()) {

There is a lot of code overlap with the AddPostureCost(). Consider to factor out the common part?

[cohnt]

Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @cohnt)

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 409 at r7 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

BTW Consider renaming body_index to child_body_index? Each joint has two bodies, the parent and the child. It is better to be more specific.

The intent was to match the argument name used for AddJointLimitConstraint. Do you think it's better to have them not match?

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 881 at r7 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

There is a lot of code overlap with the AddPostureCost(). Consider to factor out the common part?

I factored out the shared code between this and AddJointLimitConstraint.

---

multibody/inverse_kinematics/test/global_inverse_kinematics_test.cc line 212 at r7 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I think we should check if the cost value is the expected value, for each form of cost, namely add a check

EXPECT_NEAR(result.get_optimal_cost(), cost_expected);

For the existing cost (posture cost), we don't test that the achieved cost value is the exact value it should be -- just that it drives the joint angles to the correct solution. So maybe we can get away with that for these costs as well?

My concern is that it would be complicated to calculate the cost by hand, and ultimately, we'll just end up with a magic number? Is that okay?

[hongkai-dai]

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

@hongkai-dai reviewed 1 of 1 files at r9.
Reviewable status: 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 @cohnt)

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 409 at r7 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

The intent was to match the argument name used for AddJointLimitConstraint. Do you think it's better to have them not match?

that makes sense.

---

multibody/inverse_kinematics/test/global_inverse_kinematics_test.cc line 212 at r7 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

For the existing cost (posture cost), we don't test that the achieved cost value is the exact value it should be -- just that it drives the joint angles to the correct solution. So maybe we can get away with that for these costs as well?

My concern is that it would be complicated to calculate the cost by hand, and ultimately, we'll just end up with a magic number? Is that okay?

Sounds good.

[SeanCurtis-TRI]

I'm feeling incredibly ignorant. I have a very high level question. Why this? Given that this applies to revolute joints (with a vague intention of extending it to revolute-like joints), why all the vector shennigans? Why not just put a cost on the q value relative to a reference q value? Intuition says, 'Look! Rotation articulated as a single scalar value....Let's use that." But this PR implies that that intuition is very wrong. So, what does this arcane approximation do for us that simply penalizing q directly doesn't do?

(This is somewhat orthogonal to the review I'm doing, but it was the one, huge question that I have as yet been unable to resolve.)

[cohnt]

Reviewable status: 1 unresolved discussion, 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…

I'm feeling incredibly ignorant. I have a very high level question. Why this? Given that this applies to revolute joints (with a vague intention of extending it to revolute-like joints), why all the vector shennigans? Why not just put a cost on the q value relative to a reference q value? Intuition says, 'Look! Rotation articulated as a single scalar value....Let's use that." But this PR implies that that intuition is very wrong. So, what does this arcane approximation do for us that simply penalizing q directly doesn't do?

(This is somewhat orthogonal to the review I'm doing, but it was the one, huge question that I have as yet been unable to resolve.)

The short version is that this IK formulation doesn't actually have a variable corresponding to the joint angles, but rather variables describing the pose of each link. So it's not possible to construct the joint angles within the optimization without losing convexity, which is needed since it's mixed-integer.

Hongkai can probably explain a bit better than me, since this class implements a paper he wrote a few years back. We're adding these new costs because we've been experimenting with alternate costs in the global IK formulation, and found these ones were along the pareto frontier.

[SeanCurtis-TRI]

Previously, cohnt (Thomas Cohn) wrote…

The short version is that this IK formulation doesn't actually have a variable corresponding to the joint angles, but rather variables describing the pose of each link. So it's not possible to construct the joint angles within the optimization without losing convexity, which is needed since it's mixed-integer.

Hongkai can probably explain a bit better than me, since this class implements a paper he wrote a few years back. We're adding these new costs because we've been experimenting with alternate costs in the global IK formulation, and found these ones were along the pareto frontier.

That alone is very helpful. It lets me know that my assumptions are wrong and therefore shouldn't be relied on. Thanks.

[hongkai-dai]

Reviewable status: 1 unresolved discussion, 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 @cohnt)

---

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

That alone is very helpful. It lets me know that my assumptions are wrong and therefore shouldn't be relied on. Thanks.

@SeanCurtis-TRI , as Tommy explained, this global inverse kinematics formulation solves the kinematics in the maximal coordinate, namely each link has its pose as decision variable, then we impose constraints on the poses of the adjacent links to imply that they are connected through a joint. This is different from the InverseKinematics or DifferentialInverseKinematics class in Drake which uses the joint position as decision variable (namely in the minimal coordinate).

[SeanCurtis-TRI]

@SeanCurtis-TRI reviewed 3 of 4 files at r5, 2 of 6 files at r6, 4 of 4 files at r7, 1 of 1 files at r9, all commit messages.
Reviewable status: 20 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 @cohnt)

---

multibody/inverse_kinematics/test/global_inverse_kinematics_test.cc line 212 at r7 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Sounds good.

I agree that a test on cost would be valuable. For example, it would've caught (what I believe is) the bug in the L1 cost in which the weight got squared.

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 619 at r9 (raw file):

// Helper struct containing precomputed quantities shared by
// AddJointLimitConstraint() and AddJointCenteringCost().
struct RevoluteJointGeometry {

BTW Perhaps a different name? RevoluteSamples? RevoluteCostSamples? The word "geometry" is just so generic it doesn't really communicate much about the very particular semantics this has.

Code quote:

RevoluteJointGeometry

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 632 at r9 (raw file):

  double v_C_norm = v_C.norm();
  if (v_C_norm < sqrt(2) / 2) {
    // axis_F is almost parallel to [1; 0; 0]. Try another axis [0, 1, 0]

nit: I've removed my remarks about the details of this algorithm. Instead, I'm simply advocating for using: RotationMatrix<double\>::MakeFromOneUnitVector().

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 647 at r9 (raw file):

  // null(axis_F).
  std::array<Eigen::Vector3d, 2> v_basis = {{v_C, axis_F.cross(v_C)}};
  v_basis[1] /= v_basis[1].norm();

BTW This might read more easily if you initialized with the normalized value.

Suggestion:

  const std::array<Eigen::Vector3d, 2> v_basis = {
      {v_C, axis_F.cross(v_C).normalized()}};

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 653 at r9 (raw file):

  v_samples[1] = v_basis[1];
  v_samples[2] = v_basis[0] + v_basis[1];
  v_samples[2] /= v_samples[2].norm();

BTW Same note as above regarding normalization:

Suggestion:

  v_samples[2] = (v_basis[0] + v_basis[1]).normalized();

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 767 at r9 (raw file):

                                         joint_upper_bounds_[position_idx]) /
                                        2;
            const auto joint_geometry =

nit: Generally, your use of auto is more aggressive than allowed for in the styleguide. Essentially, if the type isn't "obvious" you can't use auto. For these three variables, that seems to apply.

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 874 at r9 (raw file):

void GlobalInverseKinematics::AddJointCenteringCost(BodyIndex body_index,
                                                    double nominal_value,

BTW I have a number of comments below about testing input parameters. It happens indirectly (in some cases) and late in others. Furthermore, a bunch of the code seems to have been written with the intention of supporting more joint types in the future (beyond the revolute joint supported now). Unless the follow up PR is already scheduled for implementation, it doesn't make sense to place the burden on this PR to stake out a place for work that will happen at some indefinite time in the future. Therefore I propose the following:

1. Get validation done out of the way and early.
2. Tailor this specifically to what is supported. If we add more joint types, we can refactor later.
3. We can avoid dynamic casts by using the Joint API more and then confidently static_cast.

void GlobalInverseKinematics::AddJointCenteringCost(BodyIndex body_index,
                                                    double nominal_value,
                                                    double weight, int norm,
                                                    bool squared) {
  // Validate arguments: norm parameters and body index.
  DRAKE_THROW_UNLESS(norm == 1 || norm == 2);
  DRAKE_THROW_UNLESS(norm == 2 || squared == false);
  DRAKE_THROW_UNLESS(body_index >= 0 && body_index < plant_.num_bodies() &&
                     body_index != plant_.world_body().index());
  
  // Identify joint to parent.
  const Joint<double>* joint{nullptr};
  for (JointIndex joint_index : plant_.GetJointIndices()) {
    if (plant_.get_joint(joint_index).child_body().index() == body_index) {
      joint = &(plant_.get_joint(joint_index));
      break;
    }
  }

  // Confirm the joint is of supported type (right now, only Revolute).
  if (joint->type_name() != "revolute") {
    throw std::runtime_error(fmt::format(
        "AddJointCenteringCost() can only be invoked on a body that is "
        "connected to its parent by a revolute joint. The body '{}' ({}) "
        "connects via a {} joint.",
        joint->child_body().name(), child_index, joint->type_name()));
  }

  const auto& rev_joint = *static_cast<const RevoluteJoint<double>*>(joint);

  ... below this line grab frames, sample vertices, set up constraints, etc.

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 877 at r9 (raw file):

                                                    double weight, int norm,
                                                    bool squared) {
  if (plant_.get_body(body_index).is_floating_base_body()) {

nit: Specifically calling out floating joints is unnecessary. This would be caught by an error condition below.

---

The code suggestion above simply eliminates this specific test.

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 891 at r9 (raw file):

  if (joint == nullptr) {
    throw std::runtime_error(
        fmt::format("The body {} is not the child of any joint in the plant.",

nit: I suspect the only way you will get this exception to get thrown is if the body_index is a) the world body or b) an invalid index value. For every other body index, every body is connected to a parent via a joint.

---

The code suggested above changes this test to a more direct test on the validity of body_index.

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 896 at r9 (raw file):

  const RigidBody<double>& parent = joint->parent_body();
  const int parent_idx = parent.index();
  const RigidTransformd X_PJp =

nit: Going along with the comment about the documentation of this math, you don't actually want/need the rigid transform, just the rotation. So, let's not grab and save the big thing that we have to strip down later. Let's simply grab the thing we care about.

const RotationMatrixd R_PJp =
      joint->frame_on_parent().GetFixedPoseInBodyFrame().rotation();

This makes your intent clearer and simplifies the mathematical expression below. (Same for R_C_Jc.)

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 906 at r9 (raw file):

    }
    case 1: {
      if (dynamic_cast<const RevoluteJoint<double>*>(joint) != nullptr) {

nit: We're not a big fan of dynamic casting. And as joints so readily will give you a positive signal about what they are, we should use that coupled with a static cast.

---

The code suggested above changes this test to a more direct test on the validity of body_index.

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 909 at r9 (raw file):

        const auto* revolute_joint =
            dynamic_cast<const RevoluteJoint<double>*>(joint);
        const Vector3d axis_F = revolute_joint->revolute_axis();

BTW: You've introduced a new frame F without saying anything about its semantics. I'd recommend either omitting F entirely or, if you want, pick C or P (as the measures of axis_P and axis_C are the same, by construction.

Alternatively, a note that says F is a generic frame aligned with both P and C.

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 928 at r9 (raw file):

              prog_.AddLinearConstraint(s >= unit_vector_difference[i]);
              prog_.AddLinearConstraint(s >= -unit_vector_difference[i]);
              prog_.AddLinearCost(weight * s);

nit: Maybe I'm misunderstanding, but given as we've already multiplied weight into unit_vector_difference, does this extra product of weight not scale the cost function by weight squared?

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 364 at r9 (raw file):

  /**
   * Adds a cost that penalizes deviation of a revolute joint from a specified

nit: You should say something about periodicity. What if you have an infinite revolute joint? This cost won't be able to distinguish between repeated loops. Is that on purpose? Or should we document the intended space?

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 368 at r9 (raw file):

   * For the joint connecting the specified body to its parent, this method adds
   * a cost that encourages the joint’s orientation to align with the nominal
   * rotation nominal_value about its revolute axis. The cost is evaluated by

nit: With the name of the parameter included, the phrase "norminal rotation nominal_value about its revolute axis" seems redundant. The context allows us to throw out some of the redundancy as below.

Suggestion:

   * a cost that encourages the joint’s orientation to align with the
   * given nominal_value. The cost is evaluated by

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 373 at r9 (raw file):

   *
   * Specifically, let
   * R_WB be the rotation matrix of the child body frame B in world frame W.

Nit: You've got two frames B and C which are supposed to be the same. We should eliminate one of the symbols. Perhaps simplest to refer to the "child body" as C right from the get go?

That'll make the expression R_WB X_CJc multiply out better as R_WC X_CJc.

Code quote:

R_WB

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 381 at r9 (raw file):

   * For a small set of unit vectors vᵢ orthogonal to the joint axis, the
   * cost penalizes
   * ∑ᵢ ‖ R_WB X_CJc vᵢ − R_WP X_PJp R(k, nominal_value) vᵢ ‖ₙ

nit: Given v_i is a vector, you shouldn't be multiplying it by the full transform X_CJc, but the rotation R_CJc.

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 381 at r9 (raw file):

   * For a small set of unit vectors vᵢ orthogonal to the joint axis, the
   * cost penalizes
   * ∑ᵢ ‖ R_WB X_CJc vᵢ − R_WP X_PJp R(k, nominal_value) vᵢ ‖ₙ

BTW For these long concatenations of matrices, I often find an explicit concatenation operator to help readability (especially in a text editor). For me, it helps me better cluster my terms. You might consider:

Σᵢ‖R_WC·R_CJc·vᵢ - R_WP·R_PJp·R(k, nominal_value)·vᵢ‖ₙ

---

multibody/inverse_kinematics/global_inverse_kinematics.h line 383 at r9 (raw file):

   * ∑ᵢ ‖ R_WB X_CJc vᵢ − R_WP X_PJp R(k, nominal_value) vᵢ ‖ₙ
   * where ‖·‖ₙ denotes either the L1 or L2 norm (depending on norm), and the
   * result is scaled by weight. If squared is true and norm = 2, the

BTW This surprised me. So, again, excuse my ignorance. But I would generally assume that the squared L2 norm would generally be preferred (as it eliminates the square root inherent in the L2).

Is there a quick and dirty explanation for why someone would prefer L2 over squared L2?

---

multibody/inverse_kinematics/test/global_inverse_kinematics_test.cc line 122 at r9 (raw file):

  solvers::GurobiSolver gurobi_solver;
  if (!gurobi_solver.available()) {
    return;

nit: What is the risk of this test being misconfigured such that this test never runs and it instantly becomes bitrot?

[SeanCurtis-TRI]

I've pushed a new revision, @hongkai, I'd appreciate you taking a look. You can either pass edits to me or push a commit yourself. At this point, no more commit curation or force pushes, please.

Reviewable status: 19 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 @cohnt)

[SeanCurtis-TRI]

Reviewable status: 20 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 @cohnt)

---

a discussion (no related file):
BTW Looks like we need to do a merge from master.

[SeanCurtis-TRI]

Reviewable status: 20 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 @cohnt)

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 818 at r10 (raw file):

              if (!linear_constraint_approximation) {
                std::array<Eigen::Vector3d, 2> v_basis = {
                    {v_samples[0], (v_samples[0].cross(axis_F)).normalized()}};

BTW I just realized, (v_samples[0].cross(axis_F)).normalized() is exactly v_samples[1]. Do we want to exploit that?

[SeanCurtis-TRI]

multibody/inverse_kinematics/global_inverse_kinematics.cc line 910 at r10 (raw file):

    const auto v_Jc_theta = v;
    Eigen::Matrix<Expression, 3, 1> unit_vector_difference =
        weight * (R_WJc * v_Jc - R_WJp_theta * v_Jc_theta);

@hongkai-dai Quick question about using weight right here.

By putting it here, it automatically percolates through whichever norm the user selects. However, for the L2-squared norm, the weight is getting squared as well. That's probably not a huge problem as the L2-squared norm is radically different from either the L2 or L1 norms and the user would presumably expect to change the weight if they toggle on "squared" to keep a consistent balance between this cost and any other costs.

The question is, would it be better if the norm were weighted instead of weighting the data being normed?

[hongkai-dai]

Reviewable status: 21 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 @cohnt)

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 910 at r10 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

@hongkai-dai Quick question about using weight right here.

By putting it here, it automatically percolates through whichever norm the user selects. However, for the L2-squared norm, the weight is getting squared as well. That's probably not a huge problem as the L2-squared norm is radically different from either the L2 or L1 norms and the user would presumably expect to change the weight if they toggle on "squared" to keep a consistent balance between this cost and any other costs.

The question is, would it be better if the norm were weighted instead of weighting the data being normed?

I think first computing the norm, and then multiply the weight with the norm is probably more consistent. Anyway the documentation should be very clear.

[SeanCurtis-TRI]

Reviewable status: 20 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 @cohnt)

---

multibody/inverse_kinematics/global_inverse_kinematics.cc line 910 at r10 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I think first computing the norm, and then multiply the weight with the norm is probably more consistent. Anyway the documentation should be very clear.

Concretely, what are you going to do? (How does the code change.) You tell, I'll do it.