Drake
drake::solvers Namespace Reference

Namespaces

 detail
 
 internal
 
 test
 

Classes

struct  AddRotationMatrixBilinearMcCormickMilpConstraintsReturn
 The return types from AddRotationMatrixBilinearMcComickMilpConstraints. More...
 
class  Binding
 A binding on constraint type C is a mapping of the decision variables onto the inputs of C. More...
 
class  BoundingBoxConstraint
 Implements a constraint of the form lb <= x <= ub . More...
 
class  Constraint
 A constraint is a function + lower and upper bounds. More...
 
class  Cost
 Provides an abstract base for all costs. More...
 
class  DrealSolver
 
class  EqualityConstrainedQPSolver
 
class  EvaluatorBase
 Provides an abstract interface to represent an expression, mapping a fixed or dynamic number of inputs to a fixed number of outputs, that may be evaluated on a scalar type of double or AutoDiffXd. More...
 
class  EvaluatorConstraint
 A constraint that may be specified using another (potentially nonlinear) evaluator. More...
 
class  EvaluatorCost
 A cost that may be specified using another (potentially nonlinear) evaluator. More...
 
class  FunctionEvaluator
 An evaluator that may be specified using a callable object. More...
 
class  GurobiSolver
 
class  IpoptSolver
 
class  LinearComplementarityConstraint
 Implements a constraint of the form: More...
 
class  LinearConstraint
 Implements a constraint of the form lb <= Ax <= ub . More...
 
class  LinearCost
 Implements a cost of the form a'x + b . More...
 
class  LinearEqualityConstraint
 Implements a constraint of the form Ax = b . More...
 
class  LinearMatrixInequalityConstraint
 Impose the matrix inequality constraint on variable x

\[ F_0 + x_1 F_1 + ... + x_n F_n \text{ is p.s.d} \]

where p.s.d stands for positive semidefinite. More...

 
class  LinearSystemSolver
 
struct  LogarithmicSos2NewBinaryVariables
 The size of the new binary variables in the compile time, for Special Ordered Set of type 2 (SOS2) constraint. More...
 
struct  LogarithmicSos2NewBinaryVariables< Eigen::Dynamic >
 
class  LorentzConeConstraint
 Constraining the linear expression \( z=Ax+b \) lies within the Lorentz cone. More...
 
class  MathematicalProgram
 
class  MathematicalProgramSolverInterface
 Interface used by implementations of individual solvers. More...
 
class  MobyLCPSolver
 A class for solving Linear Complementarity Problems (LCPs). More...
 
class  MobyLcpSolverId
 Non-template class for MobyLcpSolver<T> constants. More...
 
class  MosekSolver
 
struct  NewSymmetricVariableNames
 
struct  NewVariableNames
 
struct  NewVariableNames< Eigen::Dynamic >
 
struct  NewVariableNames< Rows, Cols >
 
struct  NewVariableNames< Size >
 The type of the names for the newly added variables. More...
 
class  NloptSolver
 
class  PolynomialConstraint
 lb[i] <= Pi <= ub[i], where each P[i] is a multivariate polynomial in x, y... More...
 
class  PolynomialCost
 Implements a cost of the form P(x, y...) where P is a multivariate polynomial in x, y, ... More...
 
class  PolynomialEvaluator
 Implements an evaluator of the form P(x, y...) where P is a multivariate polynomial in x, y, ... More...
 
class  PositiveSemidefiniteConstraint
 Implements a positive semidefinite constraint on a symmetric matrix S

\[\text{ S is p.s.d }\]

namely, all eigen values of S are non-negative. More...

 
class  QuadraticConstraint
 lb <= .5 x'Qx + b'x <= ub Without loss of generality, the class stores a symmetric matrix Q. More...
 
class  QuadraticCost
 Implements a cost of the form .5 x'Qx + b'x + c . More...
 
class  RotatedLorentzConeConstraint
 Constraining that the linear expression \( z=Ax+b \) lies within rotated Lorentz cone. More...
 
class  SnoptSolver
 
class  SolverId
 Identifies a MathematicalProgramSolverInterface implementation. More...
 
class  SolverTypeConverter
 Converts between SolverType and SolverId. More...
 
class  SystemIdentification
 Utility functions for system identification. More...
 

Typedefs

using MapVarToIndex = unordered_map< Variable::Id, int >
 
template<int rows, int cols>
using MatrixDecisionVariable = Eigen::Matrix< symbolic::Variable, rows, cols >
 
template<int rows>
using VectorDecisionVariable = MatrixDecisionVariable< rows, 1 >
 
using MatrixXDecisionVariable = MatrixDecisionVariable< Eigen::Dynamic, Eigen::Dynamic >
 
using VectorXDecisionVariable = VectorDecisionVariable< Eigen::Dynamic >
 
using VariableRefList = std::list< Eigen::Ref< const VectorXDecisionVariable >>
 
template<int rows, int cols>
using MatrixIndeterminate = Eigen::Matrix< symbolic::Variable, rows, cols >
 MatrixIndeterminate<int, int> is used as an alias for Eigen::Matrix<symbolic::Variable, int, int>. More...
 
template<int rows>
using VectorIndeterminate = MatrixIndeterminate< rows, 1 >
 VectorIndeterminate<int> is used as an alias for Eigen::Matrix<symbolic::Variable, int, 1>. More...
 
using MatrixXIndeterminate = MatrixIndeterminate< Eigen::Dynamic, Eigen::Dynamic >
 MatrixXIndeterminate is used as an alias for Eigen::Matrix<symbolic::Variable, Eigen::Dynamic, Eigen::Dynamic>. More...
 
using VectorXIndeterminate = VectorIndeterminate< Eigen::Dynamic >
 VectorXIndeterminate is used as an alias for Eigen::Matrix<symbolic::Variable, Eigen::Dynamic, 1>. More...
 
using IndeterminatesRefList = std::list< Eigen::Ref< const VectorXIndeterminate >>
 
typedef uint32_t AttributesSet
 
typedef uint32_t RollPitchYawLimits
 
