Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use AddMultibodyPlantConstraints in InverseKinematics #22361

Open
wants to merge 1 commit into
base: master
Choose a base branch
from

Conversation

RussTedrake
Copy link
Contributor

@RussTedrake RussTedrake commented Dec 31, 2024

Also moves the joint limits / joint locking logic from the IK constructor to AddMultibodyPlantConstraints.

Towards #18917.


This change is Reviewable

@RussTedrake RussTedrake added the release notes: feature This pull request contains a new feature label Dec 31, 2024
Copy link
Contributor Author

@RussTedrake RussTedrake left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

+@hongkai-dai for feature review, please?

Reviewable status: LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers

Copy link
Contributor

@hongkai-dai hongkai-dai left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewed 4 of 7 files at r1.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers


multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 60 at r1 (raw file):

        const int start = joint.position_start();
        const int size = joint.num_positions();
        lb.segment(start, size).setConstant(-kInf);

Curious why not to update lb and ub to current_positions.segment(start, size), but adding a LinearEqualityConstraint? I think a BoundingBoxConstraint (as specified by lb and ub) is easier to handle than a generic LinearEqualityConstraint for the mathematical solvers.

If the purpose is to add a separate constraint just for q.segment(start, size), then I would suggest to add it as a BoundingBoxConstraint

binding.emplace_back(prog->AddBoundingBoxConstraint(current_positions.segment(start, size), current_positions.segment(start, size), q.segment(start, size))).evaluator()->set_description(fmt::format("Joint {} lock", joint.name()));

Copy link
Contributor Author

@RussTedrake RussTedrake left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers


multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 60 at r1 (raw file):

I think a BoundingBoxConstraint (as specified by lb and ub) is easier to handle than a generic LinearEqualityConstraint for the mathematical solvers.

This has been my mental model for years, as well. But when it came up in a conversation with Pablo and @AlexandreAmice ... they felt strongly that linear equality constraints were better for solvers in general.

@RussTedrake
Copy link
Contributor Author

multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 60 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

I think a BoundingBoxConstraint (as specified by lb and ub) is easier to handle than a generic LinearEqualityConstraint for the mathematical solvers.

This has been my mental model for years, as well. But when it came up in a conversation with Pablo and @AlexandreAmice ... they felt strongly that linear equality constraints were better for solvers in general.

@frankpermenter -- curious what you think? if you have a constraint of the form vars = a, would you prefer to tell the solver a <= vars <= a or eye @ vars == a ?

Copy link
Contributor

@hongkai-dai hongkai-dai left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers


multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 60 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

I think a BoundingBoxConstraint (as specified by lb and ub) is easier to handle than a generic LinearEqualityConstraint for the mathematical solvers.

This has been my mental model for years, as well. But when it came up in a conversation with Pablo and @AlexandreAmice ... they felt strongly that linear equality constraints were better for solvers in general.

@AlexandreAmice could you elaborate why linear equality constraints are better than bounding box constraints? Thanks!

Copy link
Contributor Author

@RussTedrake RussTedrake left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers


multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 60 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

@frankpermenter -- curious what you think? if you have a constraint of the form vars = a, would you prefer to tell the solver a <= vars <= a or eye @ vars == a ?

@hongkai-dai -- the rational was that it would be easier for the solver to "solve away" those variables. But honestly, in my mind, one either has to detect that lb == ub or detect that A == eye in A @ vars == b, so I'm not entirely convinced.

Copy link
Contributor

@hongkai-dai hongkai-dai left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers


multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 60 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

@hongkai-dai -- the rational was that it would be easier for the solver to "solve away" those variables. But honestly, in my mind, one either has to detect that lb == ub or detect that A == eye in A @ vars == b, so I'm not entirely convinced.

It seems detecting A==eye is harder than detecting lb==ub?

Copy link
Contributor Author

@RussTedrake RussTedrake left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers


multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 60 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

It seems detecting A==eye is harder than detecting lb==ub?

But of course you can substitute Ax == b into other constraints easily enough.

@RussTedrake
Copy link
Contributor Author

@drake-jenkins-bot retest this, please.

Copy link
Contributor Author

@RussTedrake RussTedrake left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers


multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 60 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

But of course you can substitute Ax == b into other constraints easily enough.

I've conformed and switched back to lb == ub. I think we should push ahead with that.

Copy link
Contributor

