Skip to content

Commit

Permalink
Use AddMultibodyPlantConstraints in InverseKinematics
Browse files Browse the repository at this point in the history 
Also moves the joint limits / joint locking logic from the IK
constructor to AddMultibodyPlantConstraints.

Towards RobotLocomotion#18917.
  • Loading branch information
@RussTedrake
RussTedrake committed Dec 31, 2024
1 parent 83d5f33 commit 7562f89
Show file tree
Hide file tree
Showing 7 changed files with 340 additions and 112 deletions.
2 changes: 1 addition & 1 deletion bindings/pydrake/multibody/test/inverse_kinematics_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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), 2)
self.assertEqual(len(bindings), 3)

def test_AddPositionConstraint1(self):
p_BQ = np.array([0.2, 0.3, 0.5])
Expand Down
2 changes: 2 additions & 0 deletions multibody/inverse_kinematics/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,7 @@ drake_cc_library(
],
visibility = ["//visibility:private"],
deps = [
":add_multibody_plant_constraints",
":kinematic_evaluators",
"//multibody/plant",
"//solvers:mathematical_program",
Expand Down Expand Up @@ -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",
],
Expand Down
156 changes: 127 additions & 29 deletions multibody/inverse_kinematics/add_multibody_plant_constraints.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,9 @@ std::vector<Binding<Constraint>> AddMultibodyPlantConstraints(
solvers::MathematicalProgram* prog,
systems::Context<double>* 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 =
Expand All @@ -36,49 +39,144 @@ std::vector<Binding<Constraint>> AddMultibodyPlantConstraints(
"not supported here yet (but likely should be)",
plant.num_constraints(), num_supported_constraints));
}
std::vector<Binding<Constraint>> bindings =
AddUnitQuaternionConstraintOnPlant(plant, q, prog);
std::vector<Binding<Constraint>> bindings;

// Joint limits.
const int nq = plant.num_positions();
constexpr double kInf = std::numeric_limits<double>::infinity();
Eigen::VectorXd lb = plant.GetPositionLowerLimits();
Eigen::VectorXd ub = plant.GetPositionUpperLimits();

// Joint locking.
VectorX<bool> is_locked = VectorX<bool>::Constant(nq, false);
Eigen::VectorXd current_positions(nq);
if (plant_context) {
current_positions = plant.GetPositions(*plant_context);
for (JointIndex i : plant.GetJointIndices()) {
const Joint<double>& 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).setConstant(-kInf);
ub.segment(start, size).setConstant(kInf);
bindings
.emplace_back(prog->AddLinearEqualityConstraint(
Eigen::MatrixXd::Identity(size, size),
current_positions.segment(start, size), q.segment(start, size)))
.evaluator()
->set_description(fmt::format("Joint {} lock", joint.name()));
is_locked.segment(start, size).array() = true;
}
}
}

// Add the unit quaternion constraints.
for (BodyIndex i{0}; i < plant.num_bodies(); ++i) {
const RigidBody<double>& body = plant.get_body(i);
if (body.has_quaternion_dofs()) {
const int start = body.floating_positions_start();
constexpr int size = 4;
// TODO(russt): Probably the joint limits should be [-1, 1] coming
// right out of the MultibodyPlant.
lb.segment<size>(start) = -Eigen::Vector4d::Ones();
ub.segment<size>(start) = Eigen::Vector4d::Ones();
if (plant_context && is_locked.segment<size>(start).any()) {
// Sanity check the MultibodyTree invariant.
DRAKE_DEMAND(is_locked.segment<size>(start).all());
// Lock to the normalized value, in lieu of a unit norm constraint.
const Eigen::Vector4d quat =
current_positions.segment<size>(start).normalized();
bindings
.emplace_back(prog->AddLinearEqualityConstraint(
Eigen::Matrix4d::Identity(),
quat, q.segment<size>(start)))
.evaluator()
->set_description(fmt::format("Body {} lock", body.name()));
prog->SetInitialGuess(q.segment<size>(start), quat);
} else {
bindings
.emplace_back(
prog->AddConstraint(solvers::Binding<solvers::Constraint>(
std::make_shared<UnitQuaternionConstraint>(),
q.segment<size>(start))))
.evaluator()
->set_description(fmt::format(
"Unit quaternion constraint for body {}", body.name()));
prog->SetInitialGuess(q.segment<size>(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<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));
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))
.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<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));
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))
.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<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));
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))
.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<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))
.evaluator()
->set_description(fmt::format(
"Weld orientation constraint between body {} and body {}",
spec.body_A, spec.body_B));
}
return bindings;
}
Expand Down
10 changes: 7 additions & 3 deletions multibody/inverse_kinematics/add_multibody_plant_constraints.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.,
Expand Down
68 changes: 13 additions & 55 deletions multibody/inverse_kinematics/inverse_kinematics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <limits>
#include <utility>

#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"
Expand Down Expand Up @@ -51,64 +52,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<bool> is_locked = VectorX<bool>::Constant(nq, false);
for (JointIndex i : plant.GetJointIndices()) {
const Joint<double>& 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<double>& 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<size>(start).any()) {
// Sanity check the MultibodyTree invariant.
DRAKE_DEMAND(is_locked.segment<size>(start).all());
// Lock to the normalized value, in lieu of a unit norm constraint.
const Eigen::Vector4d quat =
current_positions.segment<size>(start).normalized();
lb.segment<size>(start) = quat;
ub.segment<size>(start) = quat;
prog_->SetInitialGuess(q_.segment<size>(start), quat);
} else {
prog_->AddConstraint(solvers::Binding<solvers::Constraint>(
std::make_shared<UnitQuaternionConstraint>(),
q_.segment<size>(start)));
// Set a non-zero initial guess to help avoid singularities.
prog_->SetInitialGuess(q_.segment<size>(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<solvers::Constraint> InverseKinematics::AddPositionConstraint(
Expand Down
Loading

0 comments on commit 7562f89

Please sign in to comment.