using AddRotationMatrixMcCormickEnvelopeReturnType = std::tuple< std::vector< Matrix3< symbolic::Expression >>, std::vector< Matrix3< symbolic::Expression >>, std::vector< MatrixDecisionVariable< 3, 3 >>, std::vector< MatrixDecisionVariable< 3, 3 >>>
 

Enumerations

enum  { INITIAL_VARIABLE_ALLOCATION_NUM = 100 }
 
enum  ProgramAttributes {
  kNoCapabilities = 0, kError = 1 << 0, kGenericCost = 1 << 1, kGenericConstraint = 1 << 2,
  kQuadraticCost = 1 << 3, kQuadraticConstraint = 1 << 4, kLinearCost = 1 << 5, kLinearConstraint = 1 << 6,
  kLinearEqualityConstraint = 1 << 7, kLinearComplementarityConstraint = 1 << 8, kLorentzConeConstraint = 1 << 9, kRotatedLorentzConeConstraint = 1 << 10,
  kPositiveSemidefiniteConstraint = 1 << 11, kBinaryVariable = 1 << 12
}
 
enum  SolutionResult {
  kSolutionFound = 0, kInvalidInput = -1, kInfeasibleConstraints = -2, kUnbounded = -3,
  kUnknownError = -4, kInfeasible_Or_Unbounded = -5, kIterationLimit = -6
}
 
enum  IntervalBinning { kLogarithmic, kLinear }
 For a continuous variable whose range is cut into small intervals, we will use binary variables to represent which interval the continuous variable is in. More...
 
enum  RollPitchYawLimitOptions {
  kNoLimits = 0, kRPYError = 1 << 0, kRoll_NegPI_2_to_PI_2 = 1 << 1, kRoll_0_to_PI = 1 << 2,
  kPitch_NegPI_2_to_PI_2 = 1 << 3, kPitch_0_to_PI = 1 << 4, kYaw_NegPI_2_to_PI_2 = 1 << 5, kYaw_0_to_PI = 1 << 6,
  kRoll_0_to_PI_2 = (1 << 1) | (1 << 2), kPitch_0_to_PI_2 = (1 << 3) | (1 << 4), kYaw_0_to_PI_2 = (1 << 5) | (1 << 6)
}
 
enum  SolverType {
  kDReal, kEqualityConstrainedQP, kGurobi, kIpopt,
  kLinearSystem, kMobyLCP, kMosek, kNlopt,
  kSnopt
}
 

Functions

MapVarToIndex ConstructVarToIndexMap (const Eigen::Ref< const VectorXDecisionVariable > &x)
 
Expression ReplaceBilinearTerms (const Expression &e, const Eigen::Ref< const VectorXDecisionVariable > &x, const Eigen::Ref< const VectorXDecisionVariable > &y, const Eigen::Ref< const MatrixXDecisionVariable > &W)
 
symbolic::Expression ReplaceBilinearTerms (const symbolic::Expression &e, const Eigen::Ref< const VectorXDecisionVariable > &x, const Eigen::Ref< const VectorXDecisionVariable > &y, const Eigen::Ref< const MatrixXDecisionVariable > &W)
 Replaces all the bilinear product terms in the expression e, with the corresponding terms in W, where W represents the matrix x * yᵀ, such that after replacement, e does not have bilinear terms involving x and y. More...
 
shared_ptr< QuadraticCostMakeQuadraticErrorCost (const Eigen::Ref< const MatrixXd > &Q, const Eigen::Ref< const VectorXd > &x_desired)
 
shared_ptr< QuadraticCostMakeL2NormCost (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b)
 Creates a cost term of the form | Ax - b |^2. More...
 
std::shared_ptr< QuadraticCostMakeQuadraticErrorCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &x_desired)
 Creates a cost term of the form (x-x_desired)'Q(x-x_desired). More...
 
template<typename FF >
std::shared_ptr< CostMakeFunctionCost (FF &&f)
 Converts an input of type F to a nonlinear cost. More...
 
VectorXDecisionVariable ConcatenateVariableRefList (const VariableRefList &var_list)
 Concatenates each element in var_list into a single Eigen vector of decision variables, returns this concatenated vector. More...
 
SolutionResult SolveUnconstrainedQP (const Eigen::Ref< const Eigen::MatrixXd > &G, const Eigen::Ref< const Eigen::VectorXd > &c, double feasibility_tol, Eigen::VectorXd *x)
 Solves the un-constrained QP problem min 0.5 * xᵀ * G * x + cᵀ * x. More...
 
VectorXIndeterminate ConcatenateIndeterminatesRefList (const IndeterminatesRefList &var_list)
 Concatenates each element in var_list into a single Eigen vector of indeterminates, returns this concatenated vector. More...
 
Binding< LinearConstraintCreateLogicalAndConstraint (const symbolic::Expression &b1, const symbolic::Expression &b2, const symbolic::Expression &b1_and_b2)
 Adds linear constraints, such that when b1, b2, b1_and_b2 satisfy the constraints, and b1, b2 take binary values, it is guaranteed that b1_and_b2 = b1 ∧ b2 (b1 and b2). More...
 
Binding< LinearConstraintCreateLogicalOrConstraint (const symbolic::Expression &b1, const symbolic::Expression &b2, const symbolic::Expression &b1_or_b2)
 Adds linear constraints, such that when b1, b2, b1_or_b2 satisfy the constraints, and b1, b2 take binary values, it is guaranteed that b1_or_b2 = b1 ∨ b2 (b1 or b2). More...
 
Binding< LinearConstraintCreateLogicalXorConstraint (const symbolic::Expression &b1, const symbolic::Expression &b2, const symbolic::Expression &b1_xor_b2)
 Add linear constraints, such that when b1, b2, b1_xor_b2 satisfy the constraints, and b1, b2 take binary values, it is guaranteed that b1_xor_b2 = b1 ⊕ b2 (b1 exclusive xor b2). More...
 
void AddLogarithmicSos2Constraint (MathematicalProgram *prog, const Eigen::Ref< const VectorX< symbolic::Expression >> &lambda, const Eigen::Ref< const VectorX< symbolic::Expression >> &y)
 Adds the special ordered set 2 (SOS2) constraint,. More...
 
