Drake
GlobalInverseKinematics Class Reference

Solves the inverse kinematics problem as a mixed integer convex optimization problem. More...

#include <drake/attic/multibody/global_inverse_kinematics.h>

Inheritance diagram for GlobalInverseKinematics:
[legend]
Collaboration diagram for GlobalInverseKinematics:
[legend]

struct  Options

## Public Member Functions

GlobalInverseKinematics (const RigidBodyTreed &robot, const Options &options=Options())
Parses the robot kinematics tree. More...

~GlobalInverseKinematics () override

const solvers::MatrixDecisionVariable< 3, 3 > & body_rotation_matrix (int body_index) const
Getter for the decision variables on the rotation matrix R_WB for a body with the specified index. More...

const solvers::VectorDecisionVariable< 3 > & body_position (int body_index) const
Getter for the decision variables on the position p_WBo of the body B's origin measured and expressed in the world frame. More...

Eigen::VectorXd ReconstructGeneralizedPositionSolution () const
After solving the inverse kinematics problem and finding out the pose of each body, reconstruct the robot generalized position (joint angles, etc) that matches with the body poses. More...

solvers::Binding< solvers::LinearConstraintAddWorldPositionConstraint (int body_idx, const Eigen::Vector3d &p_BQ, const Eigen::Vector3d &box_lb_F, const Eigen::Vector3d &box_ub_F, const Eigen::Isometry3d &X_WF=Eigen::Isometry3d::Identity())
Adds the constraint that the position of a point Q on a body B (whose index is body_idx), is within a box in a specified frame F. More...

solvers::Binding< solvers::LinearConstraintAddWorldOrientationConstraint (int body_idx, const Eigen::Quaterniond &desired_orientation, double angle_tol)
Add a constraint that the angle between the body orientation and the desired orientation should not be larger than angle_tol. More...

void AddPostureCost (const Eigen::Ref< const Eigen::VectorXd > &q_desired, const Eigen::Ref< const Eigen::VectorXd > &body_position_cost, const Eigen::Ref< const Eigen::VectorXd > &body_orientation_cost)
Penalizes the deviation to the desired posture. More...

solvers::VectorXDecisionVariable BodyPointInOneOfRegions (int body_index, const Eigen::Ref< const Eigen::Vector3d > &p_BQ, const std::vector< Eigen::Matrix3Xd > &region_vertices)
Constrain the point Q lying within one of the convex polytopes. More...

void AddJointLimitConstraint (int body_index, double joint_lower_bound, double joint_upper_bound, bool linear_constraint_approximation=false)
Adds joint limits on a specified joint. More...

Does not allow copy, move, or assignment
GlobalInverseKinematics (const GlobalInverseKinematics &)=delete

GlobalInverseKinematicsoperator= (const GlobalInverseKinematics &)=delete

GlobalInverseKinematics (GlobalInverseKinematics &&)=delete

GlobalInverseKinematicsoperator= (GlobalInverseKinematics &&)=delete

Public Member Functions inherited from MathematicalProgram
MathematicalProgram ()

virtual ~MathematicalProgram ()

std::unique_ptr< MathematicalProgramClone () const
Clones an optimization program. More...

VectorXDecisionVariable NewContinuousVariables (int rows, const std::string &name="x")
Adds continuous variables, appending them to an internal vector of any existing vars. More...

template<int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
MatrixDecisionVariable< Rows, Cols > NewContinuousVariables (int rows, int cols, const std::string &name="X")
Adds continuous variables, appending them to an internal vector of any existing vars. More...

template<int Rows, int Cols = 1>
MatrixDecisionVariable< Rows, Cols > NewContinuousVariables (const std::string &name="X")
Adds continuous variables, appending them to an internal vector of any existing vars. More...

template<int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
MatrixDecisionVariable< Rows, Cols > NewBinaryVariables (int rows, int cols, const std::string &name)
Adds binary variables, appending them to an internal vector of any existing vars. More...

template<int Rows, int Cols = 1>
MatrixDecisionVariable< Rows, Cols > NewBinaryVariables (const std::string &name="b")
Adds a matrix of binary variables into the optimization program. More...

VectorXDecisionVariable NewBinaryVariables (int rows, const std::string &name="b")
Adds binary variables to this MathematicalProgram. More...

MatrixXDecisionVariable NewSymmetricContinuousVariables (int rows, const std::string &name="Symmetric")
Adds a runtime sized symmetric matrix as decision variables to this MathematicalProgram. More...

template<int rows>
MatrixDecisionVariable< rows, rows > NewSymmetricContinuousVariables (const std::string &name="Symmetric")
Adds a static sized symmetric matrix as decision variables to this MathematicalProgram. More...

void AddDecisionVariables (const Eigen::Ref< const VectorXDecisionVariable > &decision_variables)
Appends new variables to the end of the existing variables. More...

symbolic::Polynomial NewFreePolynomial (const symbolic::Variables &indeterminates, int degree, const std::string &coeff_name="a")
Returns a free polynomial in a monomial basis over indeterminates of a given degree. More...

std::pair< symbolic::Polynomial, Binding< PositiveSemidefiniteConstraint > > NewSosPolynomial (const Eigen::Ref< const VectorX< symbolic::Monomial >> &monomial_basis)
Returns a pair of a SOS polynomial p = mᵀQm and a PSD constraint for a new coefficients matrix Q, where m is the monomial basis. More...

std::pair< symbolic::Polynomial, Binding< PositiveSemidefiniteConstraint > > NewSosPolynomial (const symbolic::Variables &indeterminates, int degree)
Returns a pair of a SOS polynomial p = m(x)ᵀQm(x) of degree degree and a PSD constraint for the coefficients matrix Q, where m(x) is the result of calling MonomialBasis(indeterminates, degree/2). More...

template<int rows, int cols>
MatrixIndeterminate< rows, cols > NewIndeterminates (const std::array< std::string, rows *cols > &names)
Adds indeterminates, appending them to an internal vector of any existing indeterminates. More...

template<int rows>
VectorIndeterminate< rows > NewIndeterminates (const std::array< std::string, rows > &names)
Adds indeterminates, appending them to an internal vector of any existing indeterminates. More...

template<int rows, int cols>
MatrixIndeterminate< rows, cols > NewIndeterminates (const std::string &name="X")
Adds indeterminates, appending them to an internal vector of any existing indeterminates. More...

template<int rows>
VectorIndeterminate< rows > NewIndeterminates (const std::string &name="x")
Adds indeterminates to the program. More...

VectorXIndeterminate NewIndeterminates (int rows, const std::vector< std::string > &names)
Adds indeterminates to this MathematicalProgram. More...

VectorXIndeterminate NewIndeterminates (int rows, const std::string &name="x")
Adds indeterminates to this MathematicalProgram, with default name "x". More...

MatrixXIndeterminate NewIndeterminates (int rows, int cols, const std::vector< std::string > &names)
Adds indeterminates, appending them to an internal vector of any existing vars. More...

MatrixXIndeterminate NewIndeterminates (int rows, int cols, const std::string &name="X")
Adds indeterminates to this MathematicalProgram, with default name "X". More...

void AddIndeterminates (const Eigen::Ref< const VectorXIndeterminate > &new_indeterminates)

