diff --git a/multibody/inverse_kinematics/inverse_kinematics.cc b/multibody/inverse_kinematics/inverse_kinematics.cc index c62610410030..34e41f15b26b 100644 --- a/multibody/inverse_kinematics/inverse_kinematics.cc +++ b/multibody/inverse_kinematics/inverse_kinematics.cc @@ -1,6 +1,7 @@ #include "drake/multibody/inverse_kinematics/inverse_kinematics.h" #include +#include #include "drake/multibody/inverse_kinematics/angle_between_vectors_constraint.h" #include "drake/multibody/inverse_kinematics/angle_between_vectors_cost.h" @@ -18,34 +19,91 @@ namespace drake { namespace multibody { + +constexpr double kInf = std::numeric_limits::infinity(); + InverseKinematics::InverseKinematics(const MultibodyPlant& plant, bool with_joint_limits) - : prog_{new solvers::MathematicalProgram()}, - plant_(plant), - owned_context_(plant_.CreateDefaultContext()), - context_(owned_context_.get()), - q_(prog_->NewContinuousVariables(plant_.num_positions(), "q")) { - if (with_joint_limits) { - prog_->AddBoundingBoxConstraint(plant.GetPositionLowerLimits(), - plant.GetPositionUpperLimits(), q_); - } - AddUnitQuaternionConstraintOnPlant(plant, q_, prog_.get()); -} + : InverseKinematics(plant, plant.CreateDefaultContext(), nullptr, + with_joint_limits) {} InverseKinematics::InverseKinematics(const MultibodyPlant& plant, systems::Context* plant_context, bool with_joint_limits) - : prog_{new solvers::MathematicalProgram()}, + : InverseKinematics(plant, nullptr, plant_context, with_joint_limits) {} + +InverseKinematics::InverseKinematics( + const MultibodyPlant& plant, + std::unique_ptr> owned_context, + systems::Context* plant_context, bool with_joint_limits) + : prog_(std::make_unique()), plant_(plant), - owned_context_(nullptr), - context_(plant_context), + owned_context_(std::move(owned_context)), + context_(owned_context_ ? owned_context_.get() : plant_context), q_(prog_->NewContinuousVariables(plant.num_positions(), "q")) { - DRAKE_DEMAND(plant_context != nullptr); + if (owned_context_ != nullptr) { + // The public constructor must not pass both pointers when calling us. + DRAKE_DEMAND(plant_context == nullptr); + } else { + // The user calling the `plant_context` overload should not pass nullptr. + DRAKE_THROW_UNLESS(plant_context != nullptr); + } + 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) { - prog_->AddBoundingBoxConstraint(plant.GetPositionLowerLimits(), - plant.GetPositionUpperLimits(), q_); + 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{0}; i < plant.num_joints(); ++i) { + 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; + } } - AddUnitQuaternionConstraintOnPlant(plant, q_, prog_.get()); + + // Add the unit quaternion constraints. + for (BodyIndex i{0}; i < plant.num_bodies(); ++i) { + const Body& 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; + } else { + prog_->AddConstraint(solvers::Binding( + std::make_shared(), + q_.segment(start))); + } + } + } + + // 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 93790d5b065f..84fa4dab60b9 100644 --- a/multibody/inverse_kinematics/inverse_kinematics.h +++ b/multibody/inverse_kinematics/inverse_kinematics.h @@ -15,6 +15,10 @@ namespace multibody { * postures of the robot satisfying certain constraints. * The decision variables include the generalized position of the robot. * + * To perform IK on a subset of the plant, use the constructor overload that + * takes a `plant_context` and use `Joint::Lock` on the joints in that Context + * that should be fixed during IK. + * * @ingroup planning_kinematics */ class InverseKinematics { @@ -48,10 +52,12 @@ class InverseKinematics { * @param plant The robot on which the inverse kinematics problem will be * solved. This plant should have been connected to a SceneGraph within a * Diagram - * @param context The context for the plant. This context should be a part of - * the Diagram context. - * To construct a plant connected to a SceneGraph, with the corresponding - * plant_context, the steps are + * @param plant_context The context for the plant. This context should be a + * part of the Diagram context. Any locked joints in the `plant_context` will + * remain fixed at their locked value. (This provides a convenient way to + * perform IK on a subset of the plant.) To construct a plant connected to a + * SceneGraph, with the corresponding plant_context, the steps are: + * ``` * // 1. Add a diagram containing the MultibodyPlant and SceneGraph * systems::DiagramBuilder builder; * auto items = AddMultibodyPlantSceneGraph(&builder, 0.0); @@ -64,6 +70,7 @@ class InverseKinematics { * // 5. Get the context for the plant. * auto plant_context = &(diagram->GetMutableSubsystemContext(items.plant, * diagram_context.get())); + * ``` * This context will be modified during calling ik.prog.Solve(...). When * Solve() returns `result`, context will store the optimized posture, namely * plant.GetPositions(*context) will be the same as in @@ -390,6 +397,13 @@ class InverseKinematics { systems::Context* get_mutable_context() { return context_; } private: + /* Both public constructors delegate to here. Exactly one of owned_context or + plant_context must be non-null. */ + InverseKinematics(const MultibodyPlant& plant, + std::unique_ptr> owned_context, + systems::Context* plant_context, + bool with_joint_limits); + std::unique_ptr prog_; const MultibodyPlant& plant_; std::unique_ptr> const owned_context_; diff --git a/multibody/inverse_kinematics/test/inverse_kinematics_test.cc b/multibody/inverse_kinematics/test/inverse_kinematics_test.cc index ff66c4683dac..21be25cc4dde 100644 --- a/multibody/inverse_kinematics/test/inverse_kinematics_test.cc +++ b/multibody/inverse_kinematics/test/inverse_kinematics_test.cc @@ -1,5 +1,6 @@ #include "drake/multibody/inverse_kinematics/inverse_kinematics.h" +#include #include #include "drake/common/find_resource.h" @@ -7,6 +8,8 @@ #include "drake/math/rotation_matrix.h" #include "drake/math/wrap_to.h" #include "drake/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.h" +#include "drake/multibody/tree/quaternion_floating_joint.h" +#include "drake/multibody/tree/revolute_joint.h" #include "drake/solvers/create_constraint.h" #include "drake/solvers/solve.h" @@ -15,6 +18,11 @@ 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)); @@ -140,6 +148,88 @@ TEST_F(TwoFreeBodiesTest, ConstructorAddsUnitQuaterionConstraints) { EXPECT_NEAR(q.segment<4>(7).squaredNorm(), 1.0, 1e-6); } +GTEST_TEST(InverseKinematicsTest, ConstructorLockedJoints) { + 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.set_quaternion(&*context, Eigen::Quaternion(3.0, 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); + + // Initialize IK. + const InverseKinematics ik(plant, &*context); + const int nq = ik.q().size(); + const solvers::MathematicalProgram& prog = ik.prog(); + + // The unit quaternion constraint is only added to joint1. + ASSERT_EQ(prog.generic_constraints().size(), 1); + const solvers::Binding& unit_quat = + prog.generic_constraints().front(); + ASSERT_EQ(unit_quat.variables().size(), 4); + EXPECT_EQ(symbolic::Variables(unit_quat.variables()), + symbolic::Variables(ik.q().segment(joint1.position_start(), 4))); + + // 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. + const int j2_start = joint2.position_start(); + lower.segment(j2_start, 7) = upper.segment(j2_start, 7) = + (Vector() << 1, 0, 0, 0, 0, 0, 0).finished(); + // - Unlocked revolute joints still obey their position limits. + 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. + 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)); +} + TEST_F(TwoFreeBodiesTest, PositionConstraint) { const Eigen::Vector3d p_BQ(0.2, 0.3, 0.5); const Eigen::Vector3d p_AQ_lower(-0.1, -0.2, -0.3);