void AddSos2Constraint (MathematicalProgram *prog, const Eigen::Ref< const VectorX< symbolic::Expression >> &lambda, const Eigen::Ref< const VectorX< symbolic::Expression >> &y)
 Adds the special ordered set 2 (SOS2) constraint. More...
 
void AddLogarithmicSos1Constraint (MathematicalProgram *prog, const Eigen::Ref< const VectorX< symbolic::Expression >> &lambda, const Eigen::Ref< const VectorXDecisionVariable > &y, const Eigen::Ref< const Eigen::MatrixXi > &codes)
 Adds the special ordered set of type 1 (SOS1) constraint. More...
 
constexpr int CeilLog2 (int n)
 Return ⌈log₂(n)⌉, namely the minimal integer no smaller than log₂(n), with base 2. More...
 
template<typename Derived >
std::enable_if< std::is_base_of< Eigen::MatrixBase< Derived >, Derived >::value &&std::is_same< typename Derived::Scalar, symbolic::Expression >::value &&Derived::ColsAtCompileTime==1, typename LogarithmicSos2NewBinaryVariables< Derived::RowsAtCompileTime >::type >::type AddLogarithmicSos2Constraint (MathematicalProgram *prog, const Derived &lambda, const std::string &binary_variable_name="y")
 Adds the special ordered set 2 (SOS2) constraint,. More...
 
template<typename DerivedPhiX , typename DerivedPhiY , typename DerivedBx , typename DerivedBy >
std::enable_if< is_eigen_vector_of< DerivedPhiX, double >::value &&is_eigen_vector_of< DerivedPhiY, double >::value &&is_eigen_vector_of< DerivedBx, symbolic::Expression >::value &&is_eigen_vector_of< DerivedBy, symbolic::Expression >::value, MatrixDecisionVariable< DerivedPhiX::RowsAtCompileTime, DerivedPhiY::RowsAtCompileTime > >::type AddBilinearProductMcCormickEnvelopeSos2 (MathematicalProgram *prog, const symbolic::Variable &x, const symbolic::Variable &y, const symbolic::Expression &w, const DerivedPhiX &phi_x, const DerivedPhiY &phi_y, const DerivedBx &Bx, const DerivedBy &By, IntervalBinning binning)
 Constrain w to approximate the bilinear product x * y. More...
 
MatrixDecisionVariable< 3, 3 > NewRotationMatrixVars (MathematicalProgram *prog, const std::string &name="R")
 Allocates a 3x3 matrix of decision variables with the trivial bounding box constraint ensuring all elements are [-1,1], and the linear constraint imposing -1 <= trace(R) <= 3. More...
 
void AddBoundingBoxConstraintsImpliedByRollPitchYawLimits (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, RollPitchYawLimits limits=kNoLimits)
 Applies very conservative limits on the entries of R for the cases when rotations can be limited (for instance, if you want to search over rotations, but there is an obvious symmetry in the problem so that e.g. More...
 
void AddBoundingBoxConstraintsImpliedByRollPitchYawLimitsToBinary (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &B, RollPitchYawLimits limits)
 
void AddRotationMatrixSpectrahedralSdpConstraint (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R)
 Adds constraint (10) from https://arxiv.org/pdf/1403.4914.pdf , which exactly represents the convex hull of all rotation matrices in 3D. More...
 
void AddRotationMatrixOrthonormalSocpConstraint (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R)
 Adds a set of convex constraints which approximate the set of orthogonal matrices, O(3). More...
 
AddRotationMatrixMcCormickEnvelopeReturnType AddRotationMatrixMcCormickEnvelopeMilpConstraints (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, int num_binary_vars_per_half_axis=2, RollPitchYawLimits limits=kNoLimits)
 Adds binary variables that constrain the value of the column and row vectors of R, in order to add the following (in some cases non-convex) constraints as an MILP. More...
 
template<int kNumIntervalsPerHalfAxis>
std::enable_if< kNumIntervalsPerHalfAxis==Eigen::Dynamic||(kNumIntervalsPerHalfAxis >=1 &&kNumIntervalsPerHalfAxis<=4), typename AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< kNumIntervalsPerHalfAxis >::type >::type AddRotationMatrixBilinearMcCormickMilpConstraints (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, int num_intervals_per_half_axis)
 
template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< Eigen::Dynamic >::type AddRotationMatrixBilinearMcCormickMilpConstraints< Eigen::Dynamic > (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, int num_intervals_per_half_axis)
 
template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< 1 >::type AddRotationMatrixBilinearMcCormickMilpConstraints< 1 > (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, int num_intervals_per_half_axis)
 
template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< 2 >::type AddRotationMatrixBilinearMcCormickMilpConstraints< 2 > (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, int num_intervals_per_half_axis)
 
template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< 3 >::type AddRotationMatrixBilinearMcCormickMilpConstraints< 3 > (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, int num_intervals_per_half_axis)
 
template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< 4 >::type AddRotationMatrixBilinearMcCormickMilpConstraints< 4 > (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, int num_intervals_per_half_axis)
 
bool operator== (const SolverId &a, const SolverId &b)
 
bool operator!= (const SolverId &a, const SolverId &b)
 
std::ostream & operator<< (std::ostream &os, const SolverId &self)
 
 TEST_P (TestMinimumDistance, Test)
 
 TEST_P (TestMinimumDistanceWOrthonormalSocp, Test)
 
 INSTANTIATE_TEST_CASE_P (RotationTest, TestMinimumDistance,::testing::ValuesIn< std::vector< int >>({1, 2, 3}))
 
 INSTANTIATE_TEST_CASE_P (RotationTest, TestMinimumDistanceWOrthonormalSocp,::testing::ValuesIn< std::vector< int >>({1, 2, 3}))
 
void DrawSphere (const Eigen::RowVector3d &color=Eigen::RowVector3d(0, 0.5,0.5))
 Draw a unit sphere. More...
 
void DrawBox (const Eigen::Vector3d &bmin, const Eigen::Vector3d &bmax, const Eigen::RowVector3d &color=Eigen::RowVector3d(0.5, 0.2,0.3))
 Draw the box bmin <= x <= bmax, where the inequality is elementwise. More...
 
void DrawBoxSphereIntersection (const Eigen::Vector3d &bmin, const Eigen::Vector3d &bmax, const Eigen::RowVector3d &color=Eigen::RowVector3d(1, 0, 0))
 Draw the boundary of the intersection region, between the box bmin <= x <= bmax, and the unit sphere. More...
 
MatrixXDecisionVariable AddRotationMatrixSpectrahedralSdpRelaxation (MathematicalProgram *prog, const std::string &name="R")
 Add constraint (10) from https://arxiv.org/pdf/1403.4914.pdf , which exactly represents the convex hull of all rotation matrices in 3D. More...
 

Variables

template<int kNumIntervalsPerHalfAxis = Eigen::Dynamic>
std::enable_if< kNumIntervalsPerHalfAxis==Eigen::Dynamic||(kNumIntervalsPerHalfAxis >=1 &&kNumIntervalsPerHalfAxis<=4), typename AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< kNumIntervalsPerHalfAxis >::type >::type AddRotationMatrixBilinearMcCormickMilpConstraints (MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, int num_intervals_per_half_axis=kNumIntervalsPerHalfAxis)
 Relax the SO(3) constraint on a rotation matrix R = [R₀ R₁ R₂]. More...
 

Typedef Documentation

typedef uint32_t AttributesSet
using IndeterminatesRefList = std::list<Eigen::Ref<const VectorXIndeterminate>>
using MapVarToIndex = unordered_map<Variable::Id, int>
using MatrixDecisionVariable = Eigen::Matrix<symbolic::Variable, rows, cols>
using MatrixIndeterminate = Eigen::Matrix<symbolic::Variable, rows, cols>

MatrixIndeterminate<int, int> is used as an alias for Eigen::Matrix<symbolic::Variable, int, int>.

After resolving aliases, a compiler does not distinguish between these two. All indeterminates are a variable of type symbolic::Variable::Type::CONTINUOUS (by default).

Template Parameters
rowsThe number of rows in the new matrix containing indeterminates.
colsThe number of columns in the new matrix containing indeterminates.
using MatrixXDecisionVariable = MatrixDecisionVariable<Eigen::Dynamic, Eigen::Dynamic>
using MatrixXIndeterminate = MatrixIndeterminate<Eigen::Dynamic, Eigen::Dynamic>

MatrixXIndeterminate is used as an alias for Eigen::Matrix<symbolic::Variable, Eigen::Dynamic, Eigen::Dynamic>.

See also
MatrixIndeterminate<int, int>
typedef uint32_t RollPitchYawLimits
using VariableRefList = std::list<Eigen::Ref<const VectorXDecisionVariable>>

VectorIndeterminate<int> is used as an alias for Eigen::Matrix<symbolic::Variable, int, 1>.

Template Parameters
rowsThe number of rows in the new matrix containing indeterminates.
See also
MatrixIndeterminate<int, int>
using VectorXIndeterminate = VectorIndeterminate<Eigen::Dynamic>

VectorXIndeterminate is used as an alias for Eigen::Matrix<symbolic::Variable, Eigen::Dynamic, 1>.

See also
MatrixIndeterminate<int, int>

Enumeration Type Documentation

anonymous enum
Enumerator
INITIAL_VARIABLE_ALLOCATION_NUM 
enum IntervalBinning
strong

For a continuous variable whose range is cut into small intervals, we will use binary variables to represent which interval the continuous variable is in.

We support two representations, either using logarithmic number of binary variables, or linear number of binary variables. For more details,

See also
AddLogarithmicSos2Constraint and AddSos2Constraint
Enumerator
kLogarithmic 
kLinear 
Enumerator
kNoCapabilities 
kError 

Do not use, to avoid & vs. && typos.

kGenericCost 
kGenericConstraint 
kQuadraticCost 
kQuadraticConstraint 
kLinearCost 
kLinearConstraint 
kLinearEqualityConstraint 
kLinearComplementarityConstraint 
kLorentzConeConstraint 
kRotatedLorentzConeConstraint 
kPositiveSemidefiniteConstraint 
kBinaryVariable 
Enumerator
kNoLimits 
kRPYError 

Do not use, to avoid & vs. && typos.

kRoll_NegPI_2_to_PI_2 
kRoll_0_to_PI 
kPitch_NegPI_2_to_PI_2 
kPitch_0_to_PI 
kYaw_NegPI_2_to_PI_2 
kYaw_0_to_PI 
kRoll_0_to_PI_2 
kPitch_0_to_PI_2 
kYaw_0_to_PI_2 
Enumerator
kSolutionFound 
kInvalidInput 
kInfeasibleConstraints 
kUnbounded 
kUnknownError 
kInfeasible_Or_Unbounded 
kIterationLimit 
enum SolverType
strong
Enumerator
kDReal 
kEqualityConstrainedQP 
kGurobi 
kIpopt 
kLinearSystem 
kMobyLCP 
kMosek 
kNlopt 
kSnopt 

Function Documentation

std::enable_if< is_eigen_vector_of<DerivedPhiX, double>::value && is_eigen_vector_of<DerivedPhiY, double>::value && is_eigen_vector_of<DerivedBx, symbolic::Expression>::value && is_eigen_vector_of<DerivedBy, symbolic::Expression>::value, MatrixDecisionVariable<DerivedPhiX::RowsAtCompileTime, DerivedPhiY::RowsAtCompileTime> >::type drake::solvers::AddBilinearProductMcCormickEnvelopeSos2 ( MathematicalProgram prog,
const symbolic::Variable x,
const symbolic::Variable y,
const symbolic::Expression w,
const DerivedPhiX &  phi_x,
const DerivedPhiY &  phi_y,
const DerivedBx &  Bx,
const DerivedBy &  By,
IntervalBinning  binning 
)

Constrain w to approximate the bilinear product x * y.

We know that x is in one of the intervals [φx(i), φx(i+1)], y is in one of the intervals [φy(j), φy(j+1)]. The variable w is constrained to be in the convex hull of x * y for x in [φx(i), φx(i+1)], y in [φy(j), φy(j+1)], namely (x, y, w) is in the tetrahedron, with vertices [φx(i), φy(j), φx(i)*φy(j)], [φx(i+1), φy(j), φx(i+1)*φy(j)], [φx(i), φy(j+1), φx(i)*φy(j+1)] and [φx(i+1), φy(j+1), φx(i+1)*φy(j+1)] We use two different encoding schemes on the binary variables, to determine which interval is active. We can choose either linear or logarithmic binning. When using linear binning, for a variable with N intervals, we use N binary variables, and B(i) = 1 indicates the variable is in the i'th interval. When using logarithmic binning, we use ⌈log₂(N)⌉ binary variables. If these binary variables represent integer M in the reflected Gray code, then the continuous variable is in the M'th interval.

Parameters
progThe program to which the bilinear product constraint is added
xThe decision variable.
yThe decision variable.
wThe expression to approximate x * y
phi_xThe end points of the intervals for x.
phi_yThe end points of the intervals for y.
BxThe binary variables for the interval in which x stays encoded as described above.
ByThe binary variables for the interval in which y stays encoded as described above.
binningDetermine whether to use linear binning or logarithmic binning.
Returns
lambda The auxiliary continuous variables. x = φxᵀ * (λ.rowwise().sum()) y = φyᵀ * (λ.cowwise().sum()) w = sum_{i, j} φx(i) * φy(j) * λ(i, j) Both λ.rowwise().sum() and λ.colwise().sum() satisfy SOS2 constraint. If x ∈ [φx(M), φx(M+1)] and y ∈ [φy(N), φy(N+1)], then only λ(M, N), λ(M + 1, N), λ(M, N + 1) and λ(M+1, N+1) can be strictly positive, all other λ(i, j) are zero.

Here is the call graph for this function:

Here is the caller graph for this function:

void AddBoundingBoxConstraintsImpliedByRollPitchYawLimits ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R,
RollPitchYawLimits  limits = kNoLimits 
)

Applies very conservative limits on the entries of R for the cases when rotations can be limited (for instance, if you want to search over rotations, but there is an obvious symmetry in the problem so that e.g.

0 < pitch < PI need not be considered). A matrix so constrained may still contain rotations outside of this envelope. Note: For simple rotational symmetry over PI, prefer kPitch_NegPI_2_to_PI_2 (over 0_to_PI) because it adds one more constraint (when combined with constraints on roll and yaw). Note: The Roll-Pitch-Yaw coordinates follow the convention in - representing extrinsic rotations about Space-fixed x-y-z axes, respectively.

Here is the call graph for this function:

void drake::solvers::AddBoundingBoxConstraintsImpliedByRollPitchYawLimitsToBinary ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  B,
RollPitchYawLimits  limits 
)

Here is the call graph for this function:

Here is the caller graph for this function:

void AddLogarithmicSos1Constraint ( MathematicalProgram prog,
const Eigen::Ref< const VectorX< symbolic::Expression >> &  lambda,
const Eigen::Ref< const VectorXDecisionVariable > &  y,
const Eigen::Ref< const Eigen::MatrixXi > &  codes 
)

Adds the special ordered set of type 1 (SOS1) constraint.

Namely

  λ(0) + ... + λ(n-1) = 1
  λ(i) ≥ 0 ∀i
  ∃ j ∈ {0, 1, ..., n-1}, s.t λ(j) = 1

one and only one of λ(i) is strictly positive (equals to 1 in this case). We will need to add ⌈log₂(n)⌉ binary variables, where n is the number of rows in λ. For more information, please refer to Modeling Disjunctive Constraints with a Logarithmic Number of Binary Variables and Constraints by J. Vielma and G. Nemhauser, 2011.

Parameters
progThe program to which the SOS1 constraint is added.
lambdalambda is in SOS1.
yThe binary variables indicating which λ is positive. For a given assignment on the binary variable y, if (y(0), ..., y(⌈log₂(n)⌉) represents integer M in codes, then only λ(M) is positive. Namely, if (y(0), ..., y(⌈log₂(n)⌉) equals to codes.row(M), then λ(M) = 1
codesA n x ⌈log₂(n)⌉ matrix. code.row(i) represents integer i. No two rows of codes can be the same.
Exceptions
std::runtime_errorif codes has a non-binary entry (0, 1).

Here is the call graph for this function:

Here is the caller graph for this function:

void AddLogarithmicSos2Constraint ( MathematicalProgram prog,
const Eigen::Ref< const VectorX< symbolic::Expression >> &  lambda,
const Eigen::Ref< const VectorX< symbolic::Expression >> &  y 
)

Adds the special ordered set 2 (SOS2) constraint,.

See also
AddLogarithmicSos2Constraint.

Here is the call graph for this function:

Here is the caller graph for this function:

std::enable_if< std::is_base_of<Eigen::MatrixBase<Derived>, Derived>::value && std::is_same<typename Derived::Scalar, symbolic::Expression>::value && Derived::ColsAtCompileTime == 1, typename LogarithmicSos2NewBinaryVariables< Derived::RowsAtCompileTime>::type>::type drake::solvers::AddLogarithmicSos2Constraint ( MathematicalProgram prog,
const Derived &  lambda,
const std::string &  binary_variable_name = "y" 
)

Adds the special ordered set 2 (SOS2) constraint,.

  λ(0) + ... + λ(n) = 1
  ∀i. λ(i) ≥ 0
  ∃ j ∈ {0, 1, ..., n-1}, s.t λ(j) + λ(j + 1) = 1

Namely at most two entries in λ can be strictly positive, and these two entries have to be adjacent. All other λ should be zero. Moreover, the non-zero λ satisfies λ(j) + λ(j + 1) = 1. We will need to add ⌈log₂(n - 1)⌉ binary variables, where n is the number of rows in λ. For more information, please refer to Modeling Disjunctive Constraints with a Logarithmic Number of Binary Variables and Constraints by J. Vielma and G. Nemhauser, 2011.

Parameters
progAdd the SOS2 constraint to this mathematical program.
lambdaAt most two entries in λ can be strictly positive, and these two entries have to be adjacent. All other entries are zero.
Returns
y The newly added binary variables. The assignment of the binary variable y implies which two λ can be strictly positive. With a binary assignment on y, and suppose the integer M corresponds to (y(0), y(1), ..., y(⌈log₂(n - 1)⌉)) in Gray code, then only λ(M) and λ(M + 1) can be non-zero. For example, if the assignment of y = (1, 1), in Gray code, (1, 1) represents integer 2, so only λ(2) and λ(3) can be strictly positive.

Here is the call graph for this function:

std::enable_if< kNumIntervalsPerHalfAxis == Eigen::Dynamic || (kNumIntervalsPerHalfAxis >= 1 && kNumIntervalsPerHalfAxis <= 4), typename AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< kNumIntervalsPerHalfAxis>::type>::type drake::solvers::AddRotationMatrixBilinearMcCormickMilpConstraints ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R,
int  num_intervals_per_half_axis 
)

Here is the call graph for this function:

template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< 1>::type drake::solvers::AddRotationMatrixBilinearMcCormickMilpConstraints< 1 > ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R,
int  num_intervals_per_half_axis 
)

Here is the caller graph for this function:

template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< 2>::type drake::solvers::AddRotationMatrixBilinearMcCormickMilpConstraints< 2 > ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R,
int  num_intervals_per_half_axis 
)

Here is the caller graph for this function:

template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< 3>::type drake::solvers::AddRotationMatrixBilinearMcCormickMilpConstraints< 3 > ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R,
int  num_intervals_per_half_axis 
)

Here is the caller graph for this function:

template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn<4>::type drake::solvers::AddRotationMatrixBilinearMcCormickMilpConstraints< 4 > ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R,
int  num_intervals_per_half_axis 
)

Here is the caller graph for this function:

template AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< Eigen::Dynamic>::type drake::solvers::AddRotationMatrixBilinearMcCormickMilpConstraints< Eigen::Dynamic > ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R,
int  num_intervals_per_half_axis 
)
AddRotationMatrixMcCormickEnvelopeReturnType AddRotationMatrixMcCormickEnvelopeMilpConstraints ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R,
int  num_binary_vars_per_half_axis = 2,
RollPitchYawLimits  limits = kNoLimits 
)

Adds binary variables that constrain the value of the column and row vectors of R, in order to add the following (in some cases non-convex) constraints as an MILP.

Specifically, for column vectors Ri, we constrain:

  • forall i, |Ri| = 1 ± envelope,
  • forall i,j. i ≠ j, Ri.dot(Rj) = 0 ± envelope,
  • R2 = R0.cross(R1) ± envelope, and again for R0=R1.cross(R2), and R1=R2.cross(R0). Then all of the same constraints are also added to R^T. The size of the envelope decreases quickly as num_binary_variables_per_half_axis is is increased.

Note: Creates 9*2*num_binary_variables_per_half_axis binary variables named "BRpos*(*,*)" and "BRneg*(*,*)", and the same number of continuous variables named "CRpos*(*,*)" and "CRneg*(*,*)".

Note: The particular representation/algorithm here was developed in an attempt:

  • to enable efficient reuse of the variables between the constraints between multiple rows/columns (e.g. the constraints on R^T use the same variables as the constraints on R), and
  • to facilitate branch-and-bound solution techniques – binary regions are layered so that constraining one region establishes constraints on large portions of SO(3), and confers hopefully "useful" constraints the on other binary variables.
    Parameters
    progThe mathematical program to which the constraints are added.
    RThe rotation matrix
    num_binary_vars_per_half_axisnumber of binary variables for a half axis.
    limitsThe angle joints for space fixed z-y-x representation of the rotation.
    Default: is no constraint.
    See also
    RollPitchYawLimitOptions
    Return values
    NewVarsIncluded the newly added variables <CRpos, CRneg, BRpos, BRneg>. All new variables can only take values either 0 or 1. CRpos and CRneg are declared as continuous variables, while BRpos and BRneg are declared as binary variables. The definition for these variables are
      CRpos[k](i, j) = 1 => k / N <= R(i, j) <= (k+1) / N
      CRneg[k](i, j) = 1 => -(k+1) / N <= R(i, j) <= -k / N
      BRpos[k](i, j) = 1 => R(i, j) >= k / N
      BRneg[k](i, j) = 1 => R(i, j) <= -k / N
    
    where N is num_binary_vars_per_half_axis.

Here is the call graph for this function:

Here is the caller graph for this function:

void AddRotationMatrixOrthonormalSocpConstraint ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R 
)

Adds a set of convex constraints which approximate the set of orthogonal matrices, O(3).

Adds the bilinear constraints that the each column Ri has length <= 1 and that Ri'Rj approx 0 via -2 + |Ri|^2 + |Rj|^2 <= 2Ri'Rj <= 2 - |Ri|^2 - |Rj|^2 (for all i!=j), using a second-order-cone relaxation. Additionally, the same constraints are applied to all of the rows.

Here is the call graph for this function:

Here is the caller graph for this function:

void AddRotationMatrixSpectrahedralSdpConstraint ( MathematicalProgram prog,
const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &  R 
)

Adds constraint (10) from https://arxiv.org/pdf/1403.4914.pdf , which exactly represents the convex hull of all rotation matrices in 3D.

Here is the call graph for this function:

MatrixXDecisionVariable drake::solvers::AddRotationMatrixSpectrahedralSdpRelaxation ( MathematicalProgram prog,
const std::string &  name = "R" 
)

Add constraint (10) from https://arxiv.org/pdf/1403.4914.pdf , which exactly represents the convex hull of all rotation matrices in 3D.

Here is the call graph for this function:

Here is the caller graph for this function:

void AddSos2Constraint ( MathematicalProgram prog,
const Eigen::Ref< const VectorX< symbolic::Expression >> &  lambda,
const Eigen::Ref< const VectorX< symbolic::Expression >> &  y 
)

Adds the special ordered set 2 (SOS2) constraint.

y(i) takes binary values (either 0 or 1).

  y(i) = 1 => λ(i) + λ(i + 1) = 1.
See also
AddLogarithmicSos2Constraint for a complete explanation on SOS2 constraint.
Parameters
progThe optimization program to which the SOS2 constraint is added.
lambdaAt most two entries in λ can be strictly positive, and these two entries have to be adjacent. All other entries are zero. Moreover, these two entries should sum up to 1.
yy(i) takes binary value, and determines which two entries in λ can be strictly positive. Throw a runtime error if y.rows() != lambda.rows() - 1.

Here is the call graph for this function:

Here is the caller graph for this function:

constexpr int drake::solvers::CeilLog2 ( int  n)

Return ⌈log₂(n)⌉, namely the minimal integer no smaller than log₂(n), with base 2.

Parameters
nA positive integer.
Returns
The minimal integer no smaller than log₂(n).

Here is the caller graph for this function:

VectorXIndeterminate ConcatenateIndeterminatesRefList ( const IndeterminatesRefList var_list)

Concatenates each element in var_list into a single Eigen vector of indeterminates, returns this concatenated vector.

Here is the call graph for this function:

Here is the caller graph for this function:

VectorXDecisionVariable ConcatenateVariableRefList ( const VariableRefList var_list)

Concatenates each element in var_list into a single Eigen vector of decision variables, returns this concatenated vector.

Here is the caller graph for this function:

MapVarToIndex drake::solvers::ConstructVarToIndexMap ( const Eigen::Ref< const VectorXDecisionVariable > &  x)

Here is the caller graph for this function:

Binding< LinearConstraint > CreateLogicalAndConstraint ( const symbolic::Expression b1,
const symbolic::Expression b2,
const symbolic::Expression b1_and_b2 
)

Adds linear constraints, such that when b1, b2, b1_and_b2 satisfy the constraints, and b1, b2 take binary values, it is guaranteed that b1_and_b2 = b1 ∧ b2 (b1 and b2).

The constraints are

  b1_and_b2 >= b1 + b2 - 1
  b1_and_b2 <= b1
  b1_and_b2 <= b2
  0 <= b1_and_b2 <= 1
Parameters
b1An expression that should only take a binary value.
b2An expression that should only take a binary value.
b1_and_b2Should be the logical and between b1 and b2.
Returns
The newly added constraints, such that when b1, b2, b1_and_b2 satisfy the constraints, it is guaranteed that b1_and_b2 = b1 ∧ b2.
Precondition
b1, b2, b1_and_b2 are all linear expressions.

Here is the call graph for this function:

Binding< LinearConstraint > CreateLogicalOrConstraint ( const symbolic::Expression b1,
const symbolic::Expression b2,
const symbolic::Expression b1_or_b2 
)

Adds linear constraints, such that when b1, b2, b1_or_b2 satisfy the constraints, and b1, b2 take binary values, it is guaranteed that b1_or_b2 = b1 ∨ b2 (b1 or b2).

The constraints are

  b1_or_b2 <= b1 + b2
  b1_or_b2 >= b1
  b1_or_b2 >= b2
  0 <= b1_or_b2 <= 1
Parameters
b1An expression that should only take a binary value.
b2An expression that should only take a binary value.
b1_or_b2Should be the logical or between b1 and b2.
Returns
The newly added constraints, such that when b1, b2, b1_or_b2 satisfy the constraints, it is guaranteed that b1_or_b2 = b1 ∨ b2.
Precondition
b1, b2, b1_or_b2 are all linear expressions.

Here is the call graph for this function:

Binding< LinearConstraint > CreateLogicalXorConstraint ( const symbolic::Expression b1,
const symbolic::Expression b2,
const symbolic::Expression b1_xor_b2 
)

Add linear constraints, such that when b1, b2, b1_xor_b2 satisfy the constraints, and b1, b2 take binary values, it is guaranteed that b1_xor_b2 = b1 ⊕ b2 (b1 exclusive xor b2).

The constraints are

  b1_xor_b2 <= b1 + b2
  b1_xor_b2 >= b1 - b2
  b1_xor_b2 >= b2 - b1
  b1_xor_b2 <= 2 - b1 - b2
  0 <= b1_xor_b2 <= 1
Parameters
b1An expression that should only take a binary value.
b2An expression that should only take a binary value.
b1_xor_b2Should be the logical exclusive or between b1 and b2.
Returns
The newly added constraints, such that when b1, b2, b1_xor_b2 satisfy the constraints, it is guaranteed that b1_xor_b2 = b1 ⊕ b2.
Precondition
b1, b2, b1_xor_b2 are all linear expressions.

Here is the call graph for this function:

void DrawBox ( const Eigen::Vector3d &  bmin,
const Eigen::Vector3d &  bmax,
const Eigen::RowVector3d &  color = Eigen::RowVector3d(0.5, 0.2,0.3) 
)

Draw the box bmin <= x <= bmax, where the inequality is elementwise.

Parameters
bminThe innermost corner of the box
bmaxThe outermost corner of the box
colorThe rgb color of the surface to be plotted.
Default: is [0.5, 0.2, 0.3]
Precondition
bmin >= 0 and bmax >= 0. The inequality is elementwise.

Here is the call graph for this function:

void DrawBoxSphereIntersection ( const Eigen::Vector3d &  bmin,
const Eigen::Vector3d &  bmax,
const Eigen::RowVector3d &  color = Eigen::RowVector3d(1, 0, 0) 
)

Draw the boundary of the intersection region, between the box bmin <= x <= bmax, and the unit sphere.

Currently we only accept the box in the first orthant.

Parameters
bminThe innermost corner of the box.
bmaxThe outermost corner of the box.
colorThe rgb color of the boundary to be plotted.
Default: is red.

Here is the call graph for this function:

void DrawSphere ( const Eigen::RowVector3d &  color = Eigen::RowVector3d(0, 0.5,0.5))

Draw a unit sphere.

Parameters
colorThe rgb color of the surface to be plotted.
Default: is [0, 0.5, 0.5]

Here is the call graph for this function:

drake::solvers::INSTANTIATE_TEST_CASE_P ( RotationTest  ,
TestMinimumDistance  ,
::testing::ValuesIn< std::vector< int >>  {1, 2, 3} 
)

Here is the caller graph for this function:

drake::solvers::INSTANTIATE_TEST_CASE_P ( RotationTest  ,
TestMinimumDistanceWOrthonormalSocp  ,
::testing::ValuesIn< std::vector< int >>  {1, 2, 3} 
)
std::shared_ptr<Cost> drake::solvers::MakeFunctionCost ( FF &&  f)

Converts an input of type F to a nonlinear cost.