@hongkai-dai hongkai-dai left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewed 2 of 7 files at r1, 3 of 3 files at r2.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers


multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 71 at r2 (raw file):

    if (body.has_quaternion_dofs()) {
      const int start = body.floating_positions_start();
      constexpr int size = 4;

BTW, for this kind of constant I think we typically name it as kSize.


multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc line 210 at r2 (raw file):

                      Eigen::Vector4d(1, 1, 1, 1)));

  // joint2 is locked, so we expect a linear equality constraint and limits of

BTW, outdated documentation, we don't expect a linear equality constraint any more, neither do we have limits of [-1, 1].


multibody/inverse_kinematics/test/inverse_kinematics_test.cc line 233 at r2 (raw file):

                      Eigen::Vector4d(1, 1, 1, 1)));

  // joint2 is locked, so we expect a linear equality constraint and limits of

Ditto

@RussTedrake RussTedrake force-pushed the ik_mbp_constraints branch 2 times, most recently from a562e54 to ff5dd8b Compare January 6, 2025 13:11
Copy link
Contributor Author

@RussTedrake RussTedrake left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers


multibody/inverse_kinematics/add_multibody_plant_constraints.cc line 71 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

BTW, for this kind of constant I think we typically name it as kSize.

Done.


multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc line 210 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

BTW, outdated documentation, we don't expect a linear equality constraint any more, neither do we have limits of [-1, 1].

Done.


multibody/inverse_kinematics/test/inverse_kinematics_test.cc line 233 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Ditto

Done.

@RussTedrake RussTedrake force-pushed the ik_mbp_constraints branch 3 times, most recently from 6eecee4 to b4a3961 Compare January 6, 2025 14:47
Copy link
Contributor

@hongkai-dai hongkai-dai left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

:lgtm: The CI failure is real, that IPOPT fails to solve the problem.

Reviewed 3 of 3 files at r3, 1 of 1 files at r4.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers

@RussTedrake
Copy link
Contributor Author

that's actually terrible. For inverse_kinematics_test the difference is only the new [-1,1] constraints. Old program

[ RUN      ] TwoFreeBodiesTest.PositionConstraint
Decision variables:  q(0)  q(1)  q(2)  q(3)  q(4)  q(5)  q(6)  q(7)  q(8)  q(9) q(10) q(11) q(12) q(13)

UnitQuaternionConstraint with 4 decision variables q(0) q(1) q(2) q(3)
UnitQuaternionConstraint with 4 decision variables q(7) q(8) q(9) q(10)
PositionConstraint with 14 decision variables q(0) q(1) q(2) q(3) q(4) q(5) q(6) q(7) q(8) q(9) q(10) q(11) q(12) q(13)
BoundingBoxConstraint
-inf <= q(0) <= inf
-inf <= q(1) <= inf
-inf <= q(2) <= inf
-inf <= q(3) <= inf
-inf <= q(4) <= inf
-inf <= q(5) <= inf
-inf <= q(6) <= inf
-inf <= q(7) <= inf
-inf <= q(8) <= inf
-inf <= q(9) <= inf
-inf <= q(10) <= inf
-inf <= q(11) <= inf
-inf <= q(12) <= inf
-inf <= q(13) <= inf

[2025-01-07 11:38:01.665] [console] [info] result:     0.999766148184183   0.01681001705351731  -0.01355465403353861  0.001159148676400541   -0.1431276280275835   -0.2298639169407503   -0.3936192847140234    0.9986453104060482  -0.03200385491261028   0.03988872632365753 -0.009601395055742623    0.1431276280276386     0.229863916940848    0.3936192847138122

and new program

[ RUN      ] TwoFreeBodiesTest.PositionConstraint
Decision variables:  q(0)  q(1)  q(2)  q(3)  q(4)  q(5)  q(6)  q(7)  q(8)  q(9) q(10) q(11) q(12) q(13)

UnitQuaternionConstraint described as 'Unit quaternion constraint for body body1' with 4 decision variables q(0) q(1) q(2) q(3)
UnitQuaternionConstraint described as 'Unit quaternion constraint for body body2' with 4 decision variables q(7) q(8) q(9) q(10)
PositionConstraint with 14 decision variables q(0) q(1) q(2) q(3) q(4) q(5) q(6) q(7) q(8) q(9) q(10) q(11) q(12) q(13)
BoundingBoxConstraint described as 'Joint limits'
-1 <= q(0) <= 1
-1 <= q(1) <= 1
-1 <= q(2) <= 1
-1 <= q(3) <= 1
-inf <= q(4) <= inf
-inf <= q(5) <= inf
-inf <= q(6) <= inf
-1 <= q(7) <= 1
-1 <= q(8) <= 1
-1 <= q(9) <= 1
-1 <= q(10) <= 1
-inf <= q(11) <= inf
-inf <= q(12) <= inf
-inf <= q(13) <= inf

in other words... the only change is the additional [-1,1] constraints on the unit quaternion values. Both the initial guess and the solution in the initial problem satisfy those constraints. but somehow adding these obviously valid constraints makes the solver fail... :-( If i remove the additional [-1, 1] constraints, then the test passes again.

this is why i prefer convex optimization (although those solvers aren't totally immune)

@RussTedrake
Copy link
Contributor Author

i'm actually legitimately torn on what to do here. the cop-out answer would be to not add those constraints. but i consider this extremely bad behavior by ipopt (or our wrapper). i really don't want to weaken the algorithm to accommodate it.

@RussTedrake
Copy link
Contributor Author

even if I add

  VectorX<double> q_init(14);
  q_init << 0.999766148184183, 0.01681001705351731, -0.01355465403353861,
      0.001159148676400541, -0.1431276280275835, -0.2298639169407503,
      -0.3936192847140234, 0.9986453104060482, -0.03200385491261028,
      0.03988872632365753, -0.009601395055742623, 0.1431276280276386,
      0.229863916940848, 0.3936192847138122;
  ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_init);

to set the initial guess... it still fails. maybe we really do have a bug in the solver wrapper...?

@RussTedrake
Copy link
Contributor Author

continuing to boil this down (I've commented out additional constraints, etc to make the programs in inverse_kinematics_test and unit_quaternion_test almost equivalent). I'm seeing

[ RUN      ] TwoFreeBodiesTest.PositionConstraint
Decision variables:  q(0)  q(1)  q(2)  q(3)  q(4)  q(5)  q(6)  q(7)  q(8)  q(9) q(10) q(11) q(12) q(13)

UnitQuaternionConstraint described as 'Unit quaternion constraint for body body1' with 4 decision variables q(0) q(1) q(2) q(3)
UnitQuaternionConstraint described as 'Unit quaternion constraint for body body2' with 4 decision variables q(7) q(8) q(9) q(10)
BoundingBoxConstraint described as 'Joint limits'
-1 <= q(0) <= 1
-1 <= q(1) <= 1
-1 <= q(2) <= 1
-1 <= q(3) <= 1
-inf <= q(4) <= inf
-inf <= q(5) <= inf
-inf <= q(6) <= inf
-1 <= q(7) <= 1
-1 <= q(8) <= 1
-1 <= q(9) <= 1
-1 <= q(10) <= 1
-inf <= q(11) <= inf
-inf <= q(12) <= inf
-inf <= q(13) <= inf

q_init:   1   0   0   0 nan nan nan   1   0   0   0 nan nan nan
q_sol: 1 0 0 0 0 0 0 1 0 0 0 0 0 0

which succeeds and looks reasonable (but has no PositionConstraint), and

[ RUN      ] TwoFreeBodiesConstraintTest.AddUnitQuaternionConstraintOnPlant
Decision variables:  x(0)  x(1)  x(2)  x(3)  x(4)  x(5)  x(6)  x(7)  x(8)  x(9) x(10) x(11) x(12) x(13)

UnitQuaternionConstraint with 4 decision variables x(0) x(1) x(2) x(3)
UnitQuaternionConstraint with 4 decision variables x(7) x(8) x(9) x(10)
BoundingBoxConstraint
-1 <= x(0) <= 1
-1 <= x(1) <= 1
-1 <= x(2) <= 1
-1 <= x(3) <= 1
BoundingBoxConstraint
-1 <= x(7) <= 1
-1 <= x(8) <= 1
-1 <= x(9) <= 1
-1 <= x(10) <= 1

q_init:   1   0   0   0 nan nan nan   1   0   0   0 nan nan nan
q_sol:  0.3735097862205261 -0.3735097862205261  0.3735097862205261 -0.7625426668669044                   0                   0                   0  0.3079553548249924  0.5909965652320961  0.5909965652320961  0.4545433083902376 

which succeeds and also looks sort of reasonable.. but it should be identical and I get numerically very different solutions. I don't understand why yet.

@RussTedrake
Copy link
Contributor Author

oops. there were there more zeros on the end of the last line.

q_sol:  0.3735097862205261 -0.3735097862205261  0.3735097862205261 -0.7625426668669044                   0                   0                   0  0.3079553548249924  0.5909965652320961  0.5909965652320961  0.4545433083902376                   0                   0                   0

@hongkai-dai
Copy link
Contributor

I've commented out additional constraints, etc to make the programs in inverse_kinematics_test and unit_quaternion_test almost equivalent

Do you have a working branch that the two tests are almost equivalent? I can look into IPOPT data structure to see where the differ.

@RussTedrake
Copy link
Contributor Author

Copy link
Contributor

@hongkai-dai hongkai-dai left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The reason why TwoFreeBodiesConstraintTest.AddUnitQuaternionConstraintOnPlant and TwoFreeBodiesTest.PositionConstraint get different result, is that AddUnitQuaternionConstraintOnPlant uses a different initial guess (not the one stored in its MathematicalProgram) in

Eigen::VectorXd q_val = Eigen::VectorXd::Zero(14);
q_val.head<4>() << 0.5, -0.5, 0.5, -1.;
q_val.segment<4>(7) << 1.0 / 3, 2.0 / 3, 2.0 / 3, 0.5;
EXPECT_EQ(prog.generic_constraints()[0].variables().rows(), 4);
EXPECT_EQ(prog.generic_constraints()[1].variables().rows(), 4);
const auto result = solvers::Solve(prog, q_val);
.

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers

Copy link
Contributor

@hongkai-dai hongkai-dai left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I played with the problem a little bit. With the bounds -1 <= quaternion <= 1, the PositionConstraint test get stuck at quat = [0.99977, ...]. The solver spends many many iterations at this value without making any change on the decision variables, until it reaches the iterations limit. I tried to change the bounds to -1.0001 <= quaternion <= 1.0001, and the test gets stuck at quat = [0.9998,...]. If I remove the bounds on the quaternion, then the solution is quat = [0.99976,...], which is very close to the bounds -1 <= quat <= 1.

Also if I change the bounds to -1.01 <= quaternion <= 1.01, then IPOPT can find the solution.

I suspect the reason is that as an interior point solver, IPOPT probably uses some barrier function to prevent the decision variable to get to the boundary of the constraints. This barrier function likely will explode near the boundary, with both large value and large gradient, making the optimization landscape very bad near the constraint boundary.

This makes me a little bit worrisome on using IPOPT. If the solution is really close to the boundary of the constraint, then we might not be able to find the solution.

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers

@RussTedrake
Copy link
Contributor Author

I agree. It's really bad. As a minimal repro that was faster to play with, I made

from pydrake.all import MathematicalProgram, IpoptSolver

solver = IpoptSolver()
prog = MathematicalProgram()
q = prog.NewContinuousVariables(4, "q")
prog.AddBoundingBoxConstraint(-1, 1, q)
prog.AddConstraint(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2 - 1 == 0)
result = solver.Solve(prog)
print(result.is_success())

(which returns False). Ouch.

Do you think there is any chance it's a problem in our solver interface?

@RussTedrake
Copy link
Contributor Author

oops, forgot to set an initial guess... now it returns properly.

from pydrake.all import MathematicalProgram, IpoptSolver

solver = IpoptSolver()
prog = MathematicalProgram()
q = prog.NewContinuousVariables(4, "q")
prog.AddBoundingBoxConstraint(-1, 1, q)
prog.AddConstraint(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2 - 1 == 0)
prog.SetInitialGuess(q, np.array([1,0,0,0]))
result = solver.Solve(prog)
print(result.is_success())
if (result.is_success()):
    print(result.GetSolution(q))

returns [1, 0, 0, 0], with no barrier issues. hrm...

Also moves the joint limits / joint locking logic from the IK
constructor to AddMultibodyPlantConstraints.

Towards RobotLocomotion#18917.
Copy link
Contributor Author

@RussTedrake RussTedrake left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok. after some experimenting, I've found the ipopt does much better if you give it some objective, almost any objective. I've added dummy objectives to the failing tests and they now pass. I'd say that's frustrating, but tolerable. PTAL.

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
release notes: feature This pull request contains a new feature
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants