Drake
GlobalInverseKinematics Class Reference

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

#include <drake/multibody/global_inverse_kinematics.h>

Inheritance diagram for GlobalInverseKinematics:
Collaboration diagram for GlobalInverseKinematics:

Public Member Functions

 GlobalInverseKinematics (const RigidBodyTreed &robot, int num_binary_vars_per_half_axis=2)
 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...
 
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 ()
 
template<int rows, int cols>
MatrixDecisionVariable< rows, cols > NewVariables (VarType type, const std::array< std::string, rows *cols > &names)
 Adds new variables to MathematicalProgram. More...
 
template<int rows>
VectorDecisionVariable< rows > NewVariables (VarType type, const std::array< std::string, rows > &names)
 Adds column vector variables to the optimization program. More...
 
template<int rows>
MatrixDecisionVariable< rows, rows > NewSymmetricVariables (VarType type, const std::array< std::string, rows *(rows+1)/2 > &names)
 Adds symmetric matrix variables to optimization program. More...
 
VectorXDecisionVariable NewContinuousVariables (int rows, const std::vector< std::string > &names)
 Adds continuous variables to this MathematicalProgram. More...
 
VectorXDecisionVariable NewContinuousVariables (int rows, const std::string &name="x")
 Adds continuous variables to this MathematicalProgram, with default name "x". More...
 
MatrixXDecisionVariable NewContinuousVariables (int rows, int cols, const std::vector< std::string > &names)
 Adds continuous variables, appending them to an internal vector of any existing vars. More...
 
MatrixXDecisionVariable NewContinuousVariables (int rows, int cols, const std::string &name="X")
 Adds continuous variables to this MathematicalProgram, with default name "X". More...
 
template<int rows, int cols>
MatrixDecisionVariable< rows, cols > NewContinuousVariables (const std::array< std::string, rows *cols > &names)
 Adds continuous variables, appending them to an internal vector of any existing vars. More...
 
template<int rows, int cols>
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>
VectorDecisionVariable< rows > NewContinuousVariables (const std::array< std::string, rows > &names)
 Adds continuous variables, appending them to an internal vector of any existing vars. More...
 
template<int rows>
VectorDecisionVariable< rows > NewContinuousVariables (const std::string &name="x")
 Adds continuous variables to the program. More...
 
template<int rows, int cols>
MatrixDecisionVariable< rows, cols > NewBinaryVariables (const std::array< std::string, rows *cols > &names)
 Adds binary variables, appending them to an internal vector of any existing vars. More...
 
template<int rows, int cols>
MatrixDecisionVariable< rows, cols > NewBinaryVariables (const std::string &name="b")
 Adds a matrix of binary variables into the optimization program. More...
 
template<int rows>
VectorDecisionVariable< rows > NewBinaryVariables (const std::array< std::string, rows > &names)
 Adds vector of binary variables into the optimization program. More...
 
template<int rows>
VectorDecisionVariable< rows > NewBinaryVariables (const std::string &name="b")
 Adds vector of binary variables into the optimization program. More...
 
MatrixXDecisionVariable NewBinaryVariables (int rows, int cols, const std::vector< std::string > &names)
 Adds binary variables, appending them to an internal vector of any existing vars. More...
 
MatrixXDecisionVariable NewBinaryVariables (int rows, int cols, const std::string &name="b")
 Adds binary variables to this MathematicalProgram, with default name "b". More...
 
VectorXDecisionVariable NewBinaryVariables (int rows, const std::string &name="b")
 Adds binary variables to this MathematicalProgram. More...
 
MatrixXDecisionVariable NewSymmetricContinuousVariables (int rows, const std::vector< std::string > &names)
 Adds a symmetric matrix as decision 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...
 
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...
 
Binding< LinearCostAddLinearCost (const symbolic::Expression &e)
 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...
 
Binding< QuadraticCostAddCost (const Binding< QuadraticCost > &binding)
 Adds a cost term of the form 0.5*x'*Q*x + b'x. More...
 
Binding< QuadraticCostAddQuadraticCost (const symbolic::Expression &e)
 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...
 
Binding< PolynomialCostAddPolynomialCost (const symbolic::Expression &e)
 Adds a cost term in the polynomial form. More...
 
Binding< CostAddCost (const symbolic::Expression &e)
 Adds a cost in the symbolic form. More...
 
Binding< ConstraintAddConstraint (const Binding< Constraint > &binding)
 Adds a generic constraint to the program. More...
 
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...
 
Binding< LinearConstraintAddLinearConstraint (const symbolic::Formula &f)
 Add a linear constraint represented by a symbolic formula to the program. More...
 
template<typename Derived >
std::enable_if< detail::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...
 
Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const symbolic::Formula &f)
 Adds a linear equality constraint represented by a symbolic formula to the program. More...
 
template<typename DerivedV , typename DerivedB >
std::enable_if< detail::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< detail::is_eigen_matrix_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)
 AddLinearEqualityConstraint. More...
 
Binding< LinearEqualityConstraintAddLinearEqualityConstraint (const Eigen::Ref< const Eigen::MatrixXd > &Aeq, const Eigen::Ref< const Eigen::VectorXd > &beq, const Eigen::Ref< const VectorXDecisionVariable > &vars)
 AddLinearEqualityConstraint. More...
 
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)
 AddBoundingBoxConstraint. More...
 
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...
 
Binding< LorentzConeConstraintAddLorentzConeConstraint (const symbolic::Expression &v1, const symbolic::Expression &v2)
 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...
 
Binding< LorentzConeConstraintAddLorentzConeConstraint (const VariableRefList &vars)
 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 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...
 
Binding< RotatedLorentzConeConstraintAddRotatedLorentzConeConstraint (const VariableRefList &vars)
 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 espressions 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...
 
template<typename DerivedA , typename DerivedB >
void SetInitialGuess (const Eigen::MatrixBase< DerivedA > &decision_variable_mat, const Eigen::MatrixBase< DerivedB > &x0)
 Set the initial guess for the decision variables stored in var to be x0. More...
 
template<typename Derived >
void SetInitialGuessForAllVariables (const Eigen::MatrixBase< Derived > &x0)
 Set the intial guess for ALL decision variables. More...
 
SolutionResult Solve ()
 Solve the MathematicalProgram. More...
 
void PrintSolution ()
 
void SetDecisionVariableValues (const Eigen::Ref< const Eigen::VectorXd > &values)
 Sets the values of all decision variables, such that the value of decision_variables_(i) is values(i). More...
 
void SetDecisionVariableValues (const Eigen::Ref< const VectorXDecisionVariable > &variables, const Eigen::Ref< const Eigen::VectorXd > &values)
 Sets the value of some decision variables, such that the value of variables(i) is values(i). More...
 
void SetDecisionVariableValue (const symbolic::Variable &var, double value)
 Sets the value of a single decision variable in the optimization program. More...
 
void SetSolverOption (SolverType solver_type, const std::string &solver_option, double option_value)
 Set an option for a particular solver. More...
 
void SetSolverOption (SolverType solver_type, const std::string &solver_option, int option_value)
 
void SetSolverOption (SolverType solver_type, const std::string &solver_option, const std::string &option_value)
 
const std::map< std::string, double > & GetSolverOptionsDouble (SolverType solver_type)
 
const std::map< std::string, int > & GetSolverOptionsInt (SolverType solver_type)
 
const std::map< std::string, std::string > & GetSolverOptionsStr (SolverType solver_type)
 
void GetSolverResult (SolverType *solver_type, int *solver_result) const
 Get the name and result code of the particular solver which was used to solve this MathematicalProgram. More...
 
void SetSolverResult (SolverType solver_type, int solver_result)
 
double GetOptimalCost () const
 Getter for optimal cost at the solution. More...
 
void SetOptimalCost (double optimal_cost)
 
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< QuadraticCost > > & quadratic_costs () const
 Getter for quadratic 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< T > GetSolverData ()
 
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...
 
const Eigen::VectorXd GetSolutionVectorValues () const
 Returns the solution in a flat Eigen::VectorXd. More...
 
int FindDecisionVariableIndex (const symbolic::Variable &var) const
 Returns the index of the decision 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 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...
 
template<typename C >
Eigen::VectorXd EvalBindingAtSolution (const Binding< C > &binding) const
 Evaluate the constraint in the Binding at the solution value. 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...
 
 MathematicalProgram (const MathematicalProgram &)=delete
 
MathematicalProgramoperator= (const MathematicalProgram &)=delete
 
 MathematicalProgram (MathematicalProgram &&)=delete
 
MathematicalProgramoperator= (MathematicalProgram &&)=delete
 

Additional Inherited Members

- 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...
 

Detailed Description

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

We use a 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 satisfy the kinematics constraints, with some error.

Constructor & Destructor Documentation

GlobalInverseKinematics ( const RigidBodyTreed robot,
int  num_binary_vars_per_half_axis = 2 
)

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
robotThe robot on which the inverse kinematics problem is solved.
num_binary_vars_per_half_axisThe number of binary variables for each half axis, to segment the unit circle.
See also
AddRotationMatrixMcCormickEnvelopeMilpConstraints() for more details on num_binary_vars_per_half_axis.

Here is the call graph for this function:

~GlobalInverseKinematics ( )
inlineoverride

Here is the call graph for this function:

Member Function Documentation

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_desiredThe desired posture.
body_position_costThe 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
aruntime error if the precondition is not satisfied.
Parameters
body_orientation_costThe 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
aruntime_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_idxThe index of the body whose orientation will be constrained.
desired_orientationThe desired orientation of the body.
angle_tolThe tolerance on the angle between the body orientation and the desired orientation. Unit is radians.
Return values
bindingThe 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_idxThe index of the body on which the position of a point is constrained.
p_BQThe position of the point Q measured and expressed in the body frame B.
box_lb_FThe lower bound of the box in frame F.
box_ub_FThe 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
bindingThe 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_indexThe 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_indexThe 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_indexThe index of the body to on which point Q is attached.
p_BQThe position of point Q in the body frame B.
region_verticesregion_vertices[i] is the vertices for the i'th region.
Return values
zThe 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
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
qThe 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: