-
Notifications
You must be signed in to change notification settings - Fork 1.3k
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
base: master
Are you sure you want to change the base?
Conversation
There was a problem hiding this 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
There was a problem hiding this 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()));
There was a problem hiding this 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
andub
) 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.
Previously, RussTedrake (Russ Tedrake) wrote…
@frankpermenter -- curious what you think? if you have a constraint of the form |
There was a problem hiding this 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
andub
) 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!
There was a problem hiding this 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 solvera <= vars <= a
oreye @ 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.
There was a problem hiding this 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 thatA == eye
inA @ vars == b
, so I'm not entirely convinced.
It seems detecting A==eye
is harder than detecting lb==ub
?
There was a problem hiding this 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 detectinglb==ub
?
But of course you can substitute Ax == b into other constraints easily enough.
@drake-jenkins-bot retest this, please. |
7562f89
to
0ebcdfe
Compare
There was a problem hiding this 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.
There was a problem hiding this 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
a562e54
to
ff5dd8b
Compare
There was a problem hiding this 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.
6eecee4
to
b4a3961
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
that's actually terrible. For inverse_kinematics_test the difference is only the new [-1,1] constraints. Old program
and new program
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) |
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. |
even if I add
to set the initial guess... it still fails. maybe we really do have a bug in the solver wrapper...? |
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
which succeeds and looks reasonable (but has no PositionConstraint), and
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. |
oops. there were there more zeros on the end of the last line.
|
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. |
I've pushed it here: https://github.com/RussTedrake/drake/pull/new/ik_mbp_constraints_ipopt_debugging |
There was a problem hiding this 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
drake/multibody/inverse_kinematics/test/unit_quaternion_constraint_test.cc
Lines 67 to 72 in 8df4db9
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
There was a problem hiding this 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
b4a3961
to
e2bd4f8
Compare
I agree. It's really bad. As a minimal repro that was faster to play with, I made
(which returns False). Ouch. Do you think there is any chance it's a problem in our solver interface? |
oops, forgot to set an initial guess... now it returns properly.
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.
e2bd4f8
to
a685dcc
Compare
There was a problem hiding this 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
Also moves the joint limits / joint locking logic from the IK constructor to AddMultibodyPlantConstraints.
Towards #18917.
This change is