Binding< VisualizationCallbackAddVisualizationCallback (const VisualizationCallback::CallbackFunction &callback, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a callback method to visualize intermediate results of the optimization. More...

Binding< VisualizationCallbackAddVisualizationCallback (const VisualizationCallback::CallbackFunction &callback, const VariableRefList &vars)
Adds a callback method to visualize intermediate results of the optimization. More...

Binding< CostAddCost (const Binding< Cost > &binding)
Adds a generic cost to the optimization program. More...

template<typename C >
auto AddCost (const std::shared_ptr< C > &obj, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost type to the optimization program. More...

template<typename C >
auto AddCost (const std::shared_ptr< C > &obj, const VariableRefList &vars)
Adds a generic cost to the optimization program. More...

template<typename F >
std::enable_if< detail::is_cost_functor_candidate< F >::value, Binding< Cost > >::type AddCost (F &&f, const VariableRefList &vars)
Adds a cost to the optimization program on a list of variables. More...

template<typename F >
std::enable_if< detail::is_cost_functor_candidate< F >::value, Binding< Cost > >::type AddCost (F &&f, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost to the optimization program on an Eigen::Vector containing decision variables. More...

template<typename F , typename Vars >
std::enable_if< detail::assert_if_is_constraint< F >::value, Binding< Cost > >::type AddCost (F &&, Vars &&)
Statically assert if a user inadvertently passes a binding-compatible Constraint. More...

Binding< LinearCostAddCost (const Binding< LinearCost > &binding)
Adds a cost term of the form c'*x. More...

Adds a linear cost term of the form a'*x + b. More...

Binding< LinearCostAddLinearCost (const Eigen::Ref< const Eigen::VectorXd > &a, double b, const VariableRefList &vars)
Adds a linear cost term of the form a'*x + b. More...

Binding< LinearCostAddLinearCost (const Eigen::Ref< const Eigen::VectorXd > &a, double b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a linear cost term of the form a'*x + b. More...

template<typename VarType >
Binding< LinearCostAddLinearCost (const Eigen::Ref< const Eigen::VectorXd > &a, const VarType &vars)
Adds a linear cost term of the form a'*x. More...

Adds a cost term of the form 0.5*x'*Q*x + b'x. More...

Add a quadratic cost term of the form 0.5*x'*Q*x + b'*x + c. More...

Binding< QuadraticCostAddQuadraticErrorCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &x_desired, const VariableRefList &vars)
Adds a cost term of the form (x-x_desired)'Q(x-x_desired). More...

Binding< QuadraticCostAddQuadraticErrorCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &x_desired, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost term of the form (x-x_desired)'Q(x-x_desired). More...

Binding< QuadraticCostAddL2NormCost (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds a cost term of the form | Ax - b |^2. More...

Binding< QuadraticCostAddL2NormCost (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost term of the form | Ax - b |^2. More...

Binding< QuadraticCostAddQuadraticCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds a cost term of the form 0.5*x'*Q*x + b'x. More...

Binding< QuadraticCostAddQuadraticCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &b, double c, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost term of the form 0.5*x'*Q*x + b'x + c Applied to subset of the variables. More...

Binding< QuadraticCostAddQuadraticCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a cost term of the form 0.5*x'*Q*x + b'x Applied to subset of the variables. More...

Adds a cost term in the polynomial form. More...

Adds a cost in the symbolic form. More...

Binding< ConstraintAddConstraint (const Binding< Constraint > &binding)
Adds a generic constraint to the program. More...

Binding< ConstraintAddConstraint (const symbolic::Expression &e, double lb, double ub)
Adds one row of constraint lb <= e <= ub where e is a symbolic expression. More...

Binding< ConstraintAddConstraint (const Eigen::Ref< const VectorX< symbolic::Expression >> &v, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)

Add a constraint represented by a symbolic formula to the program. More...

template<typename Derived >
std::enable_if< is_eigen_scalar_same< Derived, symbolic::Formula >::value, Binding< Constraint > >::type AddConstraint (const Eigen::ArrayBase< Derived > &formulas)

template<typename C >
auto AddConstraint (std::shared_ptr< C > con, const VariableRefList &vars)
Adds a generic constraint to the program. More...

template<typename C >
auto AddConstraint (std::shared_ptr< C > con, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a generic constraint to the program. More...

Binding< LinearConstraintAddConstraint (const Binding< LinearConstraint > &binding)
Adds linear constraints referencing potentially a subset of the decision variables (defined in the vars parameter). More...

Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const VariableRefList &vars)
Adds linear constraints referencing potentially a subset of the decision variables (defined in the vars parameter). More...

Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds linear constraints referencing potentially a subset of the decision variables (defined in the vars parameter). More...

Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const Eigen::RowVectorXd > &a, double lb, double ub, const VariableRefList &vars)
Adds one row of linear constraint referencing potentially a subset of the decision variables (defined in the vars parameter). More...

Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const Eigen::RowVectorXd > &a, double lb, double ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds one row of linear constraint referencing potentially a subset of the decision variables (defined in the vars parameter). More...

Binding< LinearConstraintAddLinearConstraint (const symbolic::Expression &e, double lb, double ub)
Adds one row of linear constraint lb <= e <= ub where e is a symbolic expression. More...

Binding< LinearConstraintAddLinearConstraint (const Eigen::Ref< const VectorX< symbolic::Expression >> &v, const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)
Adds linear constraints represented by symbolic expressions to the program. More...

Add a linear constraint represented by a symbolic formula to the program. More...

template<typename Derived >
std::enable_if< is_eigen_scalar_same< Derived, symbolic::Formula >::value, Binding< LinearConstraint > >::type AddLinearConstraint (const Eigen::ArrayBase< Derived > &formulas)
Add a linear constraint represented by an Eigen::Array<symbolic::Formula> to the program. More...

Binding< LinearEqualityConstraintAddConstraint (const Binding< LinearEqualityConstraint > &binding)
Adds linear equality constraints referencing potentially a subset of the decision variables. More...

Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const symbolic::Expression &e, double b)
Adds one row of linear constraint e = b where e is a symbolic expression. More...

Adds a linear equality constraint represented by a symbolic formula to the program. More...

template<typename DerivedV , typename DerivedB >
std::enable_if< is_eigen_vector_expression_double_pair< DerivedV, DerivedB >::value, Binding< LinearEqualityConstraint > >::type AddLinearEqualityConstraint (const Eigen::MatrixBase< DerivedV > &v, const Eigen::MatrixBase< DerivedB > &b)
Adds linear equality constraints $$v = b$$, where v(i) is a symbolic linear expression. More...

template<typename DerivedV , typename DerivedB >
std::enable_if< is_eigen_nonvector_expression_double_pair< DerivedV, DerivedB >::value, Binding< LinearEqualityConstraint > >::type AddLinearEqualityConstraint (const Eigen::MatrixBase< DerivedV > &V, const Eigen::MatrixBase< DerivedB > &B, bool lower_triangle=false)
Adds a linear equality constraint for a matrix of linear expression V, such that V(i, j) = B(i, j). More...

Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const Eigen::Ref< const Eigen::MatrixXd > &Aeq, const Eigen::Ref< const Eigen::VectorXd > &beq, const VariableRefList &vars)

Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const Eigen::Ref< const Eigen::MatrixXd > &Aeq, const Eigen::Ref< const Eigen::VectorXd > &beq, const Eigen::Ref< const VectorXDecisionVariable > &vars)

Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const Eigen::Ref< const Eigen::RowVectorXd > &a, double beq, const VariableRefList &vars)
Adds one row of linear equality constraint referencing potentially a subset of decision variables. More...

Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const Eigen::Ref< const Eigen::RowVectorXd > &a, double beq, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds one row of linear equality constraint referencing potentially a subset of decision variables. More...

Binding< BoundingBoxConstraintAddConstraint (const Binding< BoundingBoxConstraint > &binding)
Adds bounding box constraints referencing potentially a subest of the decision variables. More...

Binding< BoundingBoxConstraintAddBoundingBoxConstraint (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const VariableRefList &vars)

Binding< BoundingBoxConstraintAddBoundingBoxConstraint (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds bounding box constraints referencing potentially a subset of the decision variables. More...

Binding< BoundingBoxConstraintAddBoundingBoxConstraint (double lb, double ub, const symbolic::Variable &var)
Adds bounds for a single variable. More...

Binding< BoundingBoxConstraintAddBoundingBoxConstraint (double lb, double ub, const VariableRefList &vars)
Adds the same scalar lower and upper bound to every variable in the list. More...

template<typename Derived >
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value &&Derived::ColsAtCompileTime==1, Binding< BoundingBoxConstraint > >::type AddBoundingBoxConstraint (double lb, double ub, const Eigen::MatrixBase< Derived > &vars)
Adds the same scalar lower and upper bound to every variable in vars. More...

template<typename Derived >
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value &&Derived::ColsAtCompileTime!=1, Binding< BoundingBoxConstraint > >::type AddBoundingBoxConstraint (double lb, double ub, const Eigen::MatrixBase< Derived > &vars)
Adds the same scalar lower and upper bound to every variable in vars. More...

Binding< LorentzConeConstraintAddConstraint (const Binding< LorentzConeConstraint > &binding)
Adds Lorentz cone constraint referencing potentially a subset of the decision variables. More...

Binding< LorentzConeConstraintAddLorentzConeConstraint (const Eigen::Ref< const VectorX< symbolic::Expression >> &v)
Adds Lorentz cone constraint referencing potentially a subset of the decision variables. More...

Adds Lorentz cone constraint on the linear expression v1 and quadratic expression v2, such that v1 >= sqrt(v2) More...

Binding< LorentzConeConstraintAddLorentzConeConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds Lorentz cone constraint referencing potentially a subset of the decision variables (defined in the vars parameter). More...

Binding< LorentzConeConstraintAddLorentzConeConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds Lorentz cone constraint referencing potentially a subset of the decision variables (defined in the vars parameter). More...

Imposes that a vector $$x\in\mathbb{R}^m$$ lies in Lorentz cone. More...

template<int rows>
Binding< LorentzConeConstraintAddLorentzConeConstraint (const Eigen::MatrixBase< VectorDecisionVariable< rows >> &vars)
Imposes that a vector $$x\in\mathbb{R}^m$$ lies in Lorentz cone. More...

Binding< RotatedLorentzConeConstraintAddConstraint (const Binding< RotatedLorentzConeConstraint > &binding)
Adds a rotated Lorentz cone constraint referencing potentially a subset of decision variables. More...

Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const symbolic::Expression &linear_expression1, const symbolic::Expression &linear_expression2, const symbolic::Expression &quadratic_expression, double tol=0)
Adds rotated Lorentz cone constraint on the linear expression v1, v2 and quadratic expression u, such that v1 * v2 >= u, v1 >= 0, v2 >= 0. More...

Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const Eigen::Ref< const VectorX< symbolic::Expression >> &v)
Adds a constraint that a symbolic expression. More...

Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const VariableRefList &vars)
Adds a rotated Lorentz cone constraint referencing potentially a subset of decision variables, The linear expression $$z=Ax+b$$ is in rotated Lorentz cone. More...

Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a rotated Lorentz cone constraint referencing potentially a subset of decision variables, The linear expression $$z=Ax+b$$ is in rotated Lorentz cone. More...

Impose that a vector $$x\in\mathbb{R}^m$$ is in rotated Lorentz cone. More...

template<int rows>
Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const Eigen::MatrixBase< VectorDecisionVariable< rows >> &vars)
Impose that a vector $$x\in\mathbb{R}^m$$ is in rotated Lorentz cone. More...

Binding< LinearComplementarityConstraintAddConstraint (const Binding< LinearComplementarityConstraint > &binding)
Adds a linear complementarity constraints referencing a subset of the decision variables. More...

Binding< LinearComplementarityConstraintAddLinearComplementarityConstraint (const Eigen::Ref< const Eigen::MatrixXd > &M, const Eigen::Ref< const Eigen::VectorXd > &q, const VariableRefList &vars)
Adds a linear complementarity constraints referencing a subset of the decision variables. More...

