From 2006b5569f315b9f71cea8038a459a48421ce670 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Tue, 17 Dec 2024 22:47:59 -0500 Subject: [PATCH] Implements AddMultibodyPlantConstraints() This method creates a mapping from the constraints used in MultibodyPlant/SAP to the constraints used in MathematicalProgram. Towards #18917. --- .../multibody/inverse_kinematics_py.cc | 6 + .../multibody/test/inverse_kinematics_test.py | 8 ++ multibody/inverse_kinematics/BUILD.bazel | 22 +++ .../add_multibody_plant_constraints.cc | 87 ++++++++++++ .../add_multibody_plant_constraints.h | 36 +++++ .../add_multibody_plant_constraints_test.cc | 129 ++++++++++++++++++ .../test/unit_quaternion_constraint_test.cc | 3 +- .../unit_quaternion_constraint.cc | 10 +- .../unit_quaternion_constraint.h | 4 +- multibody/plant/constraint_specs.h | 4 +- multibody/plant/test/multibody_plant_test.cc | 2 +- 11 files changed, 303 insertions(+), 8 deletions(-) create mode 100644 multibody/inverse_kinematics/add_multibody_plant_constraints.cc create mode 100644 multibody/inverse_kinematics/add_multibody_plant_constraints.h create mode 100644 multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc diff --git a/bindings/pydrake/multibody/inverse_kinematics_py.cc b/bindings/pydrake/multibody/inverse_kinematics_py.cc index ed8f9ad3c532..08b068b4de1a 100644 --- a/bindings/pydrake/multibody/inverse_kinematics_py.cc +++ b/bindings/pydrake/multibody/inverse_kinematics_py.cc @@ -5,6 +5,7 @@ #include "drake/bindings/pydrake/common/sorted_pair_pybind.h" #include "drake/bindings/pydrake/documentation_pybind.h" #include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h" #include "drake/multibody/inverse_kinematics/angle_between_vectors_constraint.h" #include "drake/multibody/inverse_kinematics/angle_between_vectors_cost.h" #include "drake/multibody/inverse_kinematics/com_in_polyhedron_constraint.h" @@ -165,6 +166,11 @@ PYBIND11_MODULE(inverse_kinematics, m) { .def("get_mutable_context", &Class::get_mutable_context, py_rvp::reference_internal, cls_doc.get_mutable_context.doc); } + + m.def("AddMultibodyPlantConstraints", &AddMultibodyPlantConstraints, + py::arg("plant"), py::arg("q"), py::arg("prog"), py::arg("plant_context"), + doc.AddMultibodyPlantConstraints.doc); + { using Class = AngleBetweenVectorsConstraint; constexpr auto& cls_doc = doc.AngleBetweenVectorsConstraint; diff --git a/bindings/pydrake/multibody/test/inverse_kinematics_test.py b/bindings/pydrake/multibody/test/inverse_kinematics_test.py index ed8b714d233d..cdc9a724ac58 100644 --- a/bindings/pydrake/multibody/test/inverse_kinematics_test.py +++ b/bindings/pydrake/multibody/test/inverse_kinematics_test.py @@ -81,6 +81,14 @@ def _body2_xyz(self, q): # TODO(eric.cousineau): Replace with state indexing. return q[11:14] + def test_AddMultibodyPlantConstraints(self): + bindings = ik.AddMultibodyPlantConstraints( + plant=self.plant, + q=self.q, + prog=self.prog, + plant_context=self.ik_two_bodies.context()) + self.assertEqual(len(bindings), 2) + def test_AddPositionConstraint1(self): p_BQ = np.array([0.2, 0.3, 0.5]) p_AQ_lower = np.array([-0.1, -0.2, -0.3]) diff --git a/multibody/inverse_kinematics/BUILD.bazel b/multibody/inverse_kinematics/BUILD.bazel index eada9d1a6719..7b62464355e4 100644 --- a/multibody/inverse_kinematics/BUILD.bazel +++ b/multibody/inverse_kinematics/BUILD.bazel @@ -15,6 +15,7 @@ drake_cc_package_library( name = "inverse_kinematics", visibility = ["//visibility:public"], deps = [ + ":add_multibody_plant_constraints", ":constraint_relaxing_ik", ":differential_inverse_kinematics", ":differential_inverse_kinematics_integrator", @@ -24,6 +25,17 @@ drake_cc_package_library( ], ) +drake_cc_library( + name = "add_multibody_plant_constraints", + srcs = ["add_multibody_plant_constraints.cc"], + hdrs = ["add_multibody_plant_constraints.h"], + deps = [ + ":kinematic_evaluators", + "//multibody/plant", + "//solvers:mathematical_program", + ], +) + drake_cc_library( name = "constraint_relaxing_ik", srcs = ["constraint_relaxing_ik.cc"], @@ -150,6 +162,16 @@ drake_cc_library( #============ Test ============================ +drake_cc_googletest( + name = "add_multibody_plant_constraints_test", + srcs = ["test/add_multibody_plant_constraints_test.cc"], + deps = [ + ":add_multibody_plant_constraints", + "//solvers:snopt_solver", + "//solvers:solve", + ], +) + drake_cc_googletest( name = "constraint_relaxing_ik_test", srcs = ["test/constraint_relaxing_ik_test.cc"], diff --git a/multibody/inverse_kinematics/add_multibody_plant_constraints.cc b/multibody/inverse_kinematics/add_multibody_plant_constraints.cc new file mode 100644 index 000000000000..585cd80a6128 --- /dev/null +++ b/multibody/inverse_kinematics/add_multibody_plant_constraints.cc @@ -0,0 +1,87 @@ +#include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h" + +#include + +#include "drake/multibody/inverse_kinematics/orientation_constraint.h" +#include "drake/multibody/inverse_kinematics/point_to_point_distance_constraint.h" +#include "drake/multibody/inverse_kinematics/position_constraint.h" +#include "drake/multibody/inverse_kinematics/unit_quaternion_constraint.h" + +namespace drake { +namespace multibody { + +using solvers::Binding; +using solvers::Constraint; + +std::vector> AddMultibodyPlantConstraints( + const MultibodyPlant& plant, + const solvers::VectorXDecisionVariable& q, + solvers::MathematicalProgram* prog, + systems::Context* plant_context) { + DRAKE_THROW_UNLESS(prog != nullptr); + std::vector> bindings = + AddUnitQuaternionConstraintOnPlant(plant, q, prog); + for (const auto& [id, spec] : plant.get_coupler_constraint_specs()) { + const int q0_index = plant.get_joint(spec.joint0_index).position_start(); + const int q1_index = plant.get_joint(spec.joint1_index).position_start(); + bindings.emplace_back(prog->AddLinearEqualityConstraint( + q[q0_index] == spec.gear_ratio * q[q1_index] + spec.offset)); + } + for (const auto& [id, spec] : plant.get_distance_constraint_specs()) { + DRAKE_THROW_UNLESS(plant_context != nullptr); + // d(q) == d₀. + bindings.emplace_back(prog->AddConstraint( + std::make_shared( + &plant, plant.get_body(spec.body_A).body_frame(), spec.p_AP, + plant.get_body(spec.body_B).body_frame(), spec.p_BQ, spec.distance, + spec.distance, plant_context), + q)); + } + for (const auto& [id, spec] : plant.get_ball_constraint_specs()) { + DRAKE_THROW_UNLESS(plant_context != nullptr); + bindings.emplace_back(prog->AddConstraint( + std::make_shared( + &plant, plant.get_body(spec.body_A).body_frame(), spec.p_AP, + spec.p_AP, plant.get_body(spec.body_B).body_frame(), *spec.p_BQ, + plant_context), + q)); + } + for (const auto& [id, spec] : plant.get_weld_constraint_specs()) { + DRAKE_THROW_UNLESS(plant_context != nullptr); + // TODO(russt): Consider implementing a WeldConstraint. + bindings.emplace_back(prog->AddConstraint( + std::make_shared( + &plant, plant.get_body(spec.body_A).body_frame(), + spec.X_AP.translation(), spec.X_AP.translation(), + plant.get_body(spec.body_B).body_frame(), spec.X_BQ.translation(), + plant_context), + q)); + bindings.emplace_back(prog->AddConstraint( + std::make_shared( + &plant, plant.get_body(spec.body_A).body_frame(), + spec.X_AP.rotation(), plant.get_body(spec.body_B).body_frame(), + spec.X_BQ.rotation(), 0.0, plant_context), + q)); + } + // AddMultibodyPlantConstraints only supports coupler, ball, distance, and + // weld. + int num_supported_constraints = + plant.num_coupler_constraints() + plant.num_ball_constraints() + + plant.num_distance_constraints() + plant.num_weld_constraints(); + if (num_supported_constraints != plant.num_constraints()) { + // TODO(russt): It would be better to say something specific about the + // unidentified constraint here, but the constraint API for MultibodyPlant + // does not have that surface area (yet). + throw std::runtime_error(fmt::format( + "plant.num_constraints() = {}, but only AddMultibodyPlantConstraints() " + "only captured {} of them. This method currently supports Coupler, " + "Distance, Ball, and Weld multibody constraints. If you're seeing " + "this, MultibodyPlant must have gained a new constraint type that is " + "not supported here yet (but likely should be)", + plant.num_constraints(), num_supported_constraints)); + } + return bindings; +} + +} // namespace multibody +} // namespace drake diff --git a/multibody/inverse_kinematics/add_multibody_plant_constraints.h b/multibody/inverse_kinematics/add_multibody_plant_constraints.h new file mode 100644 index 000000000000..63d316753d9f --- /dev/null +++ b/multibody/inverse_kinematics/add_multibody_plant_constraints.h @@ -0,0 +1,36 @@ +#pragma once + +#include + +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/solvers/mathematical_program.h" + +namespace drake { +namespace multibody { + +/** For unit quaternion and (holonomic) constraints registered with `plant` adds +a corresponding solver::Constraint to `prog`, using decision variables `q` to +represent the generalized positions of the plant. + +Adds constraints for coupler, distance, ball, and weld constraints. The +distance constraint is implemented here as a hard kinematic constraint (i.e., +d(q) == d₀), instead of a soft "spring" force. + +@see AddUnitQuaternionConstraintOnPlant() for the unit quaternion constraints. + +@pre plant.is_finalized() == true. +@throws std::exception if `plant` has constraints registered that are not yet +supported by this method. +@throws std::exception if `prog` is nullptr. +@throws std::exception if `plant_context` is nullptr and one of the +MultibodyPlant constraints requires it. (unit quaternion constraints and coupler +constraints do not). +*/ +std::vector> AddMultibodyPlantConstraints( + const MultibodyPlant& plant, + const solvers::VectorXDecisionVariable& q, + solvers::MathematicalProgram* prog, + systems::Context* plant_context = nullptr); + +} // namespace multibody +} // namespace drake diff --git a/multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc b/multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc new file mode 100644 index 000000000000..b4116d88f1c6 --- /dev/null +++ b/multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc @@ -0,0 +1,129 @@ +#include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h" + +#include + +#include "drake/multibody/tree/revolute_joint.h" +#include "drake/solvers/snopt_solver.h" +#include "drake/solvers/solve.h" + +namespace drake { +namespace multibody { +namespace { + +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::VectorXd; +using math::RigidTransformd; + +bool SnoptSolverUnavailable() { + return !(solvers::SnoptSolver::is_available() && + solvers::SnoptSolver::is_enabled()); +} + +class AddMultibodyPlantConstraintsTest : public ::testing::Test { + public: + void SetUp() override { + plant_.set_discrete_contact_approximation( + DiscreteContactApproximation::kSap); + plant_.SetUseSampledOutputPorts(false); + + body_A_ = &plant_.AddRigidBody( + "body_A", SpatialInertia::SolidBoxWithMass(1, 1, 1, 1)); + body_B_ = &plant_.AddRigidBody( + "body_B", SpatialInertia::SolidBoxWithMass(1, 1, 1, 1)); + world_A_ = &plant_.AddJoint( + "world_A", plant_.world_body(), RigidTransformd(Vector3d(-1, 0, 0)), + *body_A_, RigidTransformd(), Vector3d::UnitZ()); + // Add and then immediately remove a joint so that the joint indices do not + // correspond to the position indices. + plant_.RemoveJoint(plant_.AddJoint( + "temp", *body_A_, RigidTransformd(Vector3d(1, 0, 0)), *body_B_, + RigidTransformd(), Vector3d::UnitZ())); + A_B_ = &plant_.AddJoint( + "A_B", *body_A_, RigidTransformd(Vector3d(1, 0, 0)), *body_B_, + RigidTransformd(), Vector3d::UnitZ()); + } + + void CheckConstraints(int expected_num_constraints = 1, double tol = 1e-6) { + if (!plant_.is_finalized()) { + plant_.Finalize(); + } + plant_context_ = plant_.CreateDefaultContext(); + + // First demonstrate that these constraints work with MathematicalProgram, + // and use them to find a valid initial condition. + solvers::MathematicalProgram prog; + auto q = prog.NewContinuousVariables(plant_.num_positions()); + VectorXd q0 = VectorXd::LinSpaced(plant_.num_positions(), 0, 1); + prog.AddQuadraticErrorCost( + MatrixXd::Identity(plant_.num_positions(), plant_.num_positions()), q0, + q); + + auto constraints = + AddMultibodyPlantConstraints(plant_, q, &prog, plant_context_.get()); + EXPECT_EQ(constraints.size(), expected_num_constraints); + auto result = solvers::Solve(prog); + EXPECT_TRUE(result.is_success()); + + // Now step the dynamics forward and verify that the constraint stays + // satisfied. + plant_.SetPositions(plant_context_.get(), result.GetSolution(q)); + VectorXd qn = plant_.EvalUniquePeriodicDiscreteUpdate(*plant_context_) + .value() + .head(plant_.num_positions()); + + prog.SetInitialGuessForAllVariables(qn); + EXPECT_TRUE(prog.CheckSatisfiedAtInitialGuess(constraints, tol)); + } + + protected: + MultibodyPlant plant_{0.1}; + std::unique_ptr> plant_context_{}; + const RigidBody* body_A_{nullptr}; + const RigidBody* body_B_{nullptr}; + const RevoluteJoint* world_A_{nullptr}; + const RevoluteJoint* A_B_{nullptr}; +}; + +TEST_F(AddMultibodyPlantConstraintsTest, CouplerConstraint) { + plant_.AddCouplerConstraint(*world_A_, *A_B_, 2.3); + CheckConstraints(); + + // Confirm that I also could have called the method with plant_context == + // nullptr. + solvers::MathematicalProgram prog; + auto q = prog.NewContinuousVariables(plant_.num_positions()); + EXPECT_NO_THROW(AddMultibodyPlantConstraints(plant_, q, &prog)); +} + +TEST_F(AddMultibodyPlantConstraintsTest, DistanceConstraint) { + plant_.AddDistanceConstraint(*body_A_, Vector3d(0.0, 1.0, 0.0), *body_B_, + Vector3d(0.0, 1.0, 0.0), 1.5); + CheckConstraints(); +} + +TEST_F(AddMultibodyPlantConstraintsTest, BallConstraint) { + plant_.AddBallConstraint(*body_A_, Vector3d(0.0, 2.0, 0.0), *body_B_); + if (SnoptSolverUnavailable()) { + // IPOPT is flakey on this test. + return; + } + CheckConstraints(); +} + +TEST_F(AddMultibodyPlantConstraintsTest, WeldConstraint) { + const RigidBody& body_C = plant_.AddRigidBody( + "body_C", SpatialInertia::SolidBoxWithMass(1, 1, 1, 1)); + plant_.AddWeldConstraint(*body_A_, RigidTransformd(Vector3d(1, 2, 3)), body_C, + RigidTransformd(Vector3d(4, 5, 6))); + if (SnoptSolverUnavailable()) { + // IPOPT is flakey on this test. + return; + } + const int expected_num_constraints = 4; // 2 for quaternion, 2 for weld. + CheckConstraints(expected_num_constraints, /* tol = */ 1e-2); +} + +} // namespace +} // namespace multibody +} // namespace drake diff --git a/multibody/inverse_kinematics/test/unit_quaternion_constraint_test.cc b/multibody/inverse_kinematics/test/unit_quaternion_constraint_test.cc index 2b876a8a9e3a..2be317a7083b 100644 --- a/multibody/inverse_kinematics/test/unit_quaternion_constraint_test.cc +++ b/multibody/inverse_kinematics/test/unit_quaternion_constraint_test.cc @@ -50,7 +50,8 @@ TEST_F(TwoFreeBodiesConstraintTest, AddUnitQuaternionConstraintOnPlant) { prog.SetInitialGuess( q.segment<4>(plant_->GetBodyByName("body1").floating_positions_start()), body1_guess); - AddUnitQuaternionConstraintOnPlant(*plant_, q, &prog); + auto bindings = AddUnitQuaternionConstraintOnPlant(*plant_, q, &prog); + EXPECT_EQ(bindings.size(), 2); EXPECT_EQ(prog.generic_constraints().size(), 2); // Confirm that body 1's non-default initial guess was not overwritten. EXPECT_TRUE(CompareMatrices( diff --git a/multibody/inverse_kinematics/unit_quaternion_constraint.cc b/multibody/inverse_kinematics/unit_quaternion_constraint.cc index bb1baf091517..89a519b78052 100644 --- a/multibody/inverse_kinematics/unit_quaternion_constraint.cc +++ b/multibody/inverse_kinematics/unit_quaternion_constraint.cc @@ -24,24 +24,28 @@ void UnitQuaternionConstraint::DoEval( } template -void AddUnitQuaternionConstraintOnPlant( +std::vector> +AddUnitQuaternionConstraintOnPlant( const MultibodyPlant& plant, const Eigen::Ref>& q_vars, solvers::MathematicalProgram* prog) { DRAKE_DEMAND(q_vars.rows() == plant.num_positions()); + std::vector> bindings; // Loop through each body for (BodyIndex body_index{0}; body_index < plant.num_bodies(); ++body_index) { const auto& body = plant.get_body(body_index); if (body.has_quaternion_dofs()) { Vector4 quat_vars = q_vars.segment<4>(body.floating_positions_start()); - prog->AddConstraint(solvers::Binding( - std::make_shared(), quat_vars)); + bindings.emplace_back( + prog->AddConstraint(solvers::Binding( + std::make_shared(), quat_vars))); if (prog->GetInitialGuess(quat_vars).array().isNaN().all()) { prog->SetInitialGuess(quat_vars, Eigen::Vector4d(1, 0, 0, 0)); } } } + return bindings; } DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(( diff --git a/multibody/inverse_kinematics/unit_quaternion_constraint.h b/multibody/inverse_kinematics/unit_quaternion_constraint.h index 36f4f627f22b..a4b8f196e4fa 100644 --- a/multibody/inverse_kinematics/unit_quaternion_constraint.h +++ b/multibody/inverse_kinematics/unit_quaternion_constraint.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include "drake/multibody/plant/multibody_plant.h" #include "drake/solvers/constraint.h" @@ -63,7 +64,8 @@ class UnitQuaternionConstraint : public solvers::Constraint { @tparam_default_scalar */ template -void AddUnitQuaternionConstraintOnPlant( +std::vector> +AddUnitQuaternionConstraintOnPlant( const MultibodyPlant& plant, const Eigen::Ref>& q_vars, solvers::MathematicalProgram* prog); diff --git a/multibody/plant/constraint_specs.h b/multibody/plant/constraint_specs.h index cecef1ada8b0..9fe7e390620f 100644 --- a/multibody/plant/constraint_specs.h +++ b/multibody/plant/constraint_specs.h @@ -37,8 +37,8 @@ struct CouplerConstraintSpec { // Struct to store the specification for a distance constraint. A distance // constraint is modeled as a holonomic constraint. Distance constraints can -// be "soft" which imposes the the condition: -// (d(q)-d₀) + c/k⋅ḋ(q) + 1/k⋅f = 0 +// be "soft", and are implemented as a spring force, f: +// f = -k⋅(d(q) - d₀) - c⋅ḋ(q), // where d₀ is a fixed length, k a stiffness parameter in N/m and c a damping // parameter in N⋅s/m. We use d(q) to denote the Euclidean distance between two // points P and Q, rigidly affixed to bodies A and B respectively, as a function diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc index d613ef67689c..f2d9a267867b 100644 --- a/multibody/plant/test/multibody_plant_test.cc +++ b/multibody/plant/test/multibody_plant_test.cc @@ -4862,7 +4862,7 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) { } GTEST_TEST(MultibodyPlantTests, GetConstraintIds) { - // Set up a plant with 3 constraints with arbitrary parameters. + // Set up a plant with each constraint type with arbitrary parameters. MultibodyPlant plant(0.01); EXPECT_EQ(plant.GetConstraintIds().size(), 0);