Drake
Drake C++ Documentation
QuaternionEulerIntegrationConstraint Class Referencefinal

## Detailed Description

If we have a body with orientation quaternion z₁ at time t₁, and a quaternion z₂ at time t₂ = t₁ + h, with the angular velocity ω (expressed in the world frame), we impose the constraint that the body rotates at a constant velocity ω from quaternion z₁ to quaternion z₂ within time interval h.

Namely we want to enforce the relationship that z₂ and Δz⊗z₁ represent the same orientation, where Δz is the quaternion [cos(|ω|h/2), ω/|ω|*sin(|ω|h/2)], and ⊗ is the Hamiltonian product between quaternions.

It is well-known that for any quaternion z, its element-wise negation -z correspond to the same rotation matrix as z does. One way to understand this is that -z represents the rotation that first rotate the frame by a quaternion z, and then continue to rotate about that axis for 360 degrees. We provide the option allow_quaternion_negation flag, that if set to true, then we require that the quaternion z₂ = ±Δz⊗z₁. Otherwise we require z₂ = Δz⊗z₁. Mathematically, the constraint we impose is

If allow_quaternion_negation = true:

(z₂ • (Δz⊗z₁))² = 1


else

z₂ • (Δz⊗z₁) = 1


If your robot link orientation only changes slightly, and you are free to search for both z₁ and z₂, then we would recommend to set allow_quaternion_negation to false, as the left hand side of constraint z₂ • (Δz⊗z₁) = 1 is less nonlinear than the left hand side of (z₂ • (Δz⊗z₁))² = 1.

The operation • is the dot product between two quaternions, which computes the cosine of the half angle between these two orientations. Dot product equals to ±1 means that angle between the two quaternions are 2kπ, hence they represent the same orientation.

Note
The constraint is not differentiable at ω=0 (due to the non-differentiability of |ω| at ω = 0). So it is better to initialize the angular velocity to a non-zero value in the optimization.

The decision variables of this constraint are [z₁, z₂, ω, h]

Note
We need to evaluate sin(|ω|h/2)/|ω|, when h is huge (larger than 1/machine_epsilon), and |ω| is tiny (less than machine epsilon), this evaluation is inaccurate. So don't use this constraint if you have a huge h (which would be bad practice in trajectory optimization anyway).

#include <drake/multibody/optimization/quaternion_integration_constraint.h>

## Public Member Functions

QuaternionEulerIntegrationConstraint (bool allow_quaternion_negation)

~QuaternionEulerIntegrationConstraint () override

template<typename T >
Eigen::Matrix< T, 12, 1 > ComposeVariable (const Eigen::Ref< const Vector4< T >> &quat1, const Eigen::Ref< const Vector4< T >> &quat2, const Eigen::Ref< const Vector3< T >> &angular_vel, const T &h) const

bool allow_quaternion_negation () const

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

QuaternionEulerIntegrationConstraintoperator= (const QuaternionEulerIntegrationConstraint &)=delete

QuaternionEulerIntegrationConstraint (QuaternionEulerIntegrationConstraint &&)=delete

QuaternionEulerIntegrationConstraintoperator= (QuaternionEulerIntegrationConstraint &&)=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...

std::ostream & Display (std::ostream &os, const VectorX< symbolic::Variable > &vars) const
Formats this evaluator into the given stream using vars for the bound decision variable names. More...

std::ostream & Display (std::ostream &os) const
Formats this evaluator into the given stream, without displaying the decision variables it is bound to. More...

std::string ToLatex (const VectorX< symbolic::Variable > &vars, int precision=3) const
Returns a LaTeX string describing this 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...

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 std::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

Protected Member Functions inherited from Constraint
void UpdateLowerBound (const Eigen::Ref< const Eigen::VectorXd > &new_lb)

void UpdateUpperBound (const Eigen::Ref< const Eigen::VectorXd > &new_ub)

void set_bounds (const Eigen::Ref< const Eigen::VectorXd > &new_lb, const Eigen::Ref< const Eigen::VectorXd > &new_ub)
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...

virtual std::ostream & DoDisplay (std::ostream &os, const VectorX< symbolic::Variable > &vars) const
NVI implementation of Display. More...

virtual std::string DoToLatex (const VectorX< symbolic::Variable > &vars, int precision) const

void set_num_outputs (int num_outputs)

## ◆ QuaternionEulerIntegrationConstraint() [1/3]

 QuaternionEulerIntegrationConstraint ( const QuaternionEulerIntegrationConstraint & )
delete

## ◆ QuaternionEulerIntegrationConstraint() [2/3]

 QuaternionEulerIntegrationConstraint ( QuaternionEulerIntegrationConstraint && )
delete

## ◆ QuaternionEulerIntegrationConstraint() [3/3]

 QuaternionEulerIntegrationConstraint ( bool allow_quaternion_negation )
explicit
Parameters
 allow_quaternion_negation. Refer to the class documentation.

## ◆ ~QuaternionEulerIntegrationConstraint()

 ~QuaternionEulerIntegrationConstraint ( )
override

## ◆ allow_quaternion_negation()

 bool allow_quaternion_negation ( ) const

## ◆ ComposeVariable()

 Eigen::Matrix ComposeVariable ( const Eigen::Ref< const Vector4< T >> & quat1, const Eigen::Ref< const Vector4< T >> & quat2, const Eigen::Ref< const Vector3< T >> & angular_vel, const T & h ) const

## ◆ operator=() [1/2]

 QuaternionEulerIntegrationConstraint& operator= ( const QuaternionEulerIntegrationConstraint & )
delete

## ◆ operator=() [2/2]

 QuaternionEulerIntegrationConstraint& operator= ( QuaternionEulerIntegrationConstraint && )
delete

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