Binding< LinearComplementarityConstraintAddLinearComplementarityConstraint (const Eigen::Ref< const Eigen::MatrixXd > &M, const Eigen::Ref< const Eigen::VectorXd > &q, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a linear complementarity constraints referencing a subset of the decision variables. More...

Binding< ConstraintAddPolynomialConstraint (const VectorXPoly &polynomials, const std::vector< Polynomiald::VarType > &poly_vars, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const VariableRefList &vars)
Adds a polynomial constraint to the program referencing a subset of the decision variables (defined in the vars parameter). More...

Binding< ConstraintAddPolynomialConstraint (const VectorXPoly &polynomials, const std::vector< Polynomiald::VarType > &poly_vars, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a polynomial constraint to the program referencing a subset of the decision variables (defined in the vars parameter). More...

Binding< PositiveSemidefiniteConstraintAddConstraint (const Binding< PositiveSemidefiniteConstraint > &binding)
Adds a positive semidefinite constraint on a symmetric matrix. More...

Binding< PositiveSemidefiniteConstraintAddConstraint (std::shared_ptr< PositiveSemidefiniteConstraint > con, const Eigen::Ref< const MatrixXDecisionVariable > &symmetric_matrix_var)
Adds a positive semidefinite constraint on a symmetric matrix. More...

Binding< PositiveSemidefiniteConstraintAddPositiveSemidefiniteConstraint (const Eigen::Ref< const MatrixXDecisionVariable > &symmetric_matrix_var)
Adds a positive semidefinite constraint on a symmetric matrix. More...

template<typename Derived >
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Expression >::value, Binding< PositiveSemidefiniteConstraint > >::type AddPositiveSemidefiniteConstraint (const Eigen::MatrixBase< Derived > &e)
Adds a positive semidefinite constraint on a symmetric matrix of symbolic expressions e. More...

Binding< LinearMatrixInequalityConstraintAddConstraint (const Binding< LinearMatrixInequalityConstraint > &binding)
Adds a linear matrix inequality constraint to the program. More...

Binding< LinearMatrixInequalityConstraintAddLinearMatrixInequalityConstraint (const std::vector< Eigen::Ref< const Eigen::MatrixXd >> &F, const VariableRefList &vars)
Adds a linear matrix inequality constraint to the program. More...

Binding< LinearMatrixInequalityConstraintAddLinearMatrixInequalityConstraint (const std::vector< Eigen::Ref< const Eigen::MatrixXd >> &F, const Eigen::Ref< const VectorXDecisionVariable > &vars)
Adds a linear matrix inequality constraint to the program. More...

MatrixX< symbolic::ExpressionAddPositiveDiagonallyDominantMatrixConstraint (const Eigen::Ref< const MatrixX< symbolic::Expression >> &X)
Adds the constraint that a symmetric matrix is diagonally dominant with non-negative diagonal entries. More...

std::pair< Binding< PositiveSemidefiniteConstraint >, Binding< LinearEqualityConstraint > > AddSosConstraint (const symbolic::Polynomial &p, const Eigen::Ref< const VectorX< symbolic::Monomial >> &monomial_basis)
Adds constraints that a given polynomial p is a sums-of-squares (SOS), that is, p can be decomposed into mᵀQm, where m is the monomial_basis. More...

std::pair< Binding< PositiveSemidefiniteConstraint >, Binding< LinearEqualityConstraint > > AddSosConstraint (const symbolic::Polynomial &p)
Adds constraints that a given polynomial p is a sums-of-squares (SOS), that is, p can be decomposed into mᵀQm, where m is the monomial basis of all indeterminates in the program with degree equal to half the TotalDegree of p. More...

std::pair< Binding< PositiveSemidefiniteConstraint >, Binding< LinearEqualityConstraint > > AddSosConstraint (const symbolic::Expression &e, const Eigen::Ref< const VectorX< symbolic::Monomial >> &monomial_basis)
Adds constraints that a given symbolic expression e is a sums-of-squares (SOS), that is, p can be decomposed into mᵀQm, where m is the monomial_basis. More...

std::pair< Binding< PositiveSemidefiniteConstraint >, Binding< LinearEqualityConstraint > > AddSosConstraint (const symbolic::Expression &e)
Adds constraints that a given symbolic expression e is a sums-of-squares (SOS), that is, e can be decomposed into mTQm. More...

double GetInitialGuess (const symbolic::Variable &decision_variable) const
Gets the initial guess for a single variable. More...

template<typename Derived >
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value, Eigen::Matrix< double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime > >::type GetInitialGuess (const Eigen::MatrixBase< Derived > &decision_variable_mat) const
Gets the initial guess for some variables. More...

void SetInitialGuess (const symbolic::Variable &decision_variable, double variable_guess_value)
Sets the initial guess for a single variable decision_variable. More...

template<typename DerivedA , typename DerivedB >
void SetInitialGuess (const Eigen::MatrixBase< DerivedA > &decision_variable_mat, const Eigen::MatrixBase< DerivedB > &x0)
Sets the initial guess for the decision variables stored in decision_variable_mat to be x0. More...

template<typename Derived >
void SetInitialGuessForAllVariables (const Eigen::MatrixBase< Derived > &x0)
Set the initial guess for ALL decision variables. More...

SolutionResult Solve ()
Solve the MathematicalProgram. More...

void PrintSolution ()

void SetSolverOption (const SolverId &solver_id, const std::string &solver_option, double option_value)
Set an option for a particular solver. More...

void SetSolverOption (const SolverId &solver_id, const std::string &solver_option, int option_value)

void SetSolverOption (const SolverId &solver_id, const std::string &solver_option, const std::string &option_value)

const std::map< std::string, double > & GetSolverOptionsDouble (const SolverId &solver_id) const

const std::map< std::string, int > & GetSolverOptionsInt (const SolverId &solver_id) const

const std::map< std::string, std::string > & GetSolverOptionsStr (const SolverId &solver_id) const

optional< SolverIdGetSolverId () const
Returns the ID of the solver that was used to solve this program. More...

double GetOptimalCost () const
Getter for optimal cost at the solution. More...

double GetLowerBoundCost () const
Getter for lower bound on optimal cost. More...

const std::vector< Binding< VisualizationCallback > > & visualization_callbacks () const
Getter for all callbacks. More...

const std::vector< Binding< Cost > > & generic_costs () const
Getter for all generic costs. More...

const std::vector< Binding< Constraint > > & generic_constraints () const
Getter for all generic constraints. More...

const std::vector< Binding< LinearEqualityConstraint > > & linear_equality_constraints () const
Getter for linear equality constraints. More...

const std::vector< Binding< LinearCost > > & linear_costs () const
Getter for linear costs. More...

const std::vector< Binding< LinearConstraint > > & linear_constraints () const
Getter for linear constraints. More...

const std::vector< Binding< LorentzConeConstraint > > & lorentz_cone_constraints () const
Getter for Lorentz cone constraint. More...

const std::vector< Binding< RotatedLorentzConeConstraint > > & rotated_lorentz_cone_constraints () const
Getter for rotated Lorentz cone constraint. More...

const std::vector< Binding< PositiveSemidefiniteConstraint > > & positive_semidefinite_constraints () const
Getter for positive semidefinite constraint. More...

const std::vector< Binding< LinearMatrixInequalityConstraint > > & linear_matrix_inequality_constraints () const
Getter for linear matrix inequality constraint. More...

std::vector< Binding< Cost > > GetAllCosts () const
Getter returning all costs (for now linear costs appended to generic costs, then quadratic costs appended to generic costs). More...

std::vector< Binding< LinearConstraint > > GetAllLinearConstraints () const
Getter returning all linear constraints (both linear equality and inequality constraints). More...

const std::vector< Binding< BoundingBoxConstraint > > & bounding_box_constraints () const
Getter for all bounding box constraints. More...

const std::vector< Binding< LinearComplementarityConstraint > > & linear_complementarity_constraints () const
Getter for all linear complementarity constraints. More...

template<typename T >
std::shared_ptr< TGetSolverData ()

int num_vars () const
Getter for number of variables in the optimization program. More...

const Eigen::VectorXd & initial_guess () const
Getter for the initial guess. More...

int FindDecisionVariableIndex (const symbolic::Variable &var) const
Returns the index of the decision variable. More...

std::vector< intFindDecisionVariableIndices (const Eigen::Ref< const VectorXDecisionVariable > &vars) const
Returns the indices of the decision variables. More...

int num_indeterminates () const
Gets the number of indeterminates in the optimization program. More...

size_t FindIndeterminateIndex (const symbolic::Variable &var) const
Returns the index of the indeterminate. More...

template<typename Derived >
std::enable_if< std::is_same< typename Derived::Scalar, symbolic::Variable >::value, Eigen::Matrix< double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime > >::type GetSolution (const Eigen::MatrixBase< Derived > &var) const
Gets the solution of an Eigen matrix of decision variables. More...

double GetSolution (const symbolic::Variable &var) const
Gets the value of a single decision variable. More...

symbolic::Expression SubstituteSolution (const symbolic::Expression &e) const
Replaces the variables in an expression with the solutions to the variables, returns the expression after substitution. More...

symbolic::Polynomial SubstituteSolution (const symbolic::Polynomial &p) const
Replaces the decision variables in a polynomial with the solutions to the variables, returns the polynomial after substitution. More...

template<typename C , typename DerivedX >
std::enable_if< is_eigen_vector< DerivedX >::value, VectorX< typename DerivedX::Scalar > >::type EvalBinding (const Binding< C > &binding, const Eigen::MatrixBase< DerivedX > &prog_var_vals) const
Evaluates the value of some binding, for some input value for all decision variables. More...

void EvalVisualizationCallbacks (const Eigen::Ref< const Eigen::VectorXd > &prog_var_vals) const
Evaluates all visualization callbacks registered with the MathematicalProgram. More...

template<typename C >
Eigen::VectorXd EvalBindingAtSolution (const Binding< C > &binding) const
Evaluates the evaluator in binding at the solution value. More...

template<typename C >
Eigen::VectorXd EvalBindingAtInitialGuess (const Binding< C > &binding) const
Evaluates the evaluator in binding at the initial guess. More...

const VectorXDecisionVariabledecision_variables () const
Getter for all decision variables in the program. More...

const symbolic::Variabledecision_variable (int i) const
Getter for the decision variable with index i in the program. More...

const VectorXIndeterminateindeterminates () const
Getter for all indeterminates in the program. More...

const symbolic::Variableindeterminate (int i) const
Getter for the indeterminate with index i in the program. More...

void SetSolverResult (const SolverResult &solver_result)
Solver reports its result back to MathematicalProgram, by passing the solver_result, which contains the solver result. More...

MathematicalProgram (const MathematicalProgram &)=delete

MathematicalProgramoperator= (const MathematicalProgram &)=delete

MathematicalProgram (MathematicalProgram &&)=delete

MathematicalProgramoperator= (MathematicalProgram &&)=delete

Public Types inherited from MathematicalProgram
using VarType = symbolic::Variable::Type

Static Public Member Functions inherited from MathematicalProgram
template<typename F >
static std::shared_ptr< CostMakeCost (F &&f)
Convert an input of type F to a FunctionCost object. More...

Static Public Attributes inherited from MathematicalProgram
static constexpr double kGlobalInfeasibleCost
The optimal cost is +∞ when the problem is globally infeasible. More...

static constexpr double kUnboundedCost
The optimal cost is -∞ when the problem is unbounded. More...

## Detailed Description

Solves the inverse kinematics problem as a mixed integer convex optimization problem.

We use a mixed-integer convex relaxation of the rotation matrix. So if this global inverse kinematics problem says the solution is infeasible, then it is guaranteed that the kinematics constraints are not satisfiable. If the global inverse kinematics returns a solution, the posture should approximately satisfy the kinematics constraints, with some error. The approach is described in Global Inverse Kinematics via Mixed-integer Convex Optimization by Hongkai Dai, Gregory Izatt and Russ Tedrake, ISRR, 2017.

## Constructor & Destructor Documentation

 GlobalInverseKinematics ( const GlobalInverseKinematics & )
delete
 GlobalInverseKinematics ( GlobalInverseKinematics && )
delete
 GlobalInverseKinematics ( const RigidBodyTreed & robot, const Options & options = Options() )
explicit

Parses the robot kinematics tree.

The decision variables include the pose for each body (position/orientation). This constructor loops through each body inside the robot kinematics tree, adds the constraint on each body pose, so that the adjacent bodies are connected correctly by the joint in between the bodies.

Parameters
 robot The robot on which the inverse kinematics problem is solved. options The options to relax SO(3) constraint as mixed-integer convex constraints. Refer to MixedIntegerRotationConstraintGenerator for more details on the parameters in options.

Here is the call graph for this function:

 ~GlobalInverseKinematics ( )
inlineoverride

Here is the call graph for this function:

## Member Function Documentation

 void AddJointLimitConstraint ( int body_index, double joint_lower_bound, double joint_upper_bound, bool linear_constraint_approximation = false )

Adds joint limits on a specified joint.

Parameters
 body_index The joint connecting the parent link to this body will be constrained. joint_lower_bound The lower bound for the joint. joint_upper_bound The upper bound for the joint. linear_constraint_approximation If true, joint limits are approximated as linear constraints on parent and child link orientations, otherwise they are imposed as Lorentz cone constraints. With the Lorentz cone formulation, the joint limit constraint would be tight if our mixed-integer constraint on SO(3) were tight. By enforcing the joint limits as linear constraint, the original inverse kinematics problem is further relaxed, on top of SO(3) relaxation, but potentially with faster computation. Default: is false.

Here is the call graph for this function:

Here is the caller graph for this function:

 void AddPostureCost ( const Eigen::Ref< const Eigen::VectorXd > & q_desired, const Eigen::Ref< const Eigen::VectorXd > & body_position_cost, const Eigen::Ref< const Eigen::VectorXd > & body_orientation_cost )

Penalizes the deviation to the desired posture.

For each body (except the world) in the kinematic tree, we add the cost ∑ᵢ body_position_cost(i) * body_position_error(i) + body_orientation_cost(i) * body_orientation_error(i) where body_position_error(i) is computed as the Euclidean distance error |p_WBo(i) - p_WBo_desired(i)| where

• p_WBo(i) : position of body i'th origin Bo in the world frame W.
• p_WBo_desired(i): position of body i'th origin Bo in the world frame W, computed from the desired posture q_desired.

body_orientation_error(i) is computed as (1 - cos(θ)), where θ is the angle between the orientation of body i'th frame and body i'th frame using the desired posture. Notice that 1 - cos(θ) = θ²/2 + O(θ⁴), so this cost is on the square of θ, when θ is small. Notice that since body 0 is the world, the cost on that body is always 0, no matter what value body_position_cost(0) and body_orientation_cost(0) take.

Parameters
 q_desired The desired posture. body_position_cost The cost for each body's position error. Unit is [1/m] (one over meters).
Precondition
1. body_position_cost.rows() == robot->get_num_bodies(), where robot is the input argument in the constructor of the class.
2. body_position_cost(i) is non-negative.
Exceptions
 a runtime error if the precondition is not satisfied.
Parameters
 body_orientation_cost The cost for each body's orientation error.
Precondition
1. body_orientation_cost.rows() == robot->get_num_bodies() , where robot is the input argument in the constructor of the class.
2. body_position_cost(i) is non-negative.
Exceptions
 a runtime_error if the precondition is not satisfied.

Here is the call graph for this function:

Here is the caller graph for this function:

 solvers::Binding< solvers::LinearConstraint > AddWorldOrientationConstraint ( int body_idx, const Eigen::Quaterniond & desired_orientation, double angle_tol )

Add a constraint that the angle between the body orientation and the desired orientation should not be larger than angle_tol.

If we denote the angle between two rotation matrices R1 and R2 as θ, namely θ is the angle of the angle-axis representation of the rotation matrix R1ᵀ * R2, we then know

   trace(R1ᵀ * R2) = 2 * cos(θ) + 1


as in http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/ To constraint θ < angle_tol, we can impose the following constraint

   2 * cos(angle_tol) + 1 <= trace(R1ᵀ * R2) <= 3

Parameters
 body_idx The index of the body whose orientation will be constrained. desired_orientation The desired orientation of the body. angle_tol The tolerance on the angle between the body orientation and the desired orientation. Unit is radians.
Return values
 binding The newly added constraint, together with the bound variables.

Here is the call graph for this function:

Here is the caller graph for this function:

 solvers::Binding< solvers::LinearConstraint > AddWorldPositionConstraint ( int body_idx, const Eigen::Vector3d & p_BQ, const Eigen::Vector3d & box_lb_F, const Eigen::Vector3d & box_ub_F, const Eigen::Isometry3d & X_WF = Eigen::Isometry3d::Identity() )

Adds the constraint that the position of a point Q on a body B (whose index is body_idx), is within a box in a specified frame F.

The constraint is that the point position, computed as

  p_WQ = p_WBo + R_WB * p_BQ


where

• p_WQ is the position of the body point Q measured and expressed in the world frame W.
• p_WBo is the position of the body origin Bo measured and expressed in the world frame W.
• R_WB is the rotation matrix of the body measured and expressed in the world frame W.
• p_BQ is the position of the body point Q measured and expressed in the body frame B. p_WQ should lie within a bounding box in the frame F. Namely
  box_lb_F <= p_FQ <= box_ub_F

where p_FQ is the position of the point Q measured and expressed in the F. The inequality is imposed elementwisely.

Notice that since the rotation matrix R_WB does not lie exactly on the SO(3), due to the McCormick envelope relaxation, this constraint is subject to the accumulated error from the root of the kinematics tree.

Parameters
 body_idx The index of the body on which the position of a point is constrained. p_BQ The position of the point Q measured and expressed in the body frame B. box_lb_F The lower bound of the box in frame F. box_ub_F The upper bound of the box in frame F. X_WF. The frame in which the box is specified. This frame is represented by an isometry transform X_WF, the transform from the constraint frame F to the world frame W. Namely if the position of the point Q in the world frame is p_WQ, then the constraint is  box_lb_F <= R_FW * (p_WQ-p_WFo) <= box_ub_F  where R_FW is the rotation matrix of frame W expressed and measured in frame F. R_FW = X_WF.linear().transpose(). p_WFo is the position of frame F's origin, expressed and measured in frame W. p_WFo = X_WF.translation(). Default: is the identity transform.
Return values
 binding The newly added constraint, together with the bound variables.

Here is the call graph for this function:

Here is the caller graph for this function:

 const solvers::VectorDecisionVariable< 3 > & body_position ( int body_index ) const

Getter for the decision variables on the position p_WBo of the body B's origin measured and expressed in the world frame.

Parameters
 body_index The index of the queried body. Notice that body 0 is the world, and thus not a decision variable. Throws a runtime_error if the index is smaller than 1, or greater than or equal to the total number of bodies in the robot.

Here is the call graph for this function:

Here is the caller graph for this function:

 const solvers::MatrixDecisionVariable< 3, 3 > & body_rotation_matrix ( int body_index ) const

Getter for the decision variables on the rotation matrix R_WB for a body with the specified index.

This is the orientation of body i's frame measured and expressed in the world frame.

Parameters
 body_index The index of the queried body. Notice that body 0 is the world, and thus not a decision variable. Throws a runtime_error if the index is smaller than 1, or no smaller than the total number of bodies in the robot.

Here is the call graph for this function:

Here is the caller graph for this function:

 solvers::VectorXDecisionVariable BodyPointInOneOfRegions ( int body_index, const Eigen::Ref< const Eigen::Vector3d > & p_BQ, const std::vector< Eigen::Matrix3Xd > & region_vertices )

Constrain the point Q lying within one of the convex polytopes.

Each convex polytope Pᵢ is represented by its vertices as Pᵢ = ConvexHull(v_i1, v_i2, ... v_in). Mathematically we want to impose the constraint that the p_WQ, i.e., the position of point Q in world frame W, satisfies

  p_WQ ∈ Pᵢ for one i.


To impose this constraint, we consider to introduce binary variable zᵢ, and continuous variables w_i1, w_i2, ..., w_in for each vertex of Pᵢ, with the following constraints

  p_WQ = sum_i (w_i1 * v_i1 + w_i2 * v_i2 + ... + w_in * v_in)
w_ij >= 0, ∀i,j
w_i1 + w_i2 + ... + w_in = zᵢ
sum_i zᵢ = 1
zᵢ ∈ {0, 1}


Notice that if zᵢ = 0, then w_i1 * v_i1 + w_i2 * v_i2 + ... + w_in * v_in is just 0. This function can be used for collision avoidance, where each region Pᵢ is a free space region. It can also be used for grasping, where each region Pᵢ is a surface patch on the grasped object. Note this approach also works if the region Pᵢ overlaps with each other.

Parameters
 body_index The index of the body to on which point Q is attached. p_BQ The position of point Q in the body frame B. region_vertices region_vertices[i] is the vertices for the i'th region.
Return values
 z The newly added binary variables. If point Q is in the i'th region, z(i) = 1.
Precondition
region_vertices[i] has at least 3 columns. Throw a std::runtime_error if the precondition is not satisfied.

Here is the call graph for this function:

Here is the caller graph for this function:

 GlobalInverseKinematics& operator= ( const GlobalInverseKinematics & )
delete
 GlobalInverseKinematics& operator= ( GlobalInverseKinematics && )
delete
 Eigen::VectorXd ReconstructGeneralizedPositionSolution ( ) const

After solving the inverse kinematics problem and finding out the pose of each body, reconstruct the robot generalized position (joint angles, etc) that matches with the body poses.

Notice that since the rotation matrix is approximated, that the solution of body_rotmat() might not be on SO(3) exactly, the reconstructed body posture might not match with the body poses exactly, and the kinematics constraint might not be satisfied exactly with this reconstructed posture.

Warning
Do not call this method if the problem is not solved successfully! The returned value can be NaN or meaningless number if the problem is not solved.
Return values
 q The reconstructed posture of the robot of the generalized coordinates, corresponding to the RigidBodyTree on which the inverse kinematics problem is solved.

Here is the call graph for this function:

Here is the caller graph for this function:

The documentation for this class was generated from the following files: