Drake
OrientationConstraint Class Reference

Detailed Description

Constrains that the angle difference θ between the orientation of frame A and the orientation of frame B to satisfy θ ≤ θ_bound.

The angle difference between frame A's orientation R_WA and B's orientation R_WB is θ (θ ∈ [0, π]), if there exists a rotation axis a, such that rotating frame A by angle θ about axis a aligns it with frame B. Namely R_AB = I + sinθ â + (1-cosθ)â² (1) where R_AB is the orientation of frame B expressed in frame A. â is the skew symmetric matrix of the rotation axis a. Equation (1) is the Rodrigues formula that computes the rotation matrix froma rotation axis a and an angle θ, https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula If the users want frame A and frame B to align perfectly, they can set θ_bound = 0. Mathematically, this constraint is imposed as trace(R_AB) ≥ 2cos(θ_bound) + 1 (1) To derive (1), using Rodrigues formula R_AB = I + sinθ â + (1-cosθ)â² where trace(R_AB) = 2cos(θ) + 1 ≥ 2cos(θ_bound) + 1

#include <drake/multibody/inverse_kinematics/orientation_constraint.h>

Public Member Functions

 OrientationConstraint (const MultibodyPlant< double > *const plant, const Frame< double > &frameAbar, const math::RotationMatrix< double > &R_AbarA, const Frame< double > &frameBbar, const math::RotationMatrix< double > &R_BbarB, double theta_bound, systems::Context< double > *context)
 Constructs an OrientationConstraint object. More...
 
 OrientationConstraint (const MultibodyPlant< AutoDiffXd > *const plant, const Frame< AutoDiffXd > &frameAbar, const math::RotationMatrix< double > &R_AbarA, const Frame< AutoDiffXd > &frameBbar, const math::RotationMatrix< double > &R_BbarB, double theta_bound, systems::Context< AutoDiffXd > *context)
 Overloaded constructor. More...
 
 ~OrientationConstraint () override
 
Does not allow copy, move, or assignment
 OrientationConstraint (const OrientationConstraint &)=delete
 
OrientationConstraintoperator= (const OrientationConstraint &)=delete
 
 OrientationConstraint (OrientationConstraint &&)=delete
 
OrientationConstraintoperator= (OrientationConstraint &&)=delete
 
- Public Member Functions inherited from Constraint
template<typename DerivedLB , typename DerivedUB >
 Constraint (int num_constraints, int num_vars, const Eigen::MatrixBase< DerivedLB > &lb, const Eigen::MatrixBase< DerivedUB > &ub, const std::string &description="")
 Constructs a constraint which has num_constraints rows, with an input num_vars x 1 vector. More...
 
 Constraint (int num_constraints, int num_vars)
 Constructs a constraint which has num_constraints rows, with an input num_vars x 1 vector, with no bounds. More...
 
bool CheckSatisfied (const Eigen::Ref< const Eigen::VectorXd > &x, double tol=1E-6) const
 Return whether this constraint is satisfied by the given value, x. More...
 
bool CheckSatisfied (const Eigen::Ref< const AutoDiffVecXd > &x, double tol=1E-6) const
 
symbolic::Formula CheckSatisfied (const Eigen::Ref< const VectorX< symbolic::Variable >> &x) const
 
const Eigen::VectorXd & lower_bound () const
 
const Eigen::VectorXd & upper_bound () const
 
int num_constraints () const
 Number of rows in the output constraint. More...
 
 Constraint (const Constraint &)=delete
 
Constraintoperator= (const Constraint &)=delete
 
 Constraint (Constraint &&)=delete
 
Constraintoperator= (Constraint &&)=delete
 
- Public Member Functions inherited from EvaluatorBase
virtual ~EvaluatorBase ()
 
void Eval (const Eigen::Ref< const Eigen::VectorXd > &x, Eigen::VectorXd *y) const
 Evaluates the expression. More...
 
void Eval (const Eigen::Ref< const AutoDiffVecXd > &x, AutoDiffVecXd *y) const
 Evaluates the expression. More...
 
void Eval (const Eigen::Ref< const VectorX< symbolic::Variable >> &x, VectorX< symbolic::Expression > *y) const
 Evaluates the expression. More...
 
void set_description (const std::string &description)
 Set a human-friendly description for the evaluator. More...
 
const std::string & get_description () const
 Getter for a human-friendly description for the evaluator. More...
 
int num_vars () const
 Getter for the number of variables, namely the number of rows in x, as used in Eval(x, y). More...
 
int num_outputs () const
 Getter for the number of outputs, namely the number of rows in y, as used in Eval(x, y). More...
 
void SetGradientSparsityPattern (const std::vector< std::pair< int, int >> &gradient_sparsity_pattern)
 Set the sparsity pattern of the gradient matrix ∂y/∂x (the gradient of y value in Eval, w.r.t x in Eval) . More...
 
const optional< std::vector< std::pair< int, int > > > & gradient_sparsity_pattern () const
 Returns the vector of (row_index, col_index) that contains all the entries in the gradient of Eval function (∂y/∂x) whose value could be non-zero, namely if ∂yᵢ/∂xⱼ could be non-zero, then the pair (i, j) is in gradient_sparsity_pattern. More...
 
 EvaluatorBase (const EvaluatorBase &)=delete
 
EvaluatorBaseoperator= (const EvaluatorBase &)=delete
 
 EvaluatorBase (EvaluatorBase &&)=delete
 
EvaluatorBaseoperator= (EvaluatorBase &&)=delete
 

Additional Inherited Members

- Protected Member Functions inherited from Constraint
void UpdateLowerBound (const Eigen::Ref< const Eigen::VectorXd > &new_lb)
 Updates the lower bound. More...
 
void UpdateUpperBound (const Eigen::Ref< const Eigen::VectorXd > &new_ub)
 Updates the upper bound. More...
 
void set_bounds (const Eigen::Ref< const Eigen::VectorXd > &lower_bound, const Eigen::Ref< const Eigen::VectorXd > &upper_bound)
 Set the upper and lower bounds of the constraint. More...
 
virtual bool DoCheckSatisfied (const Eigen::Ref< const Eigen::VectorXd > &x, const double tol) const
 
virtual bool DoCheckSatisfied (const Eigen::Ref< const AutoDiffVecXd > &x, const double tol) const
 
virtual symbolic::Formula DoCheckSatisfied (const Eigen::Ref< const VectorX< symbolic::Variable >> &x) const
 
- Protected Member Functions inherited from EvaluatorBase
 EvaluatorBase (int num_outputs, int num_vars, const std::string &description="")
 Constructs a evaluator. More...
 
void set_num_outputs (int num_outputs)
 

Constructor & Destructor Documentation

◆ OrientationConstraint() [1/4]

◆ OrientationConstraint() [2/4]

◆ OrientationConstraint() [3/4]

OrientationConstraint ( const MultibodyPlant< double > *const  plant,
const Frame< double > &  frameAbar,
const math::RotationMatrix< double > &  R_AbarA,
const Frame< double > &  frameBbar,
const math::RotationMatrix< double > &  R_BbarB,
double  theta_bound,
systems::Context< double > *  context 
)

Constructs an OrientationConstraint object.

The frame A is fixed to a frame A̅, with orientatation R_AbarA measured in frame A̅. The frame B is fixed to a frame B̅, with orientation R_BbarB measured in frame B. We constrain the angle between frame A and B to be less than theta_bound.

Parameters
plantThe MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.
frameAbarThe frame A̅ in the model to which frame A is fixed.
R_AbarAThe orientation of frame A measured in frame A̅.
frameBbarThe frame B̅ in the model to which frame B is fixed.
R_BbarBThe orientation of frame B measured in frame B̅.
theta_boundThe bound on the angle difference between frame A's orientation and frame B's orientation. It is denoted as θ_bound in the class documentation. theta_bound is in radians.
contextThe Context that has been allocated for this tree. We will update the context when evaluating the constraint. context should be alive during the lifetime of this constraint.
Exceptions
std::invalid_argumentif plant is nullptr.
std::logic_errorif frameAbar or frameBbar does not belong to plant.
std::invalid_argumentif angle_bound < 0.
std::invalid_argumentif context is nullptr.

◆ OrientationConstraint() [4/4]

OrientationConstraint ( const MultibodyPlant< AutoDiffXd > *const  plant,
const Frame< AutoDiffXd > &  frameAbar,
const math::RotationMatrix< double > &  R_AbarA,
const Frame< AutoDiffXd > &  frameBbar,
const math::RotationMatrix< double > &  R_BbarB,
double  theta_bound,
systems::Context< AutoDiffXd > *  context 
)

Overloaded constructor.

Constructs the constraint using MultibodyPlant<AutoDiffXd>

◆ ~OrientationConstraint()

~OrientationConstraint ( )
override

Member Function Documentation

◆ operator=() [1/2]

OrientationConstraint& operator= ( const OrientationConstraint )
delete

◆ operator=() [2/2]

OrientationConstraint& operator= ( OrientationConstraint &&  )
delete

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