m6wf5oeyl
/
Conception
forked from pz4kybsvg/Conception
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
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.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'ee31ef97f7'
${ noResults }
Conception/drake-master/solvers/mixed_integer_rotation_cons...

1067 lines
49 KiB
Raw Normal View History Unescape

中期检验所用代码以及文档
2 years ago
#include "drake/solvers/mixed_integer_rotation_constraint.h"
#include <cmath>
#include <limits>
#include "drake/common/symbolic/replace_bilinear_terms.h"
#include "drake/math/gray_code.h"
#include "drake/solvers/integer_optimization_util.h"
#include "drake/solvers/mixed_integer_rotation_constraint_internal.h"
using drake::symbolic::Expression;
namespace drake {
namespace solvers {
namespace {
// Returns true if n is positive and n is a power of 2.
bool IsPowerOfTwo(int n) {
DRAKE_ASSERT(n > 0);
return (n & (n - 1)) == 0;
}
// Relax the unit length constraint x₀² + x₁² + x₂² = 1, to a set of linear
// constraints. The relaxation is achieved by assuming the intervals of xᵢ has
// been cut into smaller intervals in the form [φ(i) φ(i+1)], and xᵢ is
// constrained to be within one of the intervals. Namely we assume that
// xᵢ = φᵀ * λᵢ, where λ is the auxiliary variables, satisfying the SOS2
// constraint already.
// We know that due to the convexity of the curve w = x², we get
// x² ≥ 2φⱼ*x-φⱼ²
// where the right hand-side of the inequality is the tangent of the curve
// w = x² at φⱼ. Thus we have 1 ≥ sum_j sum_i 2φⱼ*xᵢ-φⱼ²
// Moreover, also due to the convexity of the curve w = x², we know
// xᵢ² ≤ sum_j φ(j)² * λᵢ(j)
// So we have the constraint
// 1 ≤ sum_i sum_j φ(j)² * λᵢ(j)
void AddUnitLengthConstraintWithSos2Lambda(
MathematicalProgram* prog, const Eigen::Ref<const Eigen::VectorXd>& phi,
const Eigen::Ref<const VectorXDecisionVariable>& lambda0,
const Eigen::Ref<const VectorXDecisionVariable>& lambda1,
const Eigen::Ref<const VectorXDecisionVariable>& lambda2) {
const int num_phi = phi.rows();
DRAKE_ASSERT(num_phi == lambda0.rows());
DRAKE_ASSERT(num_phi == lambda1.rows());
DRAKE_ASSERT(num_phi == lambda2.rows());
const symbolic::Expression x0{phi.dot(lambda0.cast<symbolic::Expression>())};
const symbolic::Expression x1{phi.dot(lambda1.cast<symbolic::Expression>())};
const symbolic::Expression x2{phi.dot(lambda2.cast<symbolic::Expression>())};
for (int phi0_idx = 0; phi0_idx < num_phi; phi0_idx++) {
const symbolic::Expression x0_square_lb{2 * phi(phi0_idx) * x0 -
std::pow(phi(phi0_idx), 2)};
for (int phi1_idx = 0; phi1_idx < num_phi; phi1_idx++) {
const symbolic::Expression x1_square_lb{2 * phi(phi1_idx) * x1 -
std::pow(phi(phi1_idx), 2)};
for (int phi2_idx = 0; phi2_idx < num_phi; phi2_idx++) {
const symbolic::Expression x2_square_lb{2 * phi(phi2_idx) * x2 -
std::pow(phi(phi2_idx), 2)};
symbolic::Expression x_sum_of_squares_lb{x0_square_lb + x1_square_lb +
x2_square_lb};
if (!is_constant(x_sum_of_squares_lb)) {
prog->AddLinearConstraint(x_sum_of_squares_lb <= 1);
}
}
}
}
symbolic::Expression x_square_ub{0};
for (int i = 0; i < num_phi; ++i) {
x_square_ub += phi(i) * phi(i) * (lambda0(i) + lambda1(i) + lambda2(i));
}
prog->AddLinearConstraint(x_square_ub >= 1);
}
std::pair<int, int> Index2Subscripts(int index, int num_rows, int num_cols) {
DRAKE_ASSERT(index >= 0 && index < num_rows * num_cols);
int column_index = index / num_rows;
int row_index = index - column_index * num_rows;
return std::make_pair(row_index, column_index);
}
// Relax the orthogonal constraint
// R.col(i)ᵀ * R.col(j) = 0
// R.row(i)ᵀ * R.row(j) = 0.
// and the cross product constraint
// R.col(i) x R.col(j) = R.col(k)
// R.row(i) x R.row(j) = R.row(k)
// To handle this non-convex bilinear product, we relax any bilinear product
// in the form x * y, we relax (x, y, w) to be in the convex hull of the
// curve w = x * y, and replace all the bilinear term x * y with w. For more
// details, @see AddBilinearProductMcCormickEnvelopeSos2.
void AddOrthogonalAndCrossProductConstraintRelaxationReplacingBilinearProduct(
MathematicalProgram* prog,
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& R,
const Eigen::Ref<const Eigen::VectorXd>& phi,
const std::array<std::array<VectorXDecisionVariable, 3>, 3>& B,
IntervalBinning interval_binning) {
VectorDecisionVariable<9> R_flat;
R_flat << R.col(0), R.col(1), R.col(2);
MatrixDecisionVariable<9, 9> W;
// We cannot call W.cast<symbolic::Expression>() directly, since the diagonal
// entries in W is un-assigned, namely they are dummy variables. Converting a
// dummy variable to a symbolic expression is illegal. So we create this new
// W_expr, containing W(i, j) on the off-diagonal entries.
Eigen::Matrix<symbolic::Expression, 9, 9> W_expr;
W_expr.setZero();
for (int i = 0; i < 9; ++i) {
int Ri_row, Ri_col;
std::tie(Ri_row, Ri_col) = Index2Subscripts(i, 3, 3);
for (int j = i + 1; j < 9; ++j) {
int Rj_row, Rj_col;
std::tie(Rj_row, Rj_col) = Index2Subscripts(j, 3, 3);
std::string W_ij_name =
"R(" + std::to_string(Ri_row) + "," + std::to_string(Ri_col) +
")*R(" + std::to_string(Rj_row) + "," + std::to_string(Rj_col) + ")";
W(i, j) = prog->NewContinuousVariables<1>(W_ij_name)(0);
W_expr(i, j) = symbolic::Expression(W(i, j));
auto lambda_bilinear = AddBilinearProductMcCormickEnvelopeSos2(
prog, R(Ri_row, Ri_col), R(Rj_row, Rj_col), W(i, j), phi, phi,
B[Ri_row][Ri_col].template cast<symbolic::Expression>(),
B[Rj_row][Rj_col].template cast<symbolic::Expression>(),
interval_binning);
// Both sum_n lambda_bilinear(m, n) and sum_m lambda_bilinear(m, n)
// satisfy the SOS2 constraint, and
// R(Ri_row, Ri_col) = φᵀ * (sum_n lambda_bilinear(m, n))
// R(Rj_row, Rj_col) = φᵀ * (sum_m lambda_bilinear(m, n).transpose())
// Since we also know that both lambda[Ri_row][Ri_col] and
// lambda[Rj_row][Rj_col] satisfy the SOS2 constraint, and
// R[Ri_row][Ri_col] = φᵀ * lambda[Ri_row][Ri_col]
// R[Rj_row][Rj_col] = φᵀ * lambda[Rj_row][Rj_col]
// So sum_n lambda_bilinear(m, n) = lambda[Ri_row][Ri_col] (1)
// sum_m lambda_bilinear(m, n).transpose() = lambda[Rj_row][Rj_col] (2)
// TODO(hongkai.dai): I found the computation could be faster if we do not
// add constraint (1) and (2). Should investigate the reason.
W(j, i) = W(i, j);
W_expr(j, i) = W_expr(i, j);
}
}
for (int i = 0; i < 3; ++i) {
for (int j = i + 1; j < 3; ++j) {
// Orthogonal constraint between R.col(i), R.col(j).
prog->AddLinearConstraint(
symbolic::ReplaceBilinearTerms(
R.col(i).dot(R.col(j).cast<symbolic::Expression>()), R_flat,
R_flat, W_expr) == 0);
// Orthogonal constraint between R.row(i), R.row(j)
prog->AddLinearConstraint(
symbolic::ReplaceBilinearTerms(
R.row(i).transpose().dot(
R.row(j).cast<symbolic::Expression>().transpose()),
R_flat, R_flat, W_expr) == 0);
}
}
for (int i = 0; i < 3; ++i) {
int j = (i + 1) % 3;
int k = (i + 2) % 3;
Vector3<symbolic::Expression> cross_product1 =
R.col(i).cross(R.col(j).cast<symbolic::Expression>());
Vector3<symbolic::Expression> cross_product2 = R.row(i).transpose().cross(
R.row(j).transpose().cast<symbolic::Expression>());
for (int row = 0; row < 3; ++row) {
// R.col(i) x R.col(j) = R.col(k).
prog->AddLinearConstraint(
symbolic::ReplaceBilinearTerms(cross_product1(row), R_flat, R_flat,
W_expr) == R(row, k));
// R.row(i) x R.row(j) = R.row(k).
prog->AddLinearConstraint(
symbolic::ReplaceBilinearTerms(cross_product2(row), R_flat, R_flat,
W_expr) == R(k, row));
}
}
}
/**
* Add the constraint that vector R.col(i) and R.col(j) are not in the
* same or opposite orthants. This constraint should be satisfied since
* R.col(i) should be perpendicular to R.col(j). For example, if both
* R.col(i) and R.col(j) are in the first orthant (+++), their inner product
* has to be non-negative. If the inner product of two first orthant vectors
* is exactly zero, then both vectors has to be on the boundaries of the first
* orthant. But we can then assign the vector to a different orthant. The same
* proof applies to the opposite orthant case.
* To impose the constraint that R.col(0) and R.col(1) are not both in the first
* orthant, we consider the constraint
* Bpos0.col(0).sum() + Bpos0.col(1).sum() <= 5.
* Namely, not all 6 entries in Bpos0.col(0) and Bpos0.col(1) can be 1 at the
* same time, which is another way of saying R.col(0) and R.col(1) cannot be
* both in the first orthant.
* Similarly we can impose the constraint on the other orthant.
* @param prog Add the constraint to this mathematical program.
* @param Bpos0 Defined in AddRotationMatrixMcCormickEnvelopeMilpConstraints(),
* Bpos0(i,j) = 1 => R(i, j) >= 0. Bpos0(i, j) = 0 => R(i, j) <= 0.
*/
void AddNotInSameOrOppositeOrthantConstraint(
MathematicalProgram* prog,
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& Bpos0) {
const std::array<std::pair<int, int>, 3> column_idx = {
{{0, 1}, {0, 2}, {1, 2}}};
for (const auto& column_pair : column_idx) {
const int col_idx0 = column_pair.first;
const int col_idx1 = column_pair.second;
for (int o = 0; o < 8; ++o) {
// To enforce that R.col(i) and R.col(j) are not simultaneously in the
// o'th orthant, we will impose the constraint
// vars_same_orthant.sum() < = 5. The variables in vars_same_orthant
// depend on the orthant number o.
// To enforce that R.col(i) and R.col(j) are not in the opposite
// orthants, we will impose the constraint
// vars_oppo_orthant.sum() <= 5. The variables in vars_oppo_orthant
// depnd on the orthant number o.
Vector6<symbolic::Expression> vars_same_orthant;
Vector6<symbolic::Expression> vars_oppo_orthant;
for (int axis = 0; axis < 3; ++axis) {
// axis chooses x, y, or z axis.
if (o & (1 << axis)) {
// If the orthant has positive value along the `axis`, then
// `vars_same_orthant` choose the positive component Bpos0.
vars_same_orthant(2 * axis) = Bpos0(axis, col_idx0);
vars_same_orthant(2 * axis + 1) = Bpos0(axis, col_idx1);
vars_oppo_orthant(2 * axis) = Bpos0(axis, col_idx0);
vars_oppo_orthant(2 * axis + 1) = 1 - Bpos0(axis, col_idx1);
} else {
// If the orthant has negative value along the `axis`, then
// `vars_same_orthant` choose the negative component 1 - Bpos0.
vars_same_orthant(2 * axis) = 1 - Bpos0(axis, col_idx0);
vars_same_orthant(2 * axis + 1) = 1 - Bpos0(axis, col_idx1);
vars_oppo_orthant(2 * axis) = 1 - Bpos0(axis, col_idx0);
vars_oppo_orthant(2 * axis + 1) = Bpos0(axis, col_idx1);
}
}
prog->AddLinearConstraint(vars_same_orthant.sum() <= 5);
prog->AddLinearConstraint(vars_oppo_orthant.sum() <= 5);
}
}
}
// For the cross product c = a x b, based on the sign of a and b, we can imply
// the sign of c for some cases. For example, c0 = a1 * b2 - a2 * b1, so if
// (a1, a2, b1, b2) has sign (+, -, +, +), then c0 has to have sign +.
// @param Bpos0. Bpos0(i, j) = 1 => R(i, j) ≥ 0, Bpos0(i, j) = 0 => R(i, j) ≤ 0
void AddCrossProductImpliedOrthantConstraint(
MathematicalProgram* prog,
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& Bpos0) {
// The vertices of the polytope {x | A * x <= b} correspond to valid sign
// assignment for (a1, b2, a2, b1, c0) in the example above.
// Since R.col(k) = R.col(i) x R.col(j), we then know that
// A * [Bpos0(1, i); Bpos0(2, j); Bpos0(1, j); Bpos0(2, i); Bpos0(0, k)] <= b.
// The matrix A and b is found, by considering the polytope, whose vertices
// are {0, 1}⁵, excluding the 8 points
// (a1, b2, a2, b1, c0)
// ( 0, 0, 1, 0, 0)
// ( 0, 0, 0, 1, 0)
// ( 1, 1, 1, 0, 0)
// ( 1, 1, 0, 1, 0)
// ( 1, 0, 0, 0, 1)
// ( 1, 0, 1, 1, 1)
// ( 0, 1, 0, 0, 1)
// ( 0, 1, 1, 1, 1)
// So this polytope has 2⁵ - 8 = 24 vertices in total.
// The matrix A and b are obtained by converting this polytope from its
// vertices (V-representation), to its facets (H-representation). We did
// this conversion through Multi-parametric toolbox. Here is the MATLAB code
// P = Polyhedron(V);
// P.computeHRep();
// A = P.A;
// b = P.b;
constexpr int A_rows = 18;
constexpr int A_cols = 5;
Eigen::Matrix<double, A_rows, A_cols> A;
Eigen::Matrix<double, A_rows, 1> b;
A << 0, 0, 0, 0, -1, 1, 1, 1, -1, -1, 1, 1, -1, 1, -1, 0, 0, -1, 0, 0, -1, 0,
0, 0, 0, -1, -1, -1, 1, -1, -1, -1, 1, -1, -1, 0, -1, 0, 0, 0, 1, -1, -1,
-1, 1, -1, 1, -1, -1, 1, 0, 0, 0, -1, 0, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1,
0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0;
b << 0, 2, 2, 0, 0, 0, 0, 0, 1, 1, 0, 3, 3, 1, 1, 1, 1, 1;
for (int col0 = 0; col0 < 3; ++col0) {
int col1 = (col0 + 1) % 3;
int col2 = (col1 + 1) % 3;
// R(k, col2) = R(i, col0) * R(j, col1) - R(j, col0) * R(i, col1)
// where (i, j, k) = (0, 1, 2), (1, 2, 0) or (2, 0, 1)
VectorDecisionVariable<A_cols> var;
for (int i = 0; i < 3; ++i) {
int j = (i + 1) % 3;
int k = (j + 1) % 3;
var << Bpos0(i, col0), Bpos0(j, col1), Bpos0(j, col0), Bpos0(i, col1),
Bpos0(k, col2);
prog->AddLinearConstraint(A,
Eigen::Matrix<double, A_rows, 1>::Constant(
-std::numeric_limits<double>::infinity()),
b, var);
}
}
}
void AddConstraintInferredFromTheSign(
MathematicalProgram* prog,
const std::array<std::array<VectorXDecisionVariable, 3>, 3>& B,
int num_intervals_per_half_axis, IntervalBinning interval_binning) {
// Bpos(i, j) = 1 => R(i, j) >= 0
// Bpos(i, j) = 0 => R(i, j) <= 0
MatrixDecisionVariable<3, 3> Bpos;
switch (interval_binning) {
case IntervalBinning::kLogarithmic: {
// If num_intervals_per_half_axis is a power of 2, then B[i][j](0)
// indicates the sign of R(i, j).
if (IsPowerOfTwo(num_intervals_per_half_axis)) {
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
Bpos(i, j) = B[i][j](0);
}
}
AddNotInSameOrOppositeOrthantConstraint(prog, Bpos);
AddNotInSameOrOppositeOrthantConstraint(prog, Bpos.transpose());
AddCrossProductImpliedOrthantConstraint(prog, Bpos);
AddCrossProductImpliedOrthantConstraint(prog, Bpos.transpose());
// If num_intervals_per_half_axis is a power of 2, and it's >= 2, then
// B[i][j](1) = 1 => -0.5 <= R(i, j) <= 0.5. Furthermore, we know for
// each row/column of R, it cannot have all three entries in the
// interval [-0.5, 0.5], since that would imply the norm of the
// row/column being less than sqrt(3)/2. Thus, we have
// sum_i B[i][j](1) <= 2 and sum_j B[i][j](1) <= 2
if (num_intervals_per_half_axis >= 2) {
for (int i = 0; i < 3; ++i) {
symbolic::Expression row_sum{0};
symbolic::Expression col_sum{0};
for (int j = 0; j < 3; ++j) {
row_sum += B[i][j](1);
col_sum += B[j][i](1);
}
prog->AddLinearConstraint(row_sum <= 2);
prog->AddLinearConstraint(col_sum <= 2);
}
}
}
break;
}
case IntervalBinning::kLinear: {
// Bpos(i, j) = B[i][j](N) ∨ B[i][j](N+1) ∨ ... ∨ B[i][j](2*N-1)
// where N = num_intervals_per_half_axis_;
// This "logical or" constraint can be written as
// Bpos(i, j) ≥ B[i][j](N + k) ∀ k = 0, ..., N-1
// Bpos(i, j) ≤ ∑ₖ B[i][j](N+k)
// 0 ≤ Bpos(i, j) ≤ 1
Bpos = prog->NewContinuousVariables<3, 3>("Bpos");
prog->AddBoundingBoxConstraint(0, 1, Bpos);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
// clang-format off
prog->AddLinearConstraint(Bpos(i, j) <=
B[i][j].tail(num_intervals_per_half_axis)
.cast<symbolic::Expression>().sum());
// clang-format on
for (int k = 0; k < num_intervals_per_half_axis; ++k) {
prog->AddLinearConstraint(Bpos(i, j) >=
B[i][j](num_intervals_per_half_axis + k));
}
}
}
AddNotInSameOrOppositeOrthantConstraint(prog, Bpos);
AddNotInSameOrOppositeOrthantConstraint(prog, Bpos.transpose());
AddCrossProductImpliedOrthantConstraint(prog, Bpos);
AddCrossProductImpliedOrthantConstraint(prog, Bpos.transpose());
break;
}
}
}
void AddRotationMatrixBilinearMcCormickConstraints(
MathematicalProgram* prog,
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& R,
const std::array<std::array<VectorXDecisionVariable, 3>, 3>& B,
const std::array<std::array<VectorXDecisionVariable, 3>, 3>& lambda,
const Eigen::Ref<const Eigen::VectorXd>& phi,
IntervalBinning interval_binning) {
for (int row = 0; row < 3; ++row) {
AddUnitLengthConstraintWithSos2Lambda(prog, phi, lambda[row][0],
lambda[row][1], lambda[row][2]);
}
for (int col = 0; col < 3; ++col) {
AddUnitLengthConstraintWithSos2Lambda(prog, phi, lambda[0][col],
lambda[1][col], lambda[2][col]);
}
AddOrthogonalAndCrossProductConstraintRelaxationReplacingBilinearProduct(
prog, R, phi, B, interval_binning);
}
// Given (an integer enumeration of) the orthant, takes a vector in the
// positive orthant into that orthant by flipping the signs of the individual
// elements.
Eigen::Vector3d FlipVector(const Eigen::Ref<const Eigen::Vector3d>& vpos,
int orthant) {
DRAKE_ASSERT(vpos(0) >= 0 && vpos(1) >= 0 && vpos(2) >= 0);
DRAKE_DEMAND(orthant >= 0 && orthant <= 7);
Eigen::Vector3d v = vpos;
if (orthant & (1 << 2)) v(0) = -v(0);
if (orthant & (1 << 1)) v(1) = -v(1);
if (orthant & 1) v(2) = -v(2);
return v;
}
// Given (an integer enumeration of) the orthant, return a vector c with
// c(i) = a(i) if element i is positive in the indicated orthant, otherwise
// c(i) = b(i).
template <typename Derived>
Eigen::Matrix<Derived, 3, 1> PickPermutation(
const Eigen::Matrix<Derived, 3, 1>& a,
const Eigen::Matrix<Derived, 3, 1>& b, int orthant) {
DRAKE_DEMAND(orthant >= 0 && orthant <= 7);
Eigen::Matrix<Derived, 3, 1> c = a;
if (orthant & (1 << 2)) c(0) = b(0);
if (orthant & (1 << 1)) c(1) = b(1);
if (orthant & 1) c(2) = b(2);
return c;
}
void AddBoxSphereIntersectionConstraints(
MathematicalProgram* prog, const VectorDecisionVariable<3>& v,
const std::vector<Vector3<Expression>>& cpos,
const std::vector<Vector3<Expression>>& cneg,
const VectorDecisionVariable<3>& v1, const VectorDecisionVariable<3>& v2,
const std::vector<std::vector<std::vector<std::vector<Eigen::Vector3d>>>>&
box_sphere_intersection_vertices,
const std::vector<
std::vector<std::vector<std::pair<Eigen::Vector3d, double>>>>&
box_sphere_intersection_halfspace) {
const int N = cpos.size(); // number of discretization points.
// Iterate through regions.
for (int xi = 0; xi < N; xi++) {
for (int yi = 0; yi < N; yi++) {
for (int zi = 0; zi < N; zi++) {
Vector3<Expression> this_cpos, this_cneg;
this_cpos << cpos[xi](0), cpos[yi](1), cpos[zi](2);
this_cneg << cneg[xi](0), cneg[yi](1), cneg[zi](2);
// If the box and the sphere surface has intersection
if (box_sphere_intersection_vertices[xi][yi][zi].size() > 0) {
// The box intersects with the surface of the unit sphere.
// Two possible cases
// 1. If the box bmin <= x <= bmax intersects with the surface of the
// unit sphere at a unique point (either bmin or bmax),
// 2. Otherwise, there is a region of intersection.
if (box_sphere_intersection_vertices[xi][yi][zi].size() == 1) {
// If box_min or box_max is on the sphere, then denote the point on
// the sphere as u, we have the following condition
// if c[xi](0) = 1 and c[yi](1) == 1 and c[zi](2) == 1, then
// v = u
// vᵀ * v1 = 0
// vᵀ * v2 = 0
// v.cross(v1) = v2
// Translate this to constraint, we have
// 2 * (c[xi](0) + c[yi](1) + c[zi](2)) - 6
// <= v - u <= -2 * (c[xi](0) + c[yi](1) + c[zi](2)) + 6
//
// c[xi](0) + c[yi](1) + c[zi](2) - 3
// <= vᵀ * v1 <= 3 - (c[xi](0) + c[yi](1) + c[zi](2))
//
// c[xi](0) + c[yi](1) + c[zi](2) - 3
// <= vᵀ * v2 <= 3 - (c[xi](0) + c[yi](1) + c[zi](2))
//
// 2 * c[xi](0) + c[yi](1) + c[zi](2) - 6
// <= v.cross(v1) - v2 <= 6 - 2 * (c[xi](0) + c[yi](1) +
// c[zi](2))
// `u` in the documentation above.
const Eigen::Vector3d unique_intersection =
box_sphere_intersection_vertices[xi][yi][zi][0];
Eigen::Vector3d orthant_u;
Vector3<Expression> orthant_c;
for (int o = 0; o < 8; o++) { // iterate over orthants
orthant_u = FlipVector(unique_intersection, o);
orthant_c = PickPermutation(this_cpos, this_cneg, o);
// TODO(hongkai.dai): remove this for loop when we can handle
// Eigen::Array of symbolic formulae.
Expression orthant_c_sum = orthant_c.cast<Expression>().sum();
for (int i = 0; i < 3; ++i) {
prog->AddLinearConstraint(v(i) - orthant_u(i) <=
6 - 2 * orthant_c_sum);
prog->AddLinearConstraint(v(i) - orthant_u(i) >=
2 * orthant_c_sum - 6);
}
const Expression v_dot_v1 = orthant_u.dot(v1);
const Expression v_dot_v2 = orthant_u.dot(v2);
prog->AddLinearConstraint(v_dot_v1 <= 3 - orthant_c_sum);
prog->AddLinearConstraint(orthant_c_sum - 3 <= v_dot_v1);
prog->AddLinearConstraint(v_dot_v2 <= 3 - orthant_c_sum);
prog->AddLinearConstraint(orthant_c_sum - 3 <= v_dot_v2);
const Vector3<Expression> v_cross_v1 = orthant_u.cross(v1);
for (int i = 0; i < 3; ++i) {
prog->AddLinearConstraint(v_cross_v1(i) - v2(i) <=
6 - 2 * orthant_c_sum);
prog->AddLinearConstraint(v_cross_v1(i) - v2(i) >=
2 * orthant_c_sum - 6);
}
}
} else {
// Find the intercepts of the unit sphere with the box, then find
// the tightest linear constraint of the form:
// d <= n'*v
// that puts v inside (but as close as possible to) the unit circle.
const double d =
box_sphere_intersection_halfspace[xi][yi][zi].second;
const Eigen::Vector3d& normal =
box_sphere_intersection_halfspace[xi][yi][zi].first;
Eigen::VectorXd b(0);
Eigen::Matrix<double, Eigen::Dynamic, 3> A(0, 3);
internal::ComputeInnerFacetsForBoxSphereIntersection(
box_sphere_intersection_vertices[xi][yi][zi], &A, &b);
// theta is the maximal angle between v and normal, where v is an
// intersecting point between the box and the sphere.
double cos_theta = d;
const double theta = std::acos(cos_theta);
Eigen::Matrix<double, 1, 6> a;
Eigen::Matrix<double, 3, 9> A_cross;
Eigen::Vector3d orthant_normal;
Vector3<Expression> orthant_c;
for (int o = 0; o < 8; o++) { // iterate over orthants
orthant_normal = FlipVector(normal, o);
orthant_c = PickPermutation(this_cpos, this_cneg, o);
for (int i = 0; i < A.rows(); ++i) {
// Add the constraint that A * v <= b, representing the inner
// facets f the convex hull, obtained from the vertices of the
// intersection region.
// This constraint is only active if the box is active.
// We impose the constraint
// A.row(i) * v - b(i) <= 3 - 3 * b(i) + (b(i) - 1) * (c[xi](0)
// + c[yi](1) + c[zi](2))
// Or in words
// If c[xi](0) = 1 and c[yi](1) = 1 and c[zi](1) = 1
// A.row(i) * v <= b(i)
// Otherwise
// A.row(i) * v -b(i) is not constrained
Eigen::Vector3d orthant_a =
-FlipVector(-A.row(i).transpose(), o);
prog->AddLinearConstraint(
orthant_a.dot(v) - b(i) <=
3 - 3 * b(i) +
(b(i) - 1) *
orthant_c.cast<symbolic::Expression>().sum());
}
// Max vector norm constraint: -1 <= normal'*x <= 1.
// No need to restrict to this orthant, but also no need to apply
// the same constraint twice (would be the same for opposite
// orthants), so skip all of the -x orthants.
if (o % 2 == 0)
prog->AddLinearConstraint(orthant_normal.transpose(), -1, 1, v);
const symbolic::Expression orthant_c_sum{orthant_c.sum()};
// Dot-product constraint: ideally v.dot(v1) = v.dot(v2) = 0.
// The cone of (unit) vectors within theta of the normal vector
// defines a band of admissible vectors v1 and v2 which are
// orthogonal to v. They must satisfy the constraint:
// -sin(theta) <= normal.dot(vi) <= sin(theta)
// Proof sketch:
// v is within theta of normal.
// => vi must be within theta of a vector orthogonal to
// the normal.
// => vi must be pi/2 +/- theta from the normal.
// => |normal||vi| cos(pi/2 + theta) <= normal.dot(vi) <=
// |normal||vi| cos(pi/2 - theta).
// Since normal and vi are both unit length,
// -sin(theta) <= normal.dot(vi) <= sin(theta).
// Note: (An alternative tighter, but SOCP constraint)
// v, v1, v2 forms an orthornormal basis. So n'*v is the
// projection of n in the v direction, same for n'*v1, n'*v2.
// Thus
// (n'*v)² + (n'*v1)² + (n'*v2)² = n'*n
// which translates to "The norm of a vector is equal to the
// sum of squares of the vector projected onto each axes of an
// orthornormal basis".
// This equation is the same as
// (nᵀ*v1)² + (nᵀ*v2)² <= sin(theta)²
// we can impose a tighter Lorentz cone constraint
// [|sin(theta)|, nᵀ*v1, nᵀ*v2] is in the Lorentz cone.
// We relax this Lorentz cone constraint by linear constraints
// -sinθ <= nᵀ * v1 <= sinθ
// -sinθ <= nᵀ * v2 <= sinθ
// To activate this constraint, we define
// c_sum = c[xi](0) + c[yi](1) + c[zi](2), and impose the
// following constraint using big-M approach.
// nᵀ * v1 - sinθ <= (1 - sinθ)*(3 - c_sum)
// nᵀ * v2 - sinθ <= (1 - sinθ)*(3 - c_sum)
// nᵀ * v1 + sinθ >= (-1 + sinθ)*(3 - c_sum)
// nᵀ * v2 + sinθ >= (-1 + sinθ)*(3 - c_sum)
const double sin_theta{sin(theta)};
prog->AddLinearConstraint(orthant_normal.dot(v1) + sin_theta >=
(-1 + sin_theta) * (3 - orthant_c_sum));
prog->AddLinearConstraint(orthant_normal.dot(v2) + sin_theta >=
(-1 + sin_theta) * (3 - orthant_c_sum));
prog->AddLinearConstraint(orthant_normal.dot(v1) - sin_theta <=
(1 - sin_theta) * (3 - orthant_c_sum));
prog->AddLinearConstraint(orthant_normal.dot(v2) - sin_theta <=
(1 - sin_theta) * (3 - orthant_c_sum));
// Cross-product constraint: ideally v2 = v.cross(v1).
// Since v is within theta of normal, we will prove that
// |v2 - normal.cross(v1)| <= 2 * sin(θ / 2)
// Notice that (v2 - normal.cross(v1))ᵀ * (v2 - normal.cross(v1))
// = v2ᵀ * v2 + |normal.cross(v1))|² -
// 2 * v2ᵀ * (normal.cross(v1))
// = 1 + |normal.cross(v1))|² - 2 * normalᵀ*(v1.cross(v2))
// = 1 + |normal.cross(v1))|² - 2 * normalᵀ*v
// <= 1 + 1 - 2 * cos(θ)
// = (2 * sin(θ / 2))²
// Thus we get |v2 - normal.cross(v1)| <= 2 * sin(θ / 2)
// Here we consider to use an elementwise linear constraint
// -2*sin(theta / 2) <= v2 - normal.cross(v1) <= 2*sin(θ / 2)
// Since 0<=θ<=pi/2, this should be enough to rule out the
// det(R)=-1 case (the shortest projection of a line across the
// circle onto a single axis has length 2sqrt(3)/3 > 1.15), and
// can be significantly tighter.
// To activate this only when the box is active, the complete
// constraints are
// -2*sin(θ/2)-(2 - 2sin(θ/2))*(3-(cxi+cyi+czi))
// <= v2-normal.cross(v1)
// <= 2*sin(θ/2)+(2 - 2sin(θ/2))*(3-(cxi+cyi+czi))
// Note: Again this constraint could be tighter as a Lorenz cone
// constraint of the form:
// |v2 - normal.cross(v1)| <= 2*sin(θ/2).
const double sin_theta2 = sin(theta / 2);
const Vector3<symbolic::Expression> v2_minus_n_cross_v1{
v2 - orthant_normal.cross(v1)};
prog->AddLinearConstraint(
v2_minus_n_cross_v1 <=
Vector3<symbolic::Expression>::Constant(
2 * sin_theta2 +
(2 - 2 * sin_theta2) * (3 - orthant_c_sum)));
prog->AddLinearConstraint(
v2_minus_n_cross_v1 >=
Vector3<symbolic::Expression>::Constant(
-2 * sin_theta2 +
(-2 + 2 * sin_theta2) * (3 - orthant_c_sum)));
}
}
} else {
// This box does not intersect with the surface of the sphere.
for (int o = 0; o < 8; ++o) { // iterate over orthants
prog->AddLinearConstraint(PickPermutation(this_cpos, this_cneg, o)
.cast<Expression>()
.sum(),
0.0, 2.0);
}
}
}
}
}
}
// This function just calls AddBoxSphereIntersectionConstraints for each
// row/column of R.
void AddBoxSphereIntersectionConstraintsForR(
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& R,
const std::vector<Matrix3<symbolic::Expression>>& CRpos,
const std::vector<Matrix3<symbolic::Expression>>& CRneg,
int num_intervals_per_half_axis,
const std::vector<std::vector<std::vector<std::vector<Eigen::Vector3d>>>>&
box_sphere_intersection_vertices,
const std::vector<
std::vector<std::vector<std::pair<Eigen::Vector3d, double>>>>&
box_sphere_intersection_halfspace,
MathematicalProgram* prog) {
// Add constraints to the column and row vectors.
std::vector<Vector3<Expression>> cpos(num_intervals_per_half_axis),
cneg(num_intervals_per_half_axis);
for (int i = 0; i < 3; i++) {
// Make lists of the decision variables in terms of column vectors and row
// vectors to facilitate the calls below.
// TODO(russt): Consider reorganizing the original CRpos/CRneg variables to
// avoid this (albeit minor) cost?
for (int k = 0; k < num_intervals_per_half_axis; k++) {
cpos[k] = CRpos[k].col(i);
cneg[k] = CRneg[k].col(i);
}
AddBoxSphereIntersectionConstraints(
prog, R.col(i), cpos, cneg, R.col((i + 1) % 3), R.col((i + 2) % 3),
box_sphere_intersection_vertices, box_sphere_intersection_halfspace);
for (int k = 0; k < num_intervals_per_half_axis; k++) {
cpos[k] = CRpos[k].row(i).transpose();
cneg[k] = CRneg[k].row(i).transpose();
}
AddBoxSphereIntersectionConstraints(
prog, R.row(i).transpose(), cpos, cneg, R.row((i + 1) % 3).transpose(),
R.row((i + 2) % 3).transpose(), box_sphere_intersection_vertices,
box_sphere_intersection_halfspace);
}
}
// B[i][j] contains a vector of binary variables, such that if B[i][j] is the
// reflected gray code representation of integer k, then R(i, j) is in the k'th
// interval. We will write CRpos and CRneg as expressions of B, such that
// CRpos[k](i, j) = 1 <=> phi(k) <= R(i, j) <= phi(k + 1) => B[i][j] represents
// num_interval_per_half_axis + k in reflected gray code.
// CRneg[k](i, j) = 1 <=> -phi(k + 1) <= R(i, j) <= -phi(k) => B[i][j]
// represents num_interval_per_half_axis - k - 1 in reflected gray code.
template <typename T>
void GetCRposAndCRnegForLogarithmicBinning(
const std::array<std::array<T, 3>, 3>& B, int num_intervals_per_half_axis,
MathematicalProgram* prog, std::vector<Matrix3<Expression>>* CRpos,
std::vector<Matrix3<Expression>>* CRneg) {
if (num_intervals_per_half_axis > 1) {
const auto gray_codes = math::CalculateReflectedGrayCodes(
CeilLog2(num_intervals_per_half_axis) + 1);
CRpos->clear();
CRneg->clear();
CRpos->reserve(num_intervals_per_half_axis);
CRneg->reserve(num_intervals_per_half_axis);
for (int k = 0; k < num_intervals_per_half_axis; ++k) {
CRpos->push_back(prog->NewContinuousVariables<3, 3>(
"CRpos[" + std::to_string(k) + "]"));
CRneg->push_back(prog->NewContinuousVariables<3, 3>(
"CRneg[" + std::to_string(k) + "]"));
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
prog->AddConstraint(CreateBinaryCodeMatchConstraint(
B[i][j],
gray_codes.row(num_intervals_per_half_axis + k).transpose(),
(*CRpos)[k](i, j)));
prog->AddConstraint(CreateBinaryCodeMatchConstraint(
B[i][j],
gray_codes.row(num_intervals_per_half_axis - k - 1).transpose(),
(*CRneg)[k](i, j)));
}
}
}
} else {
// num_interval_per_half_axis = 1 is the special case, B[i][j](0) = 0 if
// -1 <= R(i, j) <= 0, and B[i][j](0) = 1 if 0 <= R(i, j) <= 1
CRpos->resize(1);
CRneg->resize(1);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
(*CRpos)[0](i, j) = B[i][j](0);
(*CRneg)[0](i, j) = 1 - B[i][j](0);
}
}
}
}
/**
* Compute the vertices of the intersection region between the boxes and the
* sphere surface. Also compute the tightest halfspace that contains the
* intersection region.
*/
void ComputeBoxSphereIntersectionAndHalfSpace(
int num_intervals_per_half_axis,
const Eigen::Ref<const Eigen::VectorXd>& phi_nonnegative,
std::vector<std::vector<std::vector<std::vector<Eigen::Vector3d>>>>*
box_sphere_intersection_vertices,
std::vector<std::vector<std::vector<std::pair<Eigen::Vector3d, double>>>>*
box_sphere_intersection_halfspace) {
const double kEpsilon = std::numeric_limits<double>::epsilon();
box_sphere_intersection_vertices->resize(num_intervals_per_half_axis);
box_sphere_intersection_halfspace->resize(num_intervals_per_half_axis);
for (int xi = 0; xi < num_intervals_per_half_axis; ++xi) {
(*box_sphere_intersection_vertices)[xi].resize(num_intervals_per_half_axis);
(*box_sphere_intersection_halfspace)[xi].resize(
num_intervals_per_half_axis);
for (int yi = 0; yi < num_intervals_per_half_axis; ++yi) {
(*box_sphere_intersection_vertices)[xi][yi].resize(
num_intervals_per_half_axis);
(*box_sphere_intersection_halfspace)[xi][yi].resize(
num_intervals_per_half_axis);
for (int zi = 0; zi < num_intervals_per_half_axis; ++zi) {
const Eigen::Vector3d box_min(phi_nonnegative(xi), phi_nonnegative(yi),
phi_nonnegative(zi));
const Eigen::Vector3d box_max(phi_nonnegative(xi + 1),
phi_nonnegative(yi + 1),
phi_nonnegative(zi + 1));
const double box_min_norm = box_min.lpNorm<2>();
const double box_max_norm = box_max.lpNorm<2>();
if (box_min_norm <= 1.0 - kEpsilon && box_max_norm >= 1.0 + kEpsilon) {
// box_min is strictly inside the sphere, box_max is strictly
// outside of the sphere.
// We choose eps here, because if a vector x has unit length, and
// another vector y is different from x by eps (||x - y||∞ < eps),
// then max ||y||₂ - 1 is eps.
(*box_sphere_intersection_vertices)[xi][yi][zi] =
internal::ComputeBoxEdgesAndSphereIntersection(box_min, box_max);
DRAKE_DEMAND((*box_sphere_intersection_vertices)[xi][yi][zi].size() >=
3);
Eigen::Vector3d normal{};
internal::ComputeHalfSpaceRelaxationForBoxSphereIntersection(
(*box_sphere_intersection_vertices)[xi][yi][zi],
&((*box_sphere_intersection_halfspace)[xi][yi][zi].first),
&((*box_sphere_intersection_halfspace)[xi][yi][zi].second));
} else if (std::abs(box_min_norm - 1) < kEpsilon) {
// box_min is on the surface. This is the unique intersection point
// between the sphere surface and the box.
(*box_sphere_intersection_vertices)[xi][yi][zi].push_back(
box_min / box_min_norm);
} else if (std::abs(box_max_norm - 1) < kEpsilon) {
// box_max is on the surface. This is the unique intersection point
// between the sphere surface and the box.
(*box_sphere_intersection_vertices)[xi][yi][zi].push_back(
box_max / box_max_norm);
}
}
}
}
}
} // namespace
std::string to_string(MixedIntegerRotationConstraintGenerator::Approach type) {
switch (type) {
case MixedIntegerRotationConstraintGenerator::Approach::
kBoxSphereIntersection: {
return "box_sphere_intersection";
}
case MixedIntegerRotationConstraintGenerator::Approach::
kBilinearMcCormick: {
return "bilinear_mccormick";
}
case MixedIntegerRotationConstraintGenerator::Approach::kBoth: {
return "both";
}
}
// The following line should not be reached. We add it due to a compiler
// defect.
throw std::runtime_error("Should not reach this part of the code.\n");
}
std::ostream& operator<<(
std::ostream& os,
const MixedIntegerRotationConstraintGenerator::Approach& type) {
os << to_string(type);
return os;
}
MixedIntegerRotationConstraintGenerator::
MixedIntegerRotationConstraintGenerator(
MixedIntegerRotationConstraintGenerator::Approach approach,
int num_intervals_per_half_axis, IntervalBinning interval_binning)
: approach_{approach},
num_intervals_per_half_axis_{num_intervals_per_half_axis},
interval_binning_{interval_binning},
phi_nonnegative_{
Eigen::VectorXd::LinSpaced(num_intervals_per_half_axis_ + 1, 0, 1)} {
phi_.resize(2 * num_intervals_per_half_axis_ + 1);
phi_(num_intervals_per_half_axis_) = 0;
for (int i = 1; i <= num_intervals_per_half_axis_; ++i) {
phi_(num_intervals_per_half_axis_ - i) = -phi_nonnegative_(i);
phi_(num_intervals_per_half_axis_ + i) = phi_nonnegative_(i);
}
// If we consider the box-sphere intersection, then we need to compute the
// halfspace nᵀx≥ d, as the tightest halfspace for each intersection region.
if (approach_ == Approach::kBoxSphereIntersection ||
approach_ == Approach::kBoth) {
ComputeBoxSphereIntersectionAndHalfSpace(
num_intervals_per_half_axis_, phi_nonnegative_,
&box_sphere_intersection_vertices_,
&box_sphere_intersection_halfspace_);
}
}
MixedIntegerRotationConstraintGenerator::ReturnType
MixedIntegerRotationConstraintGenerator::AddToProgram(
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& R,
MathematicalProgram* prog) const {
ReturnType ret;
// Add new variable λ[i][j] and B[i][j] for R(i, j).
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
const std::string lambda_name =
"lambda[" + std::to_string(i) + "][" + std::to_string(j) + "]";
ret.lambda_[i][j] = prog->NewContinuousVariables(
2 * num_intervals_per_half_axis_ + 1, lambda_name);
// R(i, j) = φᵀ * λ[i][j]
prog->AddLinearEqualityConstraint(
R(i, j) -
phi_.dot(ret.lambda_[i][j].template cast<symbolic::Expression>()),
0);
switch (interval_binning_) {
case IntervalBinning::kLogarithmic: {
ret.B_[i][j] = AddLogarithmicSos2Constraint(
prog, ret.lambda_[i][j].cast<symbolic::Expression>());
break;
}
case IntervalBinning::kLinear: {
ret.B_[i][j] = prog->NewBinaryVariables(
2 * num_intervals_per_half_axis_,
"B[" + std::to_string(i) + "][" + std::to_string(j) + "]");
AddSos2Constraint(prog,
ret.lambda_[i][j].cast<symbolic::Expression>(),
ret.B_[i][j].cast<symbolic::Expression>());
break;
}
}
}
}
// Add some cutting planes on the binary variables, by inferring the sign
// of R(i, j) from binary variables B[i][j].
AddConstraintInferredFromTheSign(prog, ret.B_, num_intervals_per_half_axis_,
interval_binning_);
// Add mixed-integer constraint by replacing the bilinear product with another
// term in the McCormick envelope of w = x * y
if (approach_ == Approach::kBilinearMcCormick ||
approach_ == Approach::kBoth) {
AddRotationMatrixBilinearMcCormickConstraints(prog, R, ret.B_, ret.lambda_,
phi_, interval_binning_);
}
// Add mixed-integer constraint by considering the intersection between the
// sphere surface and boxes.
if (approach_ == Approach::kBoxSphereIntersection ||
approach_ == Approach::kBoth) {
// CRpos[k](i, j) = 1 => φ₊(k) ≤ R(i, j) ≤ φ₊(k+1)
// CRneg[k](i, j) = 1 => -φ₊(k+1) ≤ R(i, j) ≤ -φ₊(k)
std::vector<Matrix3<Expression>> CRpos(num_intervals_per_half_axis_);
std::vector<Matrix3<Expression>> CRneg(num_intervals_per_half_axis_);
if (interval_binning_ == IntervalBinning::kLinear) {
// CRpos[k](i, j) = 1 => φ₊(k) ≤ R(i, j) ≤ φ₊(k+1) => B[i][j](N+k) = 1
// CRneg[k](i, j) = 1 => -φ₊(k+1) ≤ R(i, j) ≤ -φ₊(k) => B[i][j](N-k-1)=1
// where N = num_intervals_per_half_axis_.
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
for (int k = 0; k < num_intervals_per_half_axis_; ++k) {
CRpos[k](i, j) = +ret.B_[i][j](num_intervals_per_half_axis_ + k);
CRneg[k](i, j) =
+ret.B_[i][j](num_intervals_per_half_axis_ - k - 1);
}
}
}
} else if (interval_binning_ == IntervalBinning::kLogarithmic) {
GetCRposAndCRnegForLogarithmicBinning(
ret.B_, num_intervals_per_half_axis_, prog, &CRpos, &CRneg);
}
AddBoxSphereIntersectionConstraintsForR(
R, CRpos, CRneg, num_intervals_per_half_axis_,
box_sphere_intersection_vertices_, box_sphere_intersection_halfspace_,
prog);
}
return ret;
}
AddRotationMatrixBoxSphereIntersectionReturn
AddRotationMatrixBoxSphereIntersectionMilpConstraints(
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& R,
int num_intervals_per_half_axis, MathematicalProgram* prog) {
DRAKE_DEMAND(num_intervals_per_half_axis >= 1);
AddRotationMatrixBoxSphereIntersectionReturn ret;
// Use a simple lambda to make the constraints more readable below.
// Note that
// forall k>=0, 0<=phi(k), and
// forall k<=num_intervals_per_half_axis, phi(k)<=1.
const Eigen::VectorXd phi_nonnegative =
Eigen::VectorXd::LinSpaced(num_intervals_per_half_axis + 1, 0, 1);
// Creates binary decision variables which discretize each axis.
// BRpos[k](i,j) = 1 => R(i,j) >= phi(k)
// BRneg[k](i,j) = 1 => R(i,j) <= -phi(k)
for (int k = 0; k < num_intervals_per_half_axis; k++) {
ret.BRpos.push_back(
prog->NewBinaryVariables<3, 3>("BRpos" + std::to_string(k)));
ret.BRneg.push_back(
prog->NewBinaryVariables<3, 3>("BRneg" + std::to_string(k)));
}
// For convenience, we also introduce additional expressions to
// represent the individual sections of the real line
// CRpos[k](i, j) = 1 => phi(k) <= R(i, j) <= phi(k + 1)
// CRpos[k](i,j) = BRpos[k](i,j) if k=N-1, otherwise
// CRpos[k](i,j) = BRpos[k](i,j) - BRpos[k+1](i,j)
// Similarly CRneg[k](i, j) = 1 => -phi(k + 1) <= R(i, j) <= -phi(k)
// CRneg[k](i, j) = CRneg[k](i, j) if k = N-1, otherwise
// CRneg[k](i, j) = CRneg[k](i, j) - CRneg[k+1](i, j)
ret.CRpos.reserve(num_intervals_per_half_axis);
ret.CRneg.reserve(num_intervals_per_half_axis);
for (int k = 0; k < num_intervals_per_half_axis - 1; k++) {
ret.CRpos.push_back(ret.BRpos[k] - ret.BRpos[k + 1]);
ret.CRneg.push_back(ret.BRneg[k] - ret.BRneg[k + 1]);
}
ret.CRpos.push_back(
ret.BRpos[num_intervals_per_half_axis - 1].cast<symbolic::Expression>());
ret.CRneg.push_back(
ret.BRneg[num_intervals_per_half_axis - 1].cast<symbolic::Expression>());
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < num_intervals_per_half_axis; k++) {
// R(i,j) > phi(k) => BRpos[k](i,j) = 1
// R(i,j) < phi(k) => BRpos[k](i,j) = 0
// R(i,j) = phi(k) => BRpos[k](i,j) = 0 or 1
// Since -s1 <= R(i, j) - phi(k) <= s2,
// where s1 = 1 + phi(k), s2 = 1 - phi(k). The point
// [R(i,j) - phi(k), BRpos[k](i,j)] has to lie within the convex hull,
// whose vertices are (-s1, 0), (0, 0), (s2, 1), (0, 1). By computing
// the edges of this convex hull, we get
// -s1 + s1*BRpos[k](i,j) <= R(i,j)-phi(k) <= s2 * BRpos[k](i,j)
double s1 = 1 + phi_nonnegative(k);
double s2 = 1 - phi_nonnegative(k);
prog->AddLinearConstraint(R(i, j) - phi_nonnegative(k) >=
-s1 + s1 * ret.BRpos[k](i, j));
prog->AddLinearConstraint(R(i, j) - phi_nonnegative(k) <=
s2 * ret.BRpos[k](i, j));
// -R(i,j) > phi(k) => BRneg[k](i,j) = 1
// -R(i,j) < phi(k) => BRneg[k](i,j) = 0
// -R(i,j) = phi(k) => BRneg[k](i,j) = 0 or 1
// Since -s2 <= R(i, j) + phi(k) <= s1,
// where s1 = 1 + phi(k), s2 = 1 - phi(k). The point
// [R(i,j) + phi(k), BRneg[k](i,j)] has to lie within the convex hull
// whose vertices are (-s2, 1), (0, 0), (s1, 0), (0, 1). By computing
// the edges of the convex hull, we get
// -s2 * BRneg[k](i,j) <= R(i,j)+phi(k) <= s1-s1*BRneg[k](i,j)
prog->AddLinearConstraint(R(i, j) + phi_nonnegative(k) <=
s1 - s1 * ret.BRneg[k](i, j));
prog->AddLinearConstraint(R(i, j) + phi_nonnegative(k) >=
-s2 * ret.BRneg[k](i, j));
}
// R(i,j) has to pick a side, either non-positive or non-negative.
prog->AddLinearConstraint(ret.BRpos[0](i, j) + ret.BRneg[0](i, j) == 1);
// for debugging: constrain to positive orthant.
// prog->AddBoundingBoxConstraint(1,1,{BRpos[0].block<1,1>(i,j)});
}
}
// Add constraint that no two rows (or two columns) can lie in the same
// orthant (or opposite orthant).
AddNotInSameOrOppositeOrthantConstraint(prog, ret.BRpos[0]);
AddNotInSameOrOppositeOrthantConstraint(prog, ret.BRpos[0].transpose());
std::vector<std::vector<std::vector<std::vector<Eigen::Vector3d>>>>
box_sphere_intersection_vertices;
std::vector<std::vector<std::vector<std::pair<Eigen::Vector3d, double>>>>
box_sphere_intersection_halfspace;
ComputeBoxSphereIntersectionAndHalfSpace(
num_intervals_per_half_axis, phi_nonnegative,
&box_sphere_intersection_vertices, &box_sphere_intersection_halfspace);
AddBoxSphereIntersectionConstraintsForR(
R, ret.CRpos, ret.CRneg, num_intervals_per_half_axis,
box_sphere_intersection_vertices, box_sphere_intersection_halfspace,
prog);
AddCrossProductImpliedOrthantConstraint(prog, ret.BRpos[0]);
AddCrossProductImpliedOrthantConstraint(prog, ret.BRpos[0].transpose());
return ret;
}
} // namespace solvers
} // namespace drake