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).
|
| | 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 |
| |
|
| | QuaternionEulerIntegrationConstraint (const QuaternionEulerIntegrationConstraint &)=delete |
| |
| QuaternionEulerIntegrationConstraint & | operator= (const QuaternionEulerIntegrationConstraint &)=delete |
| |
| | QuaternionEulerIntegrationConstraint (QuaternionEulerIntegrationConstraint &&)=delete |
| |
| QuaternionEulerIntegrationConstraint & | operator= (QuaternionEulerIntegrationConstraint &&)=delete |
| |
| 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 |
| |
| Constraint & | operator= (const Constraint &)=delete |
| |
| | Constraint (Constraint &&)=delete |
| |
| Constraint & | operator= (Constraint &&)=delete |
| |
| 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...
|
| |
| 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 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 |
| |
| EvaluatorBase & | operator= (const EvaluatorBase &)=delete |
| |
| | EvaluatorBase (EvaluatorBase &&)=delete |
| |
| EvaluatorBase & | operator= (EvaluatorBase &&)=delete |
| |
Public Member Functions inherited from Constraint