From e2bd4f8ff13495b33d38f83e8fab4503dff68103 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Thu, 19 Dec 2024 07:55:26 -0500 Subject: [PATCH] Use AddMultibodyPlantConstraints in InverseKinematics Also moves the joint limits / joint locking logic from the IK constructor to AddMultibodyPlantConstraints. Towards #18917. --- .../multibody/test/inverse_kinematics_test.py | 2 +- multibody/inverse_kinematics/BUILD.bazel | 2 + .../add_multibody_plant_constraints.cc | 145 ++++++++++++++---- .../add_multibody_plant_constraints.h | 10 +- .../inverse_kinematics/inverse_kinematics.cc | 70 ++------- .../inverse_kinematics/inverse_kinematics.h | 13 +- .../add_multibody_plant_constraints_test.cc | 111 +++++++++++++- .../test/inverse_kinematics_test.cc | 60 ++++---- 8 files changed, 287 insertions(+), 126 deletions(-) diff --git a/bindings/pydrake/multibody/test/inverse_kinematics_test.py b/bindings/pydrake/multibody/test/inverse_kinematics_test.py index d57ba22ea1b5..cfc03a65aa1a 100644 --- a/bindings/pydrake/multibody/test/inverse_kinematics_test.py +++ b/bindings/pydrake/multibody/test/inverse_kinematics_test.py @@ -87,7 +87,7 @@ def test_AddMultibodyPlantConstraints(self): q=self.q, prog=self.prog, plant_context=self.ik_two_bodies.context()) - self.assertEqual(len(bindings), 4) + self.assertEqual(len(bindings), 3) def test_AddPositionConstraint1(self): p_BQ = np.array([0.2, 0.3, 0.5]) diff --git a/multibody/inverse_kinematics/BUILD.bazel b/multibody/inverse_kinematics/BUILD.bazel index 7b62464355e4..88a6ff60a62d 100644 --- a/multibody/inverse_kinematics/BUILD.bazel +++ b/multibody/inverse_kinematics/BUILD.bazel @@ -135,6 +135,7 @@ drake_cc_library( ], visibility = ["//visibility:private"], deps = [ + ":add_multibody_plant_constraints", ":kinematic_evaluators", "//multibody/plant", "//solvers:mathematical_program", @@ -167,6 +168,7 @@ drake_cc_googletest( srcs = ["test/add_multibody_plant_constraints_test.cc"], deps = [ ":add_multibody_plant_constraints", + "//common/test_utilities:eigen_matrix_compare", "//solvers:snopt_solver", "//solvers:solve", ], diff --git a/multibody/inverse_kinematics/add_multibody_plant_constraints.cc b/multibody/inverse_kinematics/add_multibody_plant_constraints.cc index ce00281859d8..e9baec395975 100644 --- a/multibody/inverse_kinematics/add_multibody_plant_constraints.cc +++ b/multibody/inverse_kinematics/add_multibody_plant_constraints.cc @@ -19,6 +19,9 @@ std::vector> AddMultibodyPlantConstraints( solvers::MathematicalProgram* prog, systems::Context* plant_context) { DRAKE_THROW_UNLESS(prog != nullptr); + if (plant_context) { + plant.ValidateContext(*plant_context); + } // AddMultibodyPlantConstraints only supports coupler, ball, distance, and // weld. int num_supported_constraints = @@ -36,49 +39,133 @@ std::vector> AddMultibodyPlantConstraints( "not supported here yet (but likely should be)", plant.num_constraints(), num_supported_constraints)); } - std::vector> bindings = - AddUnitQuaternionConstraintOnPlant(plant, q, prog); + std::vector> bindings; + + // Joint limits. + const int nq = plant.num_positions(); + Eigen::VectorXd lb = plant.GetPositionLowerLimits(); + Eigen::VectorXd ub = plant.GetPositionUpperLimits(); + + // Joint locking. + VectorX is_locked = VectorX::Constant(nq, false); + Eigen::VectorXd current_positions(nq); + if (plant_context) { + current_positions = plant.GetPositions(*plant_context); + for (JointIndex i : plant.GetJointIndices()) { + const Joint& joint = plant.get_joint(i); + if (joint.is_locked(*plant_context)) { + const int start = joint.position_start(); + const int size = joint.num_positions(); + lb.segment(start, size) = current_positions.segment(start, size); + ub.segment(start, size) = current_positions.segment(start, size); + is_locked.segment(start, size).array() = true; + } + } + } + + // Add the unit quaternion constraints. + for (BodyIndex i{0}; i < plant.num_bodies(); ++i) { + const RigidBody& body = plant.get_body(i); + if (body.has_quaternion_dofs()) { + const int start = body.floating_positions_start(); + constexpr int kSize = 4; + if (plant_context && is_locked.segment(start).any()) { + // Sanity check the MultibodyTree invariant. + DRAKE_DEMAND(is_locked.segment(start).all()); + // Lock to the normalized value, in lieu of a unit norm constraint. + const Eigen::Vector4d quat = + current_positions.segment(start).normalized(); + lb.segment(start) = quat; + ub.segment(start) = quat; + prog->SetInitialGuess(q.segment(start), quat); + } else { + // TODO(russt): Probably the joint limits should be [-1, 1] coming + // right out of the MultibodyPlant. + lb.segment(start) = -Eigen::Vector4d::Ones(); + ub.segment(start) = Eigen::Vector4d::Ones(); + bindings + .emplace_back( + prog->AddConstraint(solvers::Binding( + std::make_shared(), + q.segment(start)))) + .evaluator() + ->set_description(fmt::format( + "Unit quaternion constraint for body {}", body.name())); + prog->SetInitialGuess(q.segment(start), + Eigen::Vector4d{1, 0, 0, 0}); + } + } + } + bindings.emplace_back(prog->AddBoundingBoxConstraint(lb, ub, q)) + .evaluator() + ->set_description("Joint limits"); + 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)); + bindings + .emplace_back(prog->AddLinearEqualityConstraint( + q[q0_index] == spec.gear_ratio * q[q1_index] + spec.offset)) + .evaluator() + ->set_description( + fmt::format("Coupler constraint for joint {} and joint {}", + spec.joint0_index, spec.joint1_index)); } 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)); + 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)) + .evaluator() + ->set_description( + fmt::format("Distance constraint between body {} and body {}", + spec.body_A, spec.body_B)); } 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)); + 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)) + .evaluator() + ->set_description( + fmt::format("Ball constraint between body {} and body {}", + spec.body_A, spec.body_B)); } 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)); + 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)) + .evaluator() + ->set_description( + fmt::format("Weld position constraint between body {} and body {}", + spec.body_A, spec.body_B)); + 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)) + .evaluator() + ->set_description(fmt::format( + "Weld orientation constraint between body {} and body {}", + spec.body_A, spec.body_B)); } return bindings; } diff --git a/multibody/inverse_kinematics/add_multibody_plant_constraints.h b/multibody/inverse_kinematics/add_multibody_plant_constraints.h index 63d316753d9f..d27e302fd7a7 100644 --- a/multibody/inverse_kinematics/add_multibody_plant_constraints.h +++ b/multibody/inverse_kinematics/add_multibody_plant_constraints.h @@ -8,9 +8,13 @@ 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. +/** For all kinematic constraints associated with `plant` adds a corresponding +solver::Constraint to `prog`, using decision variables `q` to represent the +generalized positions of the plant. + +Adds joint limits constraints, unit quaternion constraints, and constraints for +any locked joints (via Joint::Lock()). Note that you must pass a valid +`plant_context` to use joint locking. Adds constraints for coupler, distance, ball, and weld constraints. The distance constraint is implemented here as a hard kinematic constraint (i.e., diff --git a/multibody/inverse_kinematics/inverse_kinematics.cc b/multibody/inverse_kinematics/inverse_kinematics.cc index b81ecdf4833b..eed9af915ac2 100644 --- a/multibody/inverse_kinematics/inverse_kinematics.cc +++ b/multibody/inverse_kinematics/inverse_kinematics.cc @@ -3,6 +3,7 @@ #include #include +#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/distance_constraint.h" @@ -21,8 +22,6 @@ namespace drake { namespace multibody { -constexpr double kInf = std::numeric_limits::infinity(); - InverseKinematics::InverseKinematics(const MultibodyPlant& plant, bool with_joint_limits) : InverseKinematics(plant, plant.CreateDefaultContext(), nullptr, @@ -51,64 +50,21 @@ InverseKinematics::InverseKinematics( } DRAKE_DEMAND(context_ != nullptr); // Sanity check. - // We're about to add constraints for position limits (if requested), locked - // joints, and quaternions (unit norm). When a quaternion is locked, we'll use - // the lock constraint instead of the unit norm constraint. - - // Obey the joint limits (if requested). - const int nq = plant.num_positions(); - Eigen::VectorXd lb; - Eigen::VectorXd ub; - if (with_joint_limits) { - lb = plant.GetPositionLowerLimits(); - ub = plant.GetPositionUpperLimits(); - } else { - lb = Eigen::VectorXd::Constant(nq, -kInf); - ub = Eigen::VectorXd::Constant(nq, +kInf); - } - - // Obey joint locking. Joint locking trumps joint limits. - const Eigen::VectorXd current_positions = plant.GetPositions(context()); - VectorX is_locked = VectorX::Constant(nq, false); - for (JointIndex i : plant.GetJointIndices()) { - const Joint& joint = plant.get_joint(i); - if (joint.is_locked(context())) { - const int start = joint.position_start(); - const int size = joint.num_positions(); - lb.segment(start, size) = current_positions.segment(start, size); - ub.segment(start, size) = current_positions.segment(start, size); - is_locked.segment(start, size).array() = true; - } - } - - // Add the unit quaternion constraints. - for (BodyIndex i{0}; i < plant.num_bodies(); ++i) { - const RigidBody& body = plant.get_body(i); - if (body.has_quaternion_dofs()) { - const int start = body.floating_positions_start(); - constexpr int size = 4; - if (is_locked.segment(start).any()) { - // Sanity check the MultibodyTree invariant. - DRAKE_DEMAND(is_locked.segment(start).all()); - // Lock to the normalized value, in lieu of a unit norm constraint. - const Eigen::Vector4d quat = - current_positions.segment(start).normalized(); - lb.segment(start) = quat; - ub.segment(start) = quat; - prog_->SetInitialGuess(q_.segment(start), quat); - } else { - prog_->AddConstraint(solvers::Binding( - std::make_shared(), - q_.segment(start))); - // Set a non-zero initial guess to help avoid singularities. - prog_->SetInitialGuess(q_.segment(start), - Eigen::Vector4d{1, 0, 0, 0}); + AddMultibodyPlantConstraints(plant, q_, prog_.get(), context_); + + if (!with_joint_limits) { + // Remove only the joint limit constraint. + const auto& bbox_bindings = prog_->bounding_box_constraints(); + bool removed_joint_limits = false; + for (const auto& binding : bbox_bindings) { + if (binding.evaluator()->get_description() == "Joint limits") { + prog_->RemoveConstraint(binding); + removed_joint_limits = true; + break; } } + DRAKE_DEMAND(removed_joint_limits); } - - // Now we can finally add the bbox constraint for joint limits and locking. - prog_->AddBoundingBoxConstraint(lb, ub, q_); } solvers::Binding InverseKinematics::AddPositionConstraint( diff --git a/multibody/inverse_kinematics/inverse_kinematics.h b/multibody/inverse_kinematics/inverse_kinematics.h index 0d2705170c58..d556f4fe6a2f 100644 --- a/multibody/inverse_kinematics/inverse_kinematics.h +++ b/multibody/inverse_kinematics/inverse_kinematics.h @@ -33,8 +33,8 @@ class InverseKinematics { * @param plant The robot on which the inverse kinematics problem will be * solved. * @param with_joint_limits If set to true, then the constructor - * imposes the joint limit (obtained from plant.GetPositionLowerLimits() - * and plant.GetPositionUpperLimits(). If set to false, then the constructor + * imposes the joint limits (obtained from plant.GetPositionLowerLimits() + * and plant.GetPositionUpperLimits()). If set to false, then the constructor * does not impose the joint limit constraints in the constructor. * @note The inverse kinematics problem constructed in this way doesn't permit * collision related constraint (such as calling @@ -77,10 +77,11 @@ class InverseKinematics { * result.GetSolution(ik.q()). The user could then use this context to perform * kinematic computation (like computing the position of the end-effector * etc.). - * @param with_joint_limits If set to true, then the constructor - * imposes the joint limit (obtained from plant.GetPositionLowerLimits() - * and plant.GetPositionUpperLimits(). If set to false, then the constructor - * does not impose the joint limit constraints in the constructor. */ + * @param with_joint_limits If set to true, then the constructor imposes the + * joint limits (obtained from plant.GetPositionLowerLimits() and + * plant.GetPositionUpperLimits(), and from any body/joint locks set in + * `plant_context`). If set to false, then the constructor does not impose + * the joint limit constraints in the constructor. */ InverseKinematics(const MultibodyPlant& plant, systems::Context* plant_context, bool with_joint_limits = true); diff --git a/multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc b/multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc index b4116d88f1c6..a86f24049c2a 100644 --- a/multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc +++ b/multibody/inverse_kinematics/test/add_multibody_plant_constraints_test.cc @@ -2,6 +2,8 @@ #include +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/multibody/tree/quaternion_floating_joint.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/solvers/snopt_solver.h" #include "drake/solvers/solve.h" @@ -44,7 +46,7 @@ class AddMultibodyPlantConstraintsTest : public ::testing::Test { RigidTransformd(), Vector3d::UnitZ()); } - void CheckConstraints(int expected_num_constraints = 1, double tol = 1e-6) { + void CheckConstraints(int expected_num_constraints = 2, double tol = 1e-6) { if (!plant_.is_finalized()) { plant_.Finalize(); } @@ -120,10 +122,115 @@ TEST_F(AddMultibodyPlantConstraintsTest, WeldConstraint) { // IPOPT is flakey on this test. return; } - const int expected_num_constraints = 4; // 2 for quaternion, 2 for weld. + const int expected_num_constraints = + 4; // 1 joint limits, 1 unit quaternion, 2 for weld. CheckConstraints(expected_num_constraints, /* tol = */ 1e-2); } +GTEST_TEST(AdditionalTests, QuaternionsAndJointLimitsAndLocks) { + MultibodyPlant plant(0); + + // Create a plant with four bodies. + const auto& world = plant.world_body(); + const auto M = SpatialInertia::SolidCubeWithMass(1.0, 0.1); + const auto& body1 = plant.AddRigidBody("body1", M); + const auto& body2 = plant.AddRigidBody("body2", M); + const auto& body3 = plant.AddRigidBody("body3", M); + const auto& body4 = plant.AddRigidBody("body4", M); + + // Attach a specific joint to each body: + // (1) A quaternion floating joint that will not be locked. + // (2) A quaternion floating joint that we'll lock to its initial position. + // (3) A revolute joint that will not be locked. + // (4) A revolute joint that we'll lock to its initial position. + math::RigidTransform I; + Eigen::Vector3d X = Eigen::Vector3d::UnitX(); + const auto& joint1 = + plant.AddJoint("joint1", world, I, body1, I); + const auto& joint2 = + plant.AddJoint("joint2", world, I, body2, I); + const auto& joint3 = + plant.AddJoint("joint3", world, I, body3, I, X); + const auto& joint4 = + plant.AddJoint("joint4", world, I, body4, I, X); + plant.Finalize(); + auto context = plant.CreateDefaultContext(); + + // Leave joint1 unlocked. + + // Lock body2's floating joint to an un-normalized initial value. + joint2.SetQuaternion(&*context, Eigen::Quaternion(0, 3.0, 0, 0)); + joint2.Lock(&*context); + + // Set limits on joint3, but do not lock it. + dynamic_cast&>(plant.get_mutable_joint(joint3.index())) + .set_position_limits(Vector1d{-0.5}, Vector1d{0.5}); + + // Lock body4's revolute joint beyond its limit. + dynamic_cast&>(plant.get_mutable_joint(joint4.index())) + .set_position_limits(Vector1d{-1}, Vector1d{1}); + joint4.set_angle(&*context, 1.1); + joint4.Lock(&*context); + + solvers::MathematicalProgram prog; + auto q = prog.NewContinuousVariables(plant.num_positions()); + AddMultibodyPlantConstraints(plant, q, &prog, context.get()); + + // The initial guess is set for the two quaternion floating joints. + EXPECT_TRUE(CompareMatrices( + prog.GetInitialGuess(q.segment(joint1.position_start(), 4)), + Eigen::Vector4d(1, 0, 0, 0))); + const Eigen::Vector4d joint2_position(0, 1, 0, 0); + EXPECT_TRUE(CompareMatrices( + prog.GetInitialGuess(q.segment(joint2.position_start(), 4)), + joint2_position)); + + // We only expect one bounding box constraint, which is the joint limits. + ASSERT_EQ(prog.bounding_box_constraints().size(), 1); + const solvers::Binding& limits = + prog.bounding_box_constraints().front(); + + // joint 1 is unlocked, we expect a unit quaternion constraint and limits of + // [-1, 1]. + ASSERT_EQ(prog.generic_constraints().size(), 1); + const solvers::Binding& unit_quat = + prog.generic_constraints().front(); + ASSERT_EQ(unit_quat.variables().size(), 4); + const int j1_start = joint1.position_start(); + EXPECT_EQ(symbolic::Variables(unit_quat.variables()), + symbolic::Variables(q.segment(j1_start, 4))); + EXPECT_TRUE( + CompareMatrices(limits.evaluator()->lower_bound().segment<4>(j1_start), + Eigen::Vector4d(-1, -1, -1, -1))); + EXPECT_TRUE( + CompareMatrices(limits.evaluator()->upper_bound().segment<4>(j1_start), + Eigen::Vector4d(1, 1, 1, 1))); + + // joint2 is locked, so we expect a limits == joint2_position. + const int j2_start = joint2.position_start(); + EXPECT_TRUE(CompareMatrices( + limits.evaluator()->lower_bound().segment<4>(j2_start), joint2_position)); + EXPECT_TRUE(CompareMatrices( + limits.evaluator()->upper_bound().segment<4>(j2_start), joint2_position)); + + // joint3 is unlocked, so we expect the joint limits to be enforced. + const int j3_start = joint3.position_start(); + EXPECT_EQ(limits.evaluator()->lower_bound()[j3_start], -0.5); + EXPECT_EQ(limits.evaluator()->upper_bound()[j3_start], +0.5); + + // joint4 is locked. Locked revolute joints obey their initial position, + // ignoring limits. + const int j4_start = joint4.position_start(); + EXPECT_EQ(limits.evaluator()->lower_bound()[j4_start], 1.1); + EXPECT_EQ(limits.evaluator()->upper_bound()[j4_start], 1.1); + + // If we don't pass in the context, then we will not get the locked joints. + solvers::MathematicalProgram prog2; + auto q2 = prog2.NewContinuousVariables(plant.num_positions()); + AddMultibodyPlantConstraints(plant, q2, &prog2); + EXPECT_EQ(prog2.linear_equality_constraints().size(), 0); +} + } // namespace } // namespace multibody } // namespace drake diff --git a/multibody/inverse_kinematics/test/inverse_kinematics_test.cc b/multibody/inverse_kinematics/test/inverse_kinematics_test.cc index e8e58f4e9fd2..0aeddf32dae5 100644 --- a/multibody/inverse_kinematics/test/inverse_kinematics_test.cc +++ b/multibody/inverse_kinematics/test/inverse_kinematics_test.cc @@ -19,10 +19,6 @@ namespace drake { namespace multibody { -namespace { -constexpr double kInf = std::numeric_limits::infinity(); -} // namespace - Eigen::Quaterniond Vector4ToQuaternion( const Eigen::Ref& q) { return Eigen::Quaterniond(q(0), q(1), q(2), q(3)); @@ -198,47 +194,55 @@ GTEST_TEST(InverseKinematicsTest, ConstructorLockedJoints) { // Initialize IK. const InverseKinematics ik(plant, &*context); - const int nq = ik.q().size(); const solvers::MathematicalProgram& prog = ik.prog(); // The initial guess is set for the two quaternion floating joints. EXPECT_TRUE(CompareMatrices( ik.prog().GetInitialGuess(ik.q().segment(joint1.position_start(), 4)), Eigen::Vector4d(1, 0, 0, 0))); + const Eigen::Vector4d joint2_position(0, 1, 0, 0); EXPECT_TRUE(CompareMatrices( - ik.prog().GetInitialGuess(ik.q().segment(joint2.position_start(), 4)), - Eigen::Vector4d(0, 1, 0, 0))); + prog.GetInitialGuess(ik.q().segment(joint2.position_start(), 4)), + joint2_position)); - // The unit quaternion constraint is only added to joint1. + // We only expect one bounding box constraint, which is the joint limits. + ASSERT_EQ(prog.bounding_box_constraints().size(), 1); + const solvers::Binding& limits = + prog.bounding_box_constraints().front(); + + // joint 1 is unlocked, we expect a unit quaternion constraint and limits of + // [-1, 1]. ASSERT_EQ(prog.generic_constraints().size(), 1); const solvers::Binding& unit_quat = prog.generic_constraints().front(); ASSERT_EQ(unit_quat.variables().size(), 4); + const int j1_start = joint1.position_start(); EXPECT_EQ(symbolic::Variables(unit_quat.variables()), - symbolic::Variables(ik.q().segment(joint1.position_start(), 4))); + symbolic::Variables(ik.q().segment(j1_start, 4))); + EXPECT_TRUE( + CompareMatrices(limits.evaluator()->lower_bound().segment<4>(j1_start), + Eigen::Vector4d(-1, -1, -1, -1))); + EXPECT_TRUE( + CompareMatrices(limits.evaluator()->upper_bound().segment<4>(j1_start), + Eigen::Vector4d(1, 1, 1, 1))); - // Check the default bbox constraint. - // Prepare our expected values: - Eigen::VectorXd lower = Eigen::VectorXd::Constant(nq, -kInf); - Eigen::VectorXd upper = Eigen::VectorXd::Constant(nq, +kInf); - // - Locked quaternion floating joints obey a single, normalized position. + // joint2 is locked, so we expect a limits == joint2_position. const int j2_start = joint2.position_start(); - lower.segment(j2_start, 7) = upper.segment(j2_start, 7) = - (Vector() << 0, 1, 0, 0, 0, 0, 0).finished(); - // - Unlocked revolute joints still obey their position limits. + EXPECT_TRUE(CompareMatrices( + limits.evaluator()->lower_bound().segment<4>(j2_start), joint2_position)); + EXPECT_TRUE(CompareMatrices( + limits.evaluator()->upper_bound().segment<4>(j2_start), joint2_position)); + + // joint3 is unlocked, so we expect the joint limits to be enforced. const int j3_start = joint3.position_start(); - lower[j3_start] = -0.5; - upper[j3_start] = +0.5; - // - Locked revolute joints obey their initial position, ignoring limits. + EXPECT_EQ(limits.evaluator()->lower_bound()[j3_start], -0.5); + EXPECT_EQ(limits.evaluator()->upper_bound()[j3_start], +0.5); + + // joint4 is locked. Locked revolute joints obey their initial position, + // ignoring limits. const int j4_start = joint4.position_start(); - lower[j4_start] = upper[j4_start] = 1.1; - // Now check the expected value against `prog`. - ASSERT_EQ(prog.bounding_box_constraints().size(), 1); - const solvers::Binding& limits = - prog.bounding_box_constraints().front(); - ASSERT_EQ(limits.variables().size(), nq); - EXPECT_TRUE(CompareMatrices(limits.evaluator()->lower_bound(), lower)); - EXPECT_TRUE(CompareMatrices(limits.evaluator()->upper_bound(), upper)); + EXPECT_EQ(limits.evaluator()->lower_bound()[j4_start], 1.1); + EXPECT_EQ(limits.evaluator()->upper_bound()[j4_start], 1.1); } TEST_F(TwoFreeBodiesTest, PositionConstraint) {