forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
668 lines
25 KiB
668 lines
25 KiB
#include "drake/solvers/create_constraint.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <sstream>
|
|
|
|
#include <fmt/format.h>
|
|
|
|
#include "drake/common/symbolic/decompose.h"
|
|
#include "drake/math/quadratic_form.h"
|
|
#include "drake/solvers/decision_variable.h"
|
|
|
|
namespace drake {
|
|
namespace solvers {
|
|
namespace internal {
|
|
|
|
using std::find;
|
|
using std::isfinite;
|
|
using std::make_shared;
|
|
using std::numeric_limits;
|
|
using std::ostringstream;
|
|
using std::runtime_error;
|
|
using std::set;
|
|
using std::shared_ptr;
|
|
using std::unordered_map;
|
|
using std::vector;
|
|
|
|
using symbolic::Expression;
|
|
using symbolic::Formula;
|
|
using symbolic::Polynomial;
|
|
using symbolic::Variable;
|
|
using symbolic::Variables;
|
|
|
|
Binding<Constraint> ParseConstraint(
|
|
const Eigen::Ref<const VectorX<Expression>>& v,
|
|
const Eigen::Ref<const Eigen::VectorXd>& lb,
|
|
const Eigen::Ref<const Eigen::VectorXd>& ub) {
|
|
DRAKE_ASSERT(v.rows() == lb.rows() && v.rows() == ub.rows());
|
|
|
|
if (!IsAffine(v)) {
|
|
auto constraint = make_shared<ExpressionConstraint>(v, lb, ub);
|
|
return CreateBinding(constraint, constraint->vars());
|
|
} // else, continue on to linear-specific version below.
|
|
|
|
if ((ub - lb).isZero()) {
|
|
return ParseLinearEqualityConstraint(v, lb);
|
|
}
|
|
|
|
// Setup map_var_to_index and vars.
|
|
// such that map_var_to_index[vars(i)] = i
|
|
VectorXDecisionVariable vars;
|
|
unordered_map<Variable::Id, int> map_var_to_index;
|
|
std::tie(vars, map_var_to_index) =
|
|
symbolic::ExtractVariablesFromExpression(v);
|
|
|
|
// Construct A, new_lb, new_ub. map_var_to_index is used here.
|
|
Eigen::MatrixXd A{Eigen::MatrixXd::Zero(v.size(), vars.size())};
|
|
Eigen::VectorXd new_lb{v.size()};
|
|
Eigen::VectorXd new_ub{v.size()};
|
|
// We will determine if lb <= v <= ub is a bounding box constraint, namely
|
|
// x_lb <= x <= x_ub.
|
|
bool is_v_bounding_box = true;
|
|
Eigen::RowVectorXd Ai(A.cols());
|
|
for (int i = 0; i < v.size(); ++i) {
|
|
double constant_term = 0;
|
|
int num_vi_variables = symbolic::DecomposeAffineExpression(
|
|
v(i), map_var_to_index, &Ai, &constant_term);
|
|
A.row(i) = Ai;
|
|
if (num_vi_variables == 0 &&
|
|
!(lb(i) <= constant_term && constant_term <= ub(i))) {
|
|
// Unsatisfiable constraint with no variables, such as 1 <= 0 <= 2
|
|
throw std::runtime_error(
|
|
fmt::format("Constraint {} <= {} <= {} is unsatisfiable but called "
|
|
"with ParseConstraint.",
|
|
lb(i), v(i).to_string(), ub(i)));
|
|
|
|
} else {
|
|
new_lb(i) = lb(i) - constant_term;
|
|
new_ub(i) = ub(i) - constant_term;
|
|
DRAKE_DEMAND(!std::isnan(new_lb(i)));
|
|
DRAKE_DEMAND(!std::isnan(new_ub(i)));
|
|
if (num_vi_variables != 1) {
|
|
is_v_bounding_box = false;
|
|
}
|
|
}
|
|
}
|
|
if (is_v_bounding_box) {
|
|
// If every lb(i) <= v(i) <= ub(i) is a bounding box constraint, then
|
|
// formulate a bounding box constraint x_lb <= x <= x_ub
|
|
VectorXDecisionVariable bounding_box_x(v.size());
|
|
for (int i = 0; i < v.size(); ++i) {
|
|
// v(i) is in the form of c * x
|
|
double x_coeff = 0;
|
|
for (const auto& x : v(i).GetVariables()) {
|
|
const double coeff = A(i, map_var_to_index[x.get_id()]);
|
|
if (coeff != 0) {
|
|
x_coeff += coeff;
|
|
bounding_box_x(i) = x;
|
|
}
|
|
}
|
|
if (x_coeff > 0) {
|
|
new_lb(i) /= x_coeff;
|
|
new_ub(i) /= x_coeff;
|
|
} else {
|
|
const double lb_i = new_lb(i);
|
|
new_lb(i) = new_ub(i) / x_coeff;
|
|
new_ub(i) = lb_i / x_coeff;
|
|
}
|
|
DRAKE_DEMAND(!std::isnan(new_lb(i)));
|
|
DRAKE_DEMAND(!std::isnan(new_ub(i)));
|
|
}
|
|
return CreateBinding(make_shared<BoundingBoxConstraint>(new_lb, new_ub),
|
|
bounding_box_x);
|
|
}
|
|
|
|
return CreateBinding(make_shared<LinearConstraint>(A, new_lb, new_ub), vars);
|
|
}
|
|
|
|
std::unique_ptr<Binding<Constraint>> MaybeParseLinearConstraint(
|
|
const symbolic::Expression& e, double lb, double ub) {
|
|
if (!e.is_polynomial()) {
|
|
return std::unique_ptr<Binding<Constraint>>{nullptr};
|
|
}
|
|
const Polynomial p{e};
|
|
if (p.TotalDegree() > 1) {
|
|
return std::unique_ptr<Binding<Constraint>>{nullptr};
|
|
}
|
|
// If p only has one indeterminates, then we can always return a bounding box
|
|
// constraint.
|
|
if (p.indeterminates().size() == 1) {
|
|
// We decompose the polynomial `p` into `constant_term + coeff * var`.
|
|
double coeff = 0;
|
|
double constant_term = 0;
|
|
for (const auto& term : p.monomial_to_coefficient_map()) {
|
|
if (term.first.total_degree() == 0) {
|
|
constant_term += get_constant_value(term.second);
|
|
} else {
|
|
coeff += get_constant_value(term.second);
|
|
}
|
|
}
|
|
// coeff should not be 0. The symbolic polynomial should be able to detect
|
|
// when the coefficient is 0, and remove it from
|
|
// monomial_to_coefficient_map.
|
|
DRAKE_DEMAND(coeff != 0);
|
|
double var_lower{}, var_upper{};
|
|
if (coeff > 0) {
|
|
var_lower = (lb - constant_term) / coeff;
|
|
var_upper = (ub - constant_term) / coeff;
|
|
} else {
|
|
var_lower = (ub - constant_term) / coeff;
|
|
var_upper = (lb - constant_term) / coeff;
|
|
}
|
|
return std::make_unique<Binding<Constraint>>(
|
|
std::make_shared<BoundingBoxConstraint>(Vector1d(var_lower),
|
|
Vector1d(var_upper)),
|
|
Vector1<symbolic::Variable>(*(p.indeterminates().begin())));
|
|
}
|
|
VectorX<symbolic::Variable> bound_variables(p.indeterminates().size());
|
|
std::unordered_map<symbolic::Variable::Id, int> map_var_to_index;
|
|
int index = 0;
|
|
for (const auto& var : p.indeterminates()) {
|
|
bound_variables(index) = var;
|
|
map_var_to_index.emplace(var.get_id(), index++);
|
|
}
|
|
Eigen::RowVectorXd a(p.indeterminates().size());
|
|
a.setZero();
|
|
double lower = lb;
|
|
double upper = ub;
|
|
for (const auto& term : p.monomial_to_coefficient_map()) {
|
|
if (term.first.total_degree() == 0) {
|
|
const double coeff = get_constant_value(term.second);
|
|
lower -= coeff;
|
|
upper -= coeff;
|
|
} else {
|
|
const int var_index =
|
|
map_var_to_index.at(term.first.GetVariables().begin()->get_id());
|
|
a(var_index) = get_constant_value(term.second);
|
|
}
|
|
}
|
|
if (lower == upper) {
|
|
return std::make_unique<Binding<Constraint>>(
|
|
std::make_shared<LinearEqualityConstraint>(a, Vector1d(lower)),
|
|
bound_variables);
|
|
} else {
|
|
return std::make_unique<Binding<Constraint>>(
|
|
std::make_shared<LinearConstraint>(a, Vector1d(lower), Vector1d(upper)),
|
|
bound_variables);
|
|
}
|
|
}
|
|
|
|
namespace {
|
|
// Given two symbolic expressions, e1 and e2, finds an equi-satisfiable
|
|
// constraint `e <= c` for `e1 <= e2`. First, it decomposes e1 and e2 into `e1 =
|
|
// c1 + e1'` and `e2 = c2 + e2'`. Then it does the following case analysis.
|
|
//
|
|
// Case 1: If c1 or c2 are finite, we use the following derivations:
|
|
//
|
|
// e1 <= e2
|
|
// -> c1 + e1' <= c2 + e2'
|
|
// -> e1' - e2' <= c2 - c1.
|
|
//
|
|
// and set e := e1' - e2' and c := c2 - c1.
|
|
//
|
|
// Case 2: If both c1 and c2 are infinite. We use the following table
|
|
//
|
|
// c1 c2
|
|
// --------------------------
|
|
// +∞ <= +∞ Trivially holds.
|
|
// +∞ <= -∞ Infeasible.
|
|
// -∞ <= +∞ Trivially holds.
|
|
// -∞ <= -∞ Trivially holds.
|
|
//
|
|
// and throw an exception for all the cases.
|
|
//
|
|
// Note that c1 (resp. c2) can be infinite only if e1 (resp. e2) is zero.
|
|
// Otherwise, it throws an exception. To understand this side-condition,
|
|
// consider the following example:
|
|
//
|
|
// e1 = 0
|
|
// e2 = x + ∞
|
|
//
|
|
// e1 <= e2 := 0 <= x + ∞ -- (1)
|
|
//
|
|
// Without the side-condition, we might derive the following (wrong)
|
|
// equi-satisfiable constraint:
|
|
//
|
|
// -x <= ∞ -- (2)
|
|
//
|
|
// This is problematic because x ↦ -∞ is a satisfying constraint of
|
|
// (2) but it's not for (1) since we have:
|
|
//
|
|
// 0 <= -∞ + ∞
|
|
// 0 <= nan
|
|
// False.
|
|
//
|
|
void FindBound(const Expression& e1, const Expression& e2, Expression* const e,
|
|
double* const c) {
|
|
DRAKE_ASSERT(e != nullptr);
|
|
DRAKE_ASSERT(c != nullptr);
|
|
double c1 = 0;
|
|
double c2 = 0;
|
|
const Expression e1_expanded{e1.Expand()};
|
|
if (is_constant(e1_expanded)) {
|
|
c1 = get_constant_value(e1_expanded);
|
|
} else if (is_addition(e1_expanded)) {
|
|
c1 = get_constant_in_addition(e1_expanded);
|
|
if (!isfinite(c1)) {
|
|
ostringstream oss;
|
|
oss << "FindBound() cannot handle the constraint: " << e1 << " <= " << e2
|
|
<< " because " << e1
|
|
<< " has infinity in the constant term after expansion.";
|
|
throw runtime_error{oss.str()};
|
|
}
|
|
*e = Expression::Zero();
|
|
for (const auto& p : get_expr_to_coeff_map_in_addition(e1_expanded)) {
|
|
*e += p.first * p.second;
|
|
}
|
|
} else {
|
|
*e = e1_expanded;
|
|
}
|
|
const Expression e2_expanded{e2.Expand()};
|
|
if (is_constant(e2_expanded)) {
|
|
c2 = get_constant_value(e2_expanded);
|
|
} else if (is_addition(e2_expanded)) {
|
|
c2 = get_constant_in_addition(e2_expanded);
|
|
if (!isfinite(c2)) {
|
|
ostringstream oss;
|
|
oss << "FindBound() cannot handle the constraint: " << e1 << " <= " << e2
|
|
<< " because " << e2
|
|
<< " has infinity in the constant term after expansion.";
|
|
throw runtime_error{oss.str()};
|
|
}
|
|
for (const auto& p : get_expr_to_coeff_map_in_addition(e2_expanded)) {
|
|
*e -= p.first * p.second;
|
|
}
|
|
} else {
|
|
*e -= e2_expanded;
|
|
}
|
|
if (isfinite(c1) || isfinite(c2)) {
|
|
*c = c2 - c1;
|
|
return;
|
|
}
|
|
// Handle special cases where both of `c1` and `c2` are infinite.
|
|
// c1 c2
|
|
// --------------------------
|
|
// +∞ <= +∞ Trivially holds.
|
|
// +∞ <= -∞ Infeasible.
|
|
// -∞ <= +∞ Trivially holds.
|
|
// -∞ <= -∞ Trivially holds.
|
|
ostringstream oss;
|
|
if (c1 == numeric_limits<double>::infinity() &&
|
|
c2 == -numeric_limits<double>::infinity()) {
|
|
oss << "FindBound() detects an infeasible constraint: " << e1
|
|
<< " <= " << e2 << ".";
|
|
throw runtime_error{oss.str()};
|
|
} else {
|
|
oss << "FindBound() detects a trivial constraint: " << e1 << " <= " << e2
|
|
<< ".";
|
|
throw runtime_error{oss.str()};
|
|
}
|
|
}
|
|
} // namespace
|
|
|
|
Binding<Constraint> ParseConstraint(
|
|
const Eigen::Ref<const MatrixX<symbolic::Formula>>& formulas) {
|
|
const int n = formulas.rows() * formulas.cols();
|
|
|
|
// Decomposes 2D-array of formulas into 1D-vector of expression, `v`, and two
|
|
// 1D-vector of double `lb` and `ub`.
|
|
VectorX<Expression> v{n};
|
|
Eigen::VectorXd lb{n};
|
|
Eigen::VectorXd ub{n};
|
|
int k{0}; // index variable for 1D components.
|
|
for (int j{0}; j < formulas.cols(); ++j) {
|
|
for (int i{0}; i < formulas.rows(); ++i) {
|
|
const symbolic::Formula& f{formulas(i, j)};
|
|
if (symbolic::is_false(f)) {
|
|
throw std::runtime_error(
|
|
fmt::format("ParseConstraint is called with formulas({}, {}) being "
|
|
"always false",
|
|
i, j));
|
|
} else if (symbolic::is_true(f)) {
|
|
continue;
|
|
} else if (is_equal_to(f)) {
|
|
// f(i) := (lhs == rhs)
|
|
// (lhs - rhs == 0)
|
|
v(k) = get_lhs_expression(f) - get_rhs_expression(f);
|
|
lb(k) = 0.0;
|
|
ub(k) = 0.0;
|
|
} else if (is_less_than_or_equal_to(f)) {
|
|
// f(i) := (lhs <= rhs)
|
|
// (-∞ <= lhs - rhs <= 0)
|
|
v(k) = get_lhs_expression(f) - get_rhs_expression(f);
|
|
lb(k) = -std::numeric_limits<double>::infinity();
|
|
ub(k) = 0.0;
|
|
} else if (is_greater_than_or_equal_to(f)) {
|
|
// f(i) := (lhs >= rhs)
|
|
// (∞ >= lhs - rhs >= 0)
|
|
v(k) = get_lhs_expression(f) - get_rhs_expression(f);
|
|
lb(k) = 0.0;
|
|
ub(k) = std::numeric_limits<double>::infinity();
|
|
} else {
|
|
std::ostringstream oss;
|
|
oss << "ParseConstraint is called with an "
|
|
"array of formulas which includes a formula "
|
|
<< f
|
|
<< " which is not a relational formula using one of {==, <=, >=} "
|
|
"operators.";
|
|
throw std::runtime_error(oss.str());
|
|
}
|
|
++k;
|
|
}
|
|
}
|
|
if (k == 0) {
|
|
// All formulas are always True, return an empty bounding box constraint.
|
|
return internal::CreateBinding(std::make_shared<BoundingBoxConstraint>(
|
|
Eigen::VectorXd(0), Eigen::VectorXd(0)),
|
|
VectorXDecisionVariable(0));
|
|
}
|
|
return ParseConstraint(v.head(k), lb.head(k), ub.head(k));
|
|
}
|
|
|
|
Binding<Constraint> ParseConstraint(const Formula& f) {
|
|
if (symbolic::is_false(f)) {
|
|
throw std::runtime_error(
|
|
"ParseConstraint is called with a formula being always false.");
|
|
} else if (symbolic::is_true(f)) {
|
|
return internal::CreateBinding(std::make_shared<BoundingBoxConstraint>(
|
|
Eigen::VectorXd(0), Eigen::VectorXd(0)),
|
|
VectorXDecisionVariable(0));
|
|
} else if (is_equal_to(f)) {
|
|
// e1 == e2
|
|
const Expression& e1{get_lhs_expression(f)};
|
|
const Expression& e2{get_rhs_expression(f)};
|
|
return ParseConstraint(e1 - e2, 0.0, 0.0);
|
|
} else if (is_greater_than_or_equal_to(f)) {
|
|
// e1 >= e2
|
|
const Expression& e1{get_lhs_expression(f)};
|
|
const Expression& e2{get_rhs_expression(f)};
|
|
Expression e;
|
|
double ub = 0.0;
|
|
FindBound(e2, e1, &e, &ub);
|
|
return ParseConstraint(e, -numeric_limits<double>::infinity(), ub);
|
|
} else if (is_less_than_or_equal_to(f)) {
|
|
// e1 <= e2
|
|
const Expression& e1{get_lhs_expression(f)};
|
|
const Expression& e2{get_rhs_expression(f)};
|
|
Expression e;
|
|
double ub = 0.0;
|
|
FindBound(e1, e2, &e, &ub);
|
|
return ParseConstraint(e, -numeric_limits<double>::infinity(), ub);
|
|
} else if (is_conjunction(f)) {
|
|
const std::set<Formula>& operands = get_operands(f);
|
|
// TODO(jwnimmer-tri) We should use an absl::InlinedVector here.
|
|
const std::vector<Formula> vec_operands(operands.begin(), operands.end());
|
|
const Eigen::Map<const VectorX<Formula>> map_operands(vec_operands.data(),
|
|
vec_operands.size());
|
|
return ParseConstraint(map_operands);
|
|
}
|
|
ostringstream oss;
|
|
oss << "ParseConstraint is called with a formula " << f
|
|
<< " which is neither a relational formula using one of {==, <=, >=} "
|
|
"operators nor a conjunction of those relational formulas.";
|
|
throw runtime_error(oss.str());
|
|
}
|
|
|
|
Binding<LinearEqualityConstraint> ParseLinearEqualityConstraint(
|
|
const set<Formula>& formulas) {
|
|
const auto n = formulas.size();
|
|
// Decomposes a set of formulas, `{e₁₁ == e₁₂, ..., eₙ₁ == eₙ₂}`
|
|
// into a 1D-vector of expressions, `v = [e₁₁ - e₁₂, ..., eₙ₁ - eₙ₂]`.
|
|
VectorX<symbolic::Expression> v{n};
|
|
int i{0}; // index variable used in the loop
|
|
for (const symbolic::Formula& f : formulas) {
|
|
if (symbolic::is_false(f)) {
|
|
throw std::runtime_error(
|
|
"ParseLinearEqualityConstraint is called with one of formulas being "
|
|
"always false.");
|
|
} else if (symbolic::is_true(f)) {
|
|
continue;
|
|
} else if (is_equal_to(f)) {
|
|
// f := (lhs == rhs)
|
|
// (lhs - rhs == 0)
|
|
v(i) = get_lhs_expression(f) - get_rhs_expression(f);
|
|
} else {
|
|
ostringstream oss;
|
|
oss << "ParseLinearEqualityConstraint(const "
|
|
<< "set<Formula>& formulas) is called while its argument 'formulas' "
|
|
<< "includes a non-equality formula " << f << ".";
|
|
throw runtime_error(oss.str());
|
|
}
|
|
++i;
|
|
}
|
|
if (i == 0) {
|
|
// All formulas are always true, return an empty linear equality constraint.
|
|
return internal::CreateBinding(
|
|
std::make_shared<LinearEqualityConstraint>(
|
|
Eigen::Matrix<double, 0, 0>(), Eigen::Matrix<double, 0, 1>()),
|
|
Eigen::Matrix<symbolic::Variable, 0, 1>());
|
|
}
|
|
return ParseLinearEqualityConstraint(v.head(i), Eigen::VectorXd::Zero(i));
|
|
}
|
|
|
|
Binding<LinearEqualityConstraint> ParseLinearEqualityConstraint(
|
|
const Formula& f) {
|
|
if (symbolic::is_false(f)) {
|
|
throw std::runtime_error(
|
|
"ParseLinearEqualityConstraint is called with a formula being always "
|
|
"false.");
|
|
}
|
|
if (symbolic::is_true(f)) {
|
|
// The formula is always true, return an empty linear equality constraint.
|
|
return internal::CreateBinding(
|
|
std::make_shared<LinearEqualityConstraint>(
|
|
Eigen::Matrix<double, 0, 0>(), Eigen::Matrix<double, 0, 1>()),
|
|
Eigen::Matrix<symbolic::Variable, 0, 1>());
|
|
} else if (is_equal_to(f)) {
|
|
// e1 == e2
|
|
const Expression& e1{get_lhs_expression(f)};
|
|
const Expression& e2{get_rhs_expression(f)};
|
|
return ParseLinearEqualityConstraint(e1 - e2, 0.0);
|
|
}
|
|
if (is_conjunction(f)) {
|
|
return ParseLinearEqualityConstraint(get_operands(f));
|
|
}
|
|
ostringstream oss;
|
|
oss << "ParseLinearConstraint is called with a formula " << f
|
|
<< " which is neither an equality formula nor a conjunction of equality "
|
|
"formulas.";
|
|
throw runtime_error(oss.str());
|
|
}
|
|
|
|
Binding<LinearEqualityConstraint> DoParseLinearEqualityConstraint(
|
|
const Eigen::Ref<const VectorX<Expression>>& v,
|
|
const Eigen::Ref<const Eigen::VectorXd>& b) {
|
|
DRAKE_DEMAND(v.rows() == b.rows());
|
|
VectorX<symbolic::Variable> vars;
|
|
unordered_map<Variable::Id, int> map_var_to_index;
|
|
std::tie(vars, map_var_to_index) =
|
|
symbolic::ExtractVariablesFromExpression(v);
|
|
// TODO(hongkai.dai): use sparse matrix.
|
|
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(v.rows(), vars.rows());
|
|
Eigen::VectorXd beq = Eigen::VectorXd::Zero(v.rows());
|
|
Eigen::RowVectorXd Ai(A.cols());
|
|
for (int i = 0; i < v.rows(); ++i) {
|
|
double constant_term(0);
|
|
symbolic::DecomposeAffineExpression(v(i), map_var_to_index, &Ai,
|
|
&constant_term);
|
|
A.row(i) = Ai;
|
|
beq(i) = b(i) - constant_term;
|
|
}
|
|
return CreateBinding(make_shared<LinearEqualityConstraint>(A, beq), vars);
|
|
}
|
|
|
|
Binding<QuadraticConstraint> ParseQuadraticConstraint(
|
|
const symbolic::Expression& e, double lower_bound, double upper_bound,
|
|
std::optional<QuadraticConstraint::HessianType> hessian_type) {
|
|
// First build an Eigen vector that contains all the bound variables.
|
|
auto p = symbolic::ExtractVariablesFromExpression(e);
|
|
const auto& vars_vec = p.first;
|
|
const auto& map_var_to_index = p.second;
|
|
|
|
// Now decompose the expression into coefficients and monomials.
|
|
const symbolic::Polynomial poly{e};
|
|
|
|
Eigen::MatrixXd Q(vars_vec.size(), vars_vec.size());
|
|
Eigen::VectorXd b(vars_vec.size());
|
|
double constant_term;
|
|
// Decompose the polynomial as 0.5xᵀQx + bᵀx + k.
|
|
symbolic::DecomposeQuadraticPolynomial(poly, map_var_to_index, &Q, &b,
|
|
&constant_term);
|
|
// The constraint to be imposed is
|
|
// lb - k ≤ 0.5 xᵀQx + bᵀx ≤ ub - k
|
|
return CreateBinding(make_shared<QuadraticConstraint>(
|
|
Q, b, lower_bound - constant_term,
|
|
upper_bound - constant_term, hessian_type),
|
|
vars_vec);
|
|
}
|
|
|
|
shared_ptr<Constraint> MakePolynomialConstraint(
|
|
const VectorXPoly& polynomials,
|
|
const vector<Polynomiald::VarType>& poly_vars, const Eigen::VectorXd& lb,
|
|
const Eigen::VectorXd& ub) {
|
|
// Polynomials that are actually affine (a sum of linear terms + a
|
|
// constant) can be special-cased. Other polynomials are treated as
|
|
// generic for now.
|
|
// TODO(ggould-tri) There may be other such special easy cases.
|
|
bool all_affine = true;
|
|
for (int i = 0; i < polynomials.rows(); i++) {
|
|
if (!polynomials[i].IsAffine()) {
|
|
all_affine = false;
|
|
break;
|
|
}
|
|
}
|
|
if (all_affine) {
|
|
Eigen::MatrixXd linear_constraint_matrix =
|
|
Eigen::MatrixXd::Zero(polynomials.rows(), poly_vars.size());
|
|
Eigen::VectorXd linear_constraint_lb = lb;
|
|
Eigen::VectorXd linear_constraint_ub = ub;
|
|
for (int poly_num = 0; poly_num < polynomials.rows(); poly_num++) {
|
|
for (const auto& monomial : polynomials[poly_num].GetMonomials()) {
|
|
if (monomial.terms.size() == 0) {
|
|
linear_constraint_lb[poly_num] -= monomial.coefficient;
|
|
linear_constraint_ub[poly_num] -= monomial.coefficient;
|
|
} else {
|
|
DRAKE_DEMAND(monomial.terms.size() == 1); // Because isAffine().
|
|
const Polynomiald::VarType term_var = monomial.terms[0].var;
|
|
int var_num = (find(poly_vars.begin(), poly_vars.end(), term_var) -
|
|
poly_vars.begin());
|
|
DRAKE_ASSERT(var_num < static_cast<int>(poly_vars.size()));
|
|
linear_constraint_matrix(poly_num, var_num) = monomial.coefficient;
|
|
}
|
|
}
|
|
}
|
|
if (ub == lb) {
|
|
return make_shared<LinearEqualityConstraint>(linear_constraint_matrix,
|
|
linear_constraint_ub);
|
|
} else {
|
|
return make_shared<LinearConstraint>(
|
|
linear_constraint_matrix, linear_constraint_lb, linear_constraint_ub);
|
|
}
|
|
} else {
|
|
return make_shared<PolynomialConstraint>(polynomials, poly_vars, lb, ub);
|
|
}
|
|
}
|
|
|
|
Binding<LorentzConeConstraint> ParseLorentzConeConstraint(
|
|
const Eigen::Ref<const VectorX<Expression>>& v,
|
|
LorentzConeConstraint::EvalType eval_type) {
|
|
DRAKE_DEMAND(v.rows() >= 2);
|
|
Eigen::MatrixXd A{};
|
|
Eigen::VectorXd b(v.size());
|
|
VectorXDecisionVariable vars{};
|
|
symbolic::DecomposeAffineExpressions(v, &A, &b, &vars);
|
|
DRAKE_DEMAND(vars.rows() >= 1);
|
|
return CreateBinding(make_shared<LorentzConeConstraint>(A, b, eval_type),
|
|
vars);
|
|
}
|
|
|
|
Binding<LorentzConeConstraint> ParseLorentzConeConstraint(
|
|
const Expression& linear_expr, const Expression& quadratic_expr, double tol,
|
|
LorentzConeConstraint::EvalType eval_type) {
|
|
const auto& quadratic_p =
|
|
symbolic::ExtractVariablesFromExpression(quadratic_expr);
|
|
const auto& quadratic_vars = quadratic_p.first;
|
|
const auto& quadratic_var_to_index_map = quadratic_p.second;
|
|
const symbolic::Polynomial poly{quadratic_expr};
|
|
Eigen::MatrixXd Q(quadratic_vars.size(), quadratic_vars.size());
|
|
Eigen::VectorXd b(quadratic_vars.size());
|
|
double a;
|
|
symbolic::DecomposeQuadraticPolynomial(poly, quadratic_var_to_index_map, &Q,
|
|
&b, &a);
|
|
// The constraint that the linear expression v1 satisfying
|
|
// v1 >= sqrt(0.5 * x' * Q * x + b' * x + a), is equivalent to the vector
|
|
// [z; y] being within a Lorentz cone, where
|
|
// z = v1
|
|
// y = C * x + d
|
|
// such that yᵀy = 0.5xᵀQx + bᵀx + a
|
|
|
|
VectorX<Expression> expr{};
|
|
Eigen::MatrixXd C;
|
|
Eigen::VectorXd d;
|
|
std::tie(C, d) = math::DecomposePositiveQuadraticForm(0.5 * Q, b, a, tol);
|
|
expr.resize(1 + C.rows());
|
|
// expr(0) is z
|
|
expr(0) = linear_expr;
|
|
// expr.segment(1, C.rows()) = y
|
|
expr.segment(1, C.rows()) = C * quadratic_vars + d;
|
|
return ParseLorentzConeConstraint(expr, eval_type);
|
|
}
|
|
|
|
Binding<RotatedLorentzConeConstraint> ParseRotatedLorentzConeConstraint(
|
|
const Eigen::Ref<const VectorX<symbolic::Expression>>& v) {
|
|
DRAKE_DEMAND(v.rows() >= 3);
|
|
Eigen::MatrixXd A{};
|
|
Eigen::VectorXd b(v.size());
|
|
VectorXDecisionVariable vars{};
|
|
symbolic::DecomposeAffineExpressions(v, &A, &b, &vars);
|
|
DRAKE_DEMAND(vars.rows() >= 1);
|
|
return CreateBinding(std::make_shared<RotatedLorentzConeConstraint>(A, b),
|
|
vars);
|
|
}
|
|
|
|
Binding<RotatedLorentzConeConstraint> ParseRotatedLorentzConeConstraint(
|
|
const symbolic::Expression& linear_expr1,
|
|
const symbolic::Expression& linear_expr2,
|
|
const symbolic::Expression& quadratic_expr, double tol) {
|
|
const auto& quadratic_p =
|
|
symbolic::ExtractVariablesFromExpression(quadratic_expr);
|
|
const auto& quadratic_vars = quadratic_p.first;
|
|
const auto& quadratic_var_to_index_map = quadratic_p.second;
|
|
const symbolic::Polynomial poly{quadratic_expr};
|
|
Eigen::MatrixXd Q(quadratic_vars.size(), quadratic_vars.size());
|
|
Eigen::VectorXd b(quadratic_vars.size());
|
|
double a;
|
|
symbolic::DecomposeQuadraticPolynomial(poly, quadratic_var_to_index_map, &Q,
|
|
&b, &a);
|
|
|
|
Eigen::MatrixXd C;
|
|
Eigen::VectorXd d;
|
|
std::tie(C, d) = math::DecomposePositiveQuadraticForm(0.5 * Q, b, a, tol);
|
|
VectorX<symbolic::Expression> expr(2 + C.rows());
|
|
expr(0) = linear_expr1;
|
|
expr(1) = linear_expr2;
|
|
expr.tail(C.rows()) = C * quadratic_vars + d;
|
|
return ParseRotatedLorentzConeConstraint(expr);
|
|
}
|
|
|
|
std::shared_ptr<RotatedLorentzConeConstraint>
|
|
ParseQuadraticAsRotatedLorentzConeConstraint(
|
|
const Eigen::Ref<const Eigen::MatrixXd>& Q,
|
|
const Eigen::Ref<const Eigen::VectorXd>& b, double c, double zero_tol) {
|
|
// [-bᵀx-c, 1, Fx] is in the rotated Lorentz cone, where FᵀF = 0.5 * Q
|
|
const Eigen::MatrixXd F = math::DecomposePSDmatrixIntoXtransposeTimesX(
|
|
(Q + Q.transpose()) / 4, zero_tol);
|
|
// A_lorentz * x + b_lorentz = [-bᵀx-c, 1, Fx]
|
|
Eigen::MatrixXd A_lorentz = Eigen::MatrixXd::Zero(2 + F.rows(), F.cols());
|
|
Eigen::VectorXd b_lorentz = Eigen::VectorXd::Zero(2 + F.rows());
|
|
A_lorentz.row(0) = -b.transpose();
|
|
b_lorentz(0) = -c;
|
|
b_lorentz(1) = 1;
|
|
A_lorentz.bottomRows(F.rows()) = F;
|
|
return std::make_shared<RotatedLorentzConeConstraint>(A_lorentz, b_lorentz);
|
|
}
|
|
} // namespace internal
|
|
} // namespace solvers
|
|
} // namespace drake
|