Skip to content

Commit 669231e

Browse files
RussTedrakeRussTedrake
committed
Implements AddMultibodyPlantConstraints()
This method creates a mapping from the constraints used in MultibodyPlant/SAP to the constraints used in MathematicalProgram. Towards RobotLocomotion#18917.
1 parent 7d74241 commit 669231e

11 files changed

+286
-8
lines changed

‎bindings/pydrake/multibody/inverse_kinematics_py.cc

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
#include "drake/bindings/pydrake/common/sorted_pair_pybind.h"
66
#include "drake/bindings/pydrake/documentation_pybind.h"
77
#include "drake/bindings/pydrake/pydrake_pybind.h"
8+
#include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h"
89
#include "drake/multibody/inverse_kinematics/angle_between_vectors_constraint.h"
910
#include "drake/multibody/inverse_kinematics/angle_between_vectors_cost.h"
1011
#include "drake/multibody/inverse_kinematics/com_in_polyhedron_constraint.h"
@@ -165,6 +166,11 @@ PYBIND11_MODULE(inverse_kinematics, m) {
165166
.def("get_mutable_context", &Class::get_mutable_context,
166167
py_rvp::reference_internal, cls_doc.get_mutable_context.doc);
167168
}
169+
170+
m.def("AddMultibodyPlantConstraints", &AddMultibodyPlantConstraints,
171+
py::arg("plant"), py::arg("q"), py::arg("prog"), py::arg("plant_context"),
172+
doc.AddMultibodyPlantConstraints.doc);
173+
168174
{
169175
using Class = AngleBetweenVectorsConstraint;
170176
constexpr auto& cls_doc = doc.AngleBetweenVectorsConstraint;

‎bindings/pydrake/multibody/test/inverse_kinematics_test.py

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,14 @@ def _body2_xyz(self, q):
8181
# TODO(eric.cousineau): Replace with state indexing.
8282
return q[11:14]
8383

84+
def test_AddMultibodyPlantConstraints(self):
85+
bindings = ik.AddMultibodyPlantConstraints(
86+
plant=self.plant,
87+
q=self.q,
88+
prog=self.prog,
89+
plant_context=self.ik_two_bodies.context())
90+
self.assertEqual(len(bindings), 2)
91+
8492
def test_AddPositionConstraint1(self):
8593
p_BQ = np.array([0.2, 0.3, 0.5])
8694
p_AQ_lower = np.array([-0.1, -0.2, -0.3])

‎multibody/inverse_kinematics/BUILD.bazel

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ drake_cc_package_library(
1515
name = "inverse_kinematics",
1616
visibility = ["//visibility:public"],
1717
deps = [
18+
":add_multibody_plant_constraints",
1819
":constraint_relaxing_ik",
1920
":differential_inverse_kinematics",
2021
":differential_inverse_kinematics_integrator",
@@ -24,6 +25,17 @@ drake_cc_package_library(
2425
],
2526
)
2627

28+
drake_cc_library(
29+
name = "add_multibody_plant_constraints",
30+
srcs = ["add_multibody_plant_constraints.cc"],
31+
hdrs = ["add_multibody_plant_constraints.h"],
32+
deps = [
33+
":kinematic_evaluators",
34+
"//multibody/plant",
35+
"//solvers:mathematical_program",
36+
],
37+
)
38+
2739
drake_cc_library(
2840
name = "constraint_relaxing_ik",
2941
srcs = ["constraint_relaxing_ik.cc"],
@@ -150,6 +162,15 @@ drake_cc_library(
150162

151163
#============ Test ============================
152164

165+
drake_cc_googletest(
166+
name = "add_multibody_plant_constraints_test",
167+
srcs = ["test/add_multibody_plant_constraints_test.cc"],
168+
deps = [
169+
":add_multibody_plant_constraints",
170+
"//solvers:solve",
171+
],
172+
)
173+
153174
drake_cc_googletest(
154175
name = "constraint_relaxing_ik_test",
155176
srcs = ["test/constraint_relaxing_ik_test.cc"],

‎multibody/inverse_kinematics/add_multibody_plant_constraints.cc

Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
#include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h"
2+
3+
#include <memory>
4+
5+
#include "drake/multibody/inverse_kinematics/orientation_constraint.h"
6+
#include "drake/multibody/inverse_kinematics/point_to_point_distance_constraint.h"
7+
#include "drake/multibody/inverse_kinematics/position_constraint.h"
8+
#include "drake/multibody/inverse_kinematics/unit_quaternion_constraint.h"
9+
10+
namespace drake {
11+
namespace multibody {
12+
13+
using solvers::Binding;
14+
using solvers::Constraint;
15+
16+
std::vector<Binding<Constraint>> AddMultibodyPlantConstraints(
17+
const MultibodyPlant<double>& plant,
18+
const solvers::VectorXDecisionVariable& q,
19+
solvers::MathematicalProgram* prog,
20+
systems::Context<double>* plant_context) {
21+
DRAKE_THROW_UNLESS(prog != nullptr);
22+
std::vector<Binding<Constraint>> bindings =
23+
AddUnitQuaternionConstraintOnPlant(plant, q, prog);
24+
int num_multibody_constraints = 0;
25+
for (const auto& [id, spec] : plant.get_coupler_constraint_specs()) {
26+
bindings.emplace_back(prog->AddLinearEqualityConstraint(
27+
q[spec.joint0_index] ==
28+
spec.gear_ratio * q[spec.joint1_index] + spec.offset));
29+
++num_multibody_constraints;
30+
}
31+
for (const auto& [id, spec] : plant.get_distance_constraint_specs()) {
32+
DRAKE_THROW_UNLESS(plant_context != nullptr);
33+
// d(q) == d₀.
34+
bindings.emplace_back(prog->AddConstraint(
35+
std::make_shared<PointToPointDistanceConstraint>(
36+
&plant, plant.get_body(spec.body_A).body_frame(), spec.p_AP,
37+
plant.get_body(spec.body_B).body_frame(), spec.p_BQ, spec.distance,
38+
spec.distance, plant_context),
39+
q));
40+
++num_multibody_constraints;
41+
}
42+
for (const auto& [id, spec] : plant.get_ball_constraint_specs()) {
43+
DRAKE_THROW_UNLESS(plant_context != nullptr);
44+
bindings.emplace_back(prog->AddConstraint(
45+
std::make_shared<PositionConstraint>(
46+
&plant, plant.get_body(spec.body_A).body_frame(), spec.p_AP,
47+
spec.p_AP, plant.get_body(spec.body_B).body_frame(), *spec.p_BQ,
48+
plant_context),
49+
q));
50+
++num_multibody_constraints;
51+
}
52+
for (const auto& [id, spec] : plant.get_weld_constraint_specs()) {
53+
DRAKE_THROW_UNLESS(plant_context != nullptr);
54+
// TODO(russt): Consider implementing a WeldConstraint.
55+
bindings.emplace_back(prog->AddConstraint(
56+
std::make_shared<PositionConstraint>(
57+
&plant, plant.get_body(spec.body_A).body_frame(),
58+
spec.X_AP.translation(), spec.X_AP.translation(),
59+
plant.get_body(spec.body_B).body_frame(), spec.X_BQ.translation(),
60+
plant_context),
61+
q));
62+
bindings.emplace_back(prog->AddConstraint(
63+
std::make_shared<OrientationConstraint>(
64+
&plant, plant.get_body(spec.body_A).body_frame(),
65+
spec.X_AP.rotation(), plant.get_body(spec.body_B).body_frame(),
66+
spec.X_BQ.rotation(), 0.0, plant_context),
67+
q));
68+
++num_multibody_constraints;
69+
}
70+
if (num_multibody_constraints != plant.num_constraints()) {
71+
// TODO(russt): It would be better to say something specific about the
72+
// unidentified constraint here, but the constraint API for MultibodyPlant
73+
// does not have that surface area (yet).
74+
log()->warn(
75+
"plant.num_constraints() = {}, but only AddMultibodyPlantConstraints() "
76+
"only captured {} of them. This method currently supports Coupler, "
77+
"Distance, Ball, and Weld multibody constraints. If you're seeing "
78+
"this, MultibodyPlant may have gained a new constraint type that is "
79+
"not supported here yet (but perhaps should be)",
80+
plant.num_constraints(), num_multibody_constraints);
81+
}
82+
return bindings;
83+
}
84+
85+
} // namespace multibody
86+
} // namespace drake

‎multibody/inverse_kinematics/add_multibody_plant_constraints.h

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#pragma once
2+
3+
#include <vector>
4+
5+
#include "drake/multibody/plant/multibody_plant.h"
6+
#include "drake/solvers/mathematical_program.h"
7+
8+
namespace drake {
9+
namespace multibody {
10+
11+
/** For unit quaternion and (holonomic) constraints registered with `plant` adds
12+
a corresponding solver::Constraint to `prog`, using decision variables `q` to
13+
represent the generalized positions of the plant.
14+
15+
Adds constraints for coupler, distance, ball, and weld constraints. The
16+
distance constraint is implemented here as a hard kinematic constraint (i.e.,
17+
d(q) == d₀), instead of a soft "spring" force.
18+
19+
@see AddUnitQuaternionConstraintOnPlant() for the unit quaternion constraints.
20+
21+
@pre plant.is_finalized() == true.
22+
@throws std::exception if `prog` is nullptr.
23+
@throws std::exception if `plant_context` is nullptr and one of the
24+
MultibodyPlant constraints requires it. (unit quaternion constraints and coupler
25+
constraints do not).
26+
*/
27+
std::vector<solvers::Binding<solvers::Constraint>> AddMultibodyPlantConstraints(
28+
const MultibodyPlant<double>& plant,
29+
const solvers::VectorXDecisionVariable& q,
30+
solvers::MathematicalProgram* prog,
31+
systems::Context<double>* plant_context = nullptr);
32+
33+
} // namespace multibody
34+
} // namespace drake

‎multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
#include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h"
2+
3+
#include <gtest/gtest.h>
4+
5+
#include "drake/multibody/tree/revolute_joint.h"
6+
#include "drake/solvers/solve.h"
7+
8+
namespace drake {
9+
namespace multibody {
10+
namespace {
11+
12+
using Eigen::MatrixXd;
13+
using Eigen::Vector3d;
14+
using Eigen::VectorXd;
15+
using math::RigidTransformd;
16+
17+
class AddMultibodyPlantConstraintsTest : public ::testing::Test {
18+
public:
19+
void SetUp() override {
20+
plant_.set_discrete_contact_approximation(
21+
DiscreteContactApproximation::kSap);
22+
plant_.SetUseSampledOutputPorts(false);
23+
24+
body_A_ = &plant_.AddRigidBody(
25+
"body_A", SpatialInertia<double>::SolidBoxWithMass(1, 1, 1, 1));
26+
body_B_ = &plant_.AddRigidBody(
27+
"body_B", SpatialInertia<double>::SolidBoxWithMass(1, 1, 1, 1));
28+
world_A_ = &plant_.AddJoint<RevoluteJoint>(
29+
"world_A", plant_.world_body(), RigidTransformd(Vector3d(-1, 0, 0)),
30+
*body_A_, RigidTransformd(), Vector3d::UnitZ());
31+
A_B_ = &plant_.AddJoint<RevoluteJoint>(
32+
"A_B", *body_A_, RigidTransformd(Vector3d(1, 0, 0)), *body_B_,
33+
RigidTransformd(), Vector3d::UnitZ());
34+
}
35+
36+
void CheckConstraints(int expected_num_constraints = 1, double tol = 1e-6) {
37+
if (!plant_.is_finalized()) {
38+
plant_.Finalize();
39+
}
40+
plant_context_ = plant_.CreateDefaultContext();
41+
42+
// First demonstrate that these constraints work with MathematicalProgram,
43+
// and use them to find a valid initial condition.
44+
solvers::MathematicalProgram prog;
45+
auto q = prog.NewContinuousVariables(plant_.num_positions());
46+
VectorXd q0 = VectorXd::LinSpaced(plant_.num_positions(), 0, 1);
47+
prog.AddQuadraticErrorCost(
48+
MatrixXd::Identity(plant_.num_positions(), plant_.num_positions()), q0,
49+
q);
50+
51+
auto constraints =
52+
AddMultibodyPlantConstraints(plant_, q, &prog, plant_context_.get());
53+
EXPECT_EQ(constraints.size(), expected_num_constraints);
54+
auto result = solvers::Solve(prog);
55+
EXPECT_TRUE(result.is_success());
56+
57+
// Now step the dynamics forward and verify that the constraint stays
58+
// satisfied.
59+
plant_.SetPositions(plant_context_.get(), result.GetSolution(q));
60+
VectorXd qn = plant_.EvalUniquePeriodicDiscreteUpdate(*plant_context_)
61+
.value()
62+
.head(plant_.num_positions());
63+
64+
prog.SetInitialGuessForAllVariables(qn);
65+
EXPECT_TRUE(prog.CheckSatisfiedAtInitialGuess(constraints, tol));
66+
67+
result.set_x_val(qn);
68+
log()->info(
69+
"Infeasible constraints:\n{}",
70+
fmt::join(result.GetInfeasibleConstraintNames(prog, tol), "\n"));
71+
}
72+
73+
protected:
74+
MultibodyPlant<double> plant_{0.1};
75+
std::unique_ptr<systems::Context<double>> plant_context_{};
76+
const RigidBody<double>* body_A_{nullptr};
77+
const RigidBody<double>* body_B_{nullptr};
78+
const RevoluteJoint<double>* world_A_{nullptr};
79+
const RevoluteJoint<double>* A_B_{nullptr};
80+
};
81+
82+
TEST_F(AddMultibodyPlantConstraintsTest, CouplerConstraint) {
83+
plant_.AddCouplerConstraint(*world_A_, *A_B_, 2.3);
84+
CheckConstraints();
85+
86+
// Confirm that I also could have called the method with plant_context ==
87+
// nullptr.
88+
solvers::MathematicalProgram prog;
89+
auto q = prog.NewContinuousVariables(plant_.num_positions());
90+
EXPECT_NO_THROW(AddMultibodyPlantConstraints(plant_, q, &prog));
91+
}
92+
93+
TEST_F(AddMultibodyPlantConstraintsTest, DistanceConstraint) {
94+
plant_.AddDistanceConstraint(*body_A_, Vector3d(0.0, 1.0, 0.0), *body_B_,
95+
Vector3d(0.0, 1.0, 0.0), 1.5);
96+
CheckConstraints();
97+
}
98+
99+
TEST_F(AddMultibodyPlantConstraintsTest, BallConstraint) {
100+
plant_.AddBallConstraint(*body_A_, Vector3d(0.0, 2.0, 0.0), *body_B_);
101+
CheckConstraints();
102+
}
103+
104+
TEST_F(AddMultibodyPlantConstraintsTest, WeldConstraint) {
105+
const RigidBody<double>& body_C = plant_.AddRigidBody(
106+
"body_C", SpatialInertia<double>::SolidBoxWithMass(1, 1, 1, 1));
107+
plant_.AddWeldConstraint(*body_A_, RigidTransformd(Vector3d(1, 2, 3)), body_C,
108+
RigidTransformd(Vector3d(4, 5, 6)));
109+
CheckConstraints(/* expected_num_constraints = */ 3, /* tol = */ 1e-2);
110+
}
111+
112+
} // namespace
113+
} // namespace multibody
114+
} // namespace drake

‎multibody/inverse_kinematics/test/unit_quaternion_constraint_test.cc

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,8 @@ TEST_F(TwoFreeBodiesConstraintTest, AddUnitQuaternionConstraintOnPlant) {
5050
prog.SetInitialGuess(
5151
q.segment<4>(plant_->GetBodyByName("body1").floating_positions_start()),
5252
body1_guess);
53-
AddUnitQuaternionConstraintOnPlant(*plant_, q, &prog);
53+
auto bindings = AddUnitQuaternionConstraintOnPlant(*plant_, q, &prog);
54+
EXPECT_EQ(bindings.size(), 2);
5455
EXPECT_EQ(prog.generic_constraints().size(), 2);
5556
// Confirm that body 1's non-default initial guess was not overwritten.
5657
EXPECT_TRUE(CompareMatrices(

‎multibody/inverse_kinematics/unit_quaternion_constraint.cc

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,24 +24,28 @@ void UnitQuaternionConstraint::DoEval(
2424
}
2525

2626
template <typename T>
27-
void AddUnitQuaternionConstraintOnPlant(
27+
std::vector<solvers::Binding<solvers::Constraint>>
28+
AddUnitQuaternionConstraintOnPlant(
2829
const MultibodyPlant<T>& plant,
2930
const Eigen::Ref<const VectorX<symbolic::Variable>>& q_vars,
3031
solvers::MathematicalProgram* prog) {
3132
DRAKE_DEMAND(q_vars.rows() == plant.num_positions());
33+
std::vector<solvers::Binding<solvers::Constraint>> bindings;
3234
// Loop through each body
3335
for (BodyIndex body_index{0}; body_index < plant.num_bodies(); ++body_index) {
3436
const auto& body = plant.get_body(body_index);
3537
if (body.has_quaternion_dofs()) {
3638
Vector4<symbolic::Variable> quat_vars =
3739
q_vars.segment<4>(body.floating_positions_start());
38-
prog->AddConstraint(solvers::Binding<solvers::Constraint>(
39-
std::make_shared<UnitQuaternionConstraint>(), quat_vars));
40+
bindings.emplace_back(
41+
prog->AddConstraint(solvers::Binding<solvers::Constraint>(
42+
std::make_shared<UnitQuaternionConstraint>(), quat_vars)));
4043
if (prog->GetInitialGuess(quat_vars).array().isNaN().all()) {
4144
prog->SetInitialGuess(quat_vars, Eigen::Vector4d(1, 0, 0, 0));
4245
}
4346
}
4447
}
48+
return bindings;
4549
}
4650

4751
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((

‎multibody/inverse_kinematics/unit_quaternion_constraint.h

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#pragma once
22

33
#include <memory>
4+
#include <vector>
45

56
#include "drake/multibody/plant/multibody_plant.h"
67
#include "drake/solvers/constraint.h"
@@ -63,7 +64,8 @@ class UnitQuaternionConstraint : public solvers::Constraint {
6364
@tparam_default_scalar
6465
*/
6566
template <typename T>
66-
void AddUnitQuaternionConstraintOnPlant(
67+
std::vector<solvers::Binding<solvers::Constraint>>
68+
AddUnitQuaternionConstraintOnPlant(
6769
const MultibodyPlant<T>& plant,
6870
const Eigen::Ref<const VectorX<symbolic::Variable>>& q_vars,
6971
solvers::MathematicalProgram* prog);

‎multibody/plant/constraint_specs.h

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,10 @@ struct CouplerConstraintSpec {
3737

3838
// Struct to store the specification for a distance constraint. A distance
3939
// constraint is modeled as a holonomic constraint. Distance constraints can
40-
// be "soft" which imposes the the condition:
41-
// (d(q)-d₀) + c/k⋅ḋ(q) + 1/k⋅f = 0
40+
// be "soft", and are implemented as a spring force, f:
41+
// f = -k⋅(d(q) - d₀) - c⋅ḋ(q),
42+
// which is implemented as the condition:
43+
// (d(q)-d₀) + c/k⋅ḋ(q) + 1/k⋅f = 0,
4244
// where d₀ is a fixed length, k a stiffness parameter in N/m and c a damping
4345
// parameter in N⋅s/m. We use d(q) to denote the Euclidean distance between two
4446
// points P and Q, rigidly affixed to bodies A and B respectively, as a function

‎multibody/plant/test/multibody_plant_test.cc

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4862,7 +4862,7 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) {
48624862
}
48634863

48644864
GTEST_TEST(MultibodyPlantTests, GetConstraintIds) {
4865-
// Set up a plant with 3 constraints with arbitrary parameters.
4865+
// Set up a plant with each constraint type with arbitrary parameters.
48664866
MultibodyPlant<double> plant(0.01);
48674867

48684868
EXPECT_EQ(plant.GetConstraintIds().size(), 0);

0 commit comments

Comments
 (0)