Template Parameters
FFThe forwarded function type (e.g., const F&,F&&, ...). The classF` should have functions numInputs(), numOutputs(), and eval(x, y).
See also
detail::FunctionTraits.

Here is the caller graph for this function:

std::shared_ptr< QuadraticCost > MakeL2NormCost ( const Eigen::Ref< const Eigen::MatrixXd > &  A,
const Eigen::Ref< const Eigen::VectorXd > &  b 
)

Creates a cost term of the form | Ax - b |^2.

Here is the call graph for this function:

Here is the caller graph for this function:

shared_ptr<QuadraticCost> drake::solvers::MakeQuadraticErrorCost ( const Eigen::Ref< const MatrixXd > &  Q,
const Eigen::Ref< const VectorXd > &  x_desired 
)

Here is the caller graph for this function:

std::shared_ptr<QuadraticCost> drake::solvers::MakeQuadraticErrorCost ( const Eigen::Ref< const Eigen::MatrixXd > &  Q,
const Eigen::Ref< const Eigen::VectorXd > &  x_desired 
)

Creates a cost term of the form (x-x_desired)'Q(x-x_desired).

MatrixDecisionVariable< 3, 3 > NewRotationMatrixVars ( MathematicalProgram prog,
const std::string &  name = "R" 
)

Allocates a 3x3 matrix of decision variables with the trivial bounding box constraint ensuring all elements are [-1,1], and the linear constraint imposing -1 <= trace(R) <= 3.

Here is the call graph for this function:

Here is the caller graph for this function:

bool operator!= ( const SolverId a,
const SolverId b 
)
std::ostream & operator<< ( std::ostream &  os,
const SolverId self 
)
bool operator== ( const SolverId a,
const SolverId b 
)
symbolic::Expression drake::solvers::ReplaceBilinearTerms ( const symbolic::Expression e,
const Eigen::Ref< const VectorXDecisionVariable > &  x,
const Eigen::Ref< const VectorXDecisionVariable > &  y,
const Eigen::Ref< const MatrixXDecisionVariable > &  W 
)

Replaces all the bilinear product terms in the expression e, with the corresponding terms in W, where W represents the matrix x * yᵀ, such that after replacement, e does not have bilinear terms involving x and y.

For example, if e = x(0)*y(0) + 2 * x(0)*y(1) + x(1) * y(1) + 3 * x(1), e has bilinear terms x(0)*y(0), x(0) * y(1) and x(2) * y(1), if we call ReplaceBilinearTerms(e, x, y, W) where W(i, j) represent the term x(i) * y(j), then this function returns W(0, 0) + 2 * W(0, 1) + W(1, 1) + 3 * x(1).

Parameters
eAn expression potentially contains bilinear products between x and y.
xThe bilinear product between x and y will be replaced by the corresponding term in W. Throws a runtime error ifx` contains duplicate entries.
yThe bilinear product between x and y will be replaced by the corresponding term in W.Throws a runtime error ify` contains duplicate entries.
WBilinear product term x(i) * y(j) will be replaced by W(i, j).
Returns
The symbolic expression after replacing x(i) * y(j) with W(i, j).
Expression drake::solvers::ReplaceBilinearTerms ( const Expression &  e,
const Eigen::Ref< const VectorXDecisionVariable > &  x,
const Eigen::Ref< const VectorXDecisionVariable > &  y,
const Eigen::Ref< const MatrixXDecisionVariable > &  W 
)

Here is the call graph for this function:

Here is the caller graph for this function:

SolutionResult drake::solvers::SolveUnconstrainedQP ( const Eigen::Ref< const Eigen::MatrixXd > &  G,
const Eigen::Ref< const Eigen::VectorXd > &  c,
double  feasibility_tol,
Eigen::VectorXd *  x 
)

Solves the un-constrained QP problem min 0.5 * xᵀ * G * x + cᵀ * x.

Here is the caller graph for this function:

drake::solvers::TEST_P ( TestMinimumDistance  ,
Test   
)
drake::solvers::TEST_P ( TestMinimumDistanceWOrthonormalSocp  ,
Test   
)

Here is the call graph for this function:

Variable Documentation

std::enable_if< kNumIntervalsPerHalfAxis == Eigen::Dynamic || (kNumIntervalsPerHalfAxis >= 1 && kNumIntervalsPerHalfAxis <= 4), typename AddRotationMatrixBilinearMcCormickMilpConstraintsReturn< kNumIntervalsPerHalfAxis>::type>::type AddRotationMatrixBilinearMcCormickMilpConstraints(MathematicalProgram *prog, const Eigen::Ref< const MatrixDecisionVariable< 3, 3 >> &R, int num_intervals_per_half_axis=kNumIntervalsPerHalfAxis)

Relax the SO(3) constraint on a rotation matrix R = [R₀ R₁ R₂].

Rᵢᵀ * Rᵢ = 1
Rᵢᵀ * Rⱼ = 0
Rᵢ x Rⱼ = Rₖ

to a set of mixed-integer linear constraints. This is achieved by replacing the bilinear product terms in the SO(3) constraint, with a new auxiliary variable in the McCormick envelope of bilinear product. For more details, please refer to Global Inverse Kinematics via Mixed-Integer Convex Optimization by Hongkai Dai, Gregory Izatt and Russ Tedrake, 2017.

Template Parameters
kNumIntervalsPerHalfAxisWe cut the interval [-1, 1] evenly into KNumIntervalsPerHalfAxis * 2 intervals. Then depending on in which interval R(i, j) is, we impose corresponding linear constraints. Currently only certain kNumIntervalsPerHalfAxis are accepted, including Eigen::Dynamic, and integer 1 to 4. If the user wants to support more kNumIntervalsPerHalfAxis, please instantiate them explicitly in rotation_constraint.cc.
Parameters
progThe optimization program to which the SO(3) relaxation is added.
RThe rotation matrix.
num_intervals_per_half_axisSame as NumIntervalsPerHalfAxis, use this variable when NumIntervalsPerHalfAxis is dynamic.
Returns
pair. pair = (B, φ). B[i][j] is a column vector. If B[i][j] represents integer M in the reflected Gray code, then R(i, j) is in the interval [φ(M), φ(M+1)]. φ contains the end points of the all the intervals, namely φ(i) = -1 + 1 / num_intervals_per_half_axis * i.