forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 8
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Implements AddMultibodyPlantConstraints()
This method creates a mapping from the constraints used in MultibodyPlant/SAP to the constraints used in MathematicalProgram. Towards RobotLocomotion#18917.
- Loading branch information
1 parent
7d74241
commit 2006b55
Showing
11 changed files
with
303 additions
and
8 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
87 changes: 87 additions & 0 deletions
87
multibody/inverse_kinematics/add_multibody_plant_constraints.cc
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,87 @@ | ||
| #include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h" | ||
|
|
||
| #include <memory> | ||
|
|
||
| #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<Binding<Constraint>> AddMultibodyPlantConstraints( | ||
| const MultibodyPlant<double>& plant, | ||
| const solvers::VectorXDecisionVariable& q, | ||
| solvers::MathematicalProgram* prog, | ||
| systems::Context<double>* plant_context) { | ||
| DRAKE_THROW_UNLESS(prog != nullptr); | ||
| std::vector<Binding<Constraint>> 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<PointToPointDistanceConstraint>( | ||
| &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<PositionConstraint>( | ||
| &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<PositionConstraint>( | ||
| &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<OrientationConstraint>( | ||
| &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 |
36 changes: 36 additions & 0 deletions
36
multibody/inverse_kinematics/add_multibody_plant_constraints.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,36 @@ | ||
| #pragma once | ||
|
|
||
| #include <vector> | ||
|
|
||
| #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<solvers::Binding<solvers::Constraint>> AddMultibodyPlantConstraints( | ||
| const MultibodyPlant<double>& plant, | ||
| const solvers::VectorXDecisionVariable& q, | ||
| solvers::MathematicalProgram* prog, | ||
| systems::Context<double>* plant_context = nullptr); | ||
|
|
||
| } // namespace multibody | ||
| } // namespace drake |
129 changes: 129 additions & 0 deletions
129
multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,129 @@ | ||
| #include "drake/multibody/inverse_kinematics/add_multibody_plant_constraints.h" | ||
|
|
||
| #include <gtest/gtest.h> | ||
|
|
||
| #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<double>::SolidBoxWithMass(1, 1, 1, 1)); | ||
| body_B_ = &plant_.AddRigidBody( | ||
| "body_B", SpatialInertia<double>::SolidBoxWithMass(1, 1, 1, 1)); | ||
| world_A_ = &plant_.AddJoint<RevoluteJoint>( | ||
| "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<RevoluteJoint>( | ||
| "temp", *body_A_, RigidTransformd(Vector3d(1, 0, 0)), *body_B_, | ||
| RigidTransformd(), Vector3d::UnitZ())); | ||
| A_B_ = &plant_.AddJoint<RevoluteJoint>( | ||
| "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<double> plant_{0.1}; | ||
| std::unique_ptr<systems::Context<double>> plant_context_{}; | ||
| const RigidBody<double>* body_A_{nullptr}; | ||
| const RigidBody<double>* body_B_{nullptr}; | ||
| const RevoluteJoint<double>* world_A_{nullptr}; | ||
| const RevoluteJoint<double>* 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<double>& body_C = plant_.AddRigidBody( | ||
| "body_C", SpatialInertia<double>::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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.