Modelica.Mechanics.MultiBody.Frames.Quaternions

Information

Package Frames.Quaternions contains type definitions and functions to transform rotational frame quantities with quaternions. Functions of this package are currently only utilized in MultiBody.Parts.Body components, when quaternions shall be used as parts of the body states. Some functions are also used in a new Modelica package for B-Spline interpolation that is able to interpolate paths consisting of position vectors and orientation objects.

Content

In the table below an example is given for every function definition. The used variables have the following declaration:

   Quaternions.Orientation Q, Q1, Q2, Q_rel, Q_inv;
   Real[3,3]   T, T_inv;
   Real[3]     v1, v2, w1, w2, n_x, n_y, n_z, res_ori, phi;
   Real[6]     res_equal;
   Real        L, angle;

Extends from Modelica.Icons.Package (Icon for standard packages).

Package Content

Name	Description
Orientation	Orientation type defining rotation from a frame 1 into a frame 2 with quaternions {p1,p2,p3,p0}
der_Orientation	First time derivative of Quaternions.Orientation
orientationConstraint	Return residues of orientation constraints (shall be zero)
angularVelocity1	Compute angular velocity resolved in frame 1 from quaternion orientation object and its derivative
angularVelocity2	Compute angular velocity resolved in frame 2 from quaternions orientation object and its derivative
resolve1	Transform vector from frame 2 to frame 1
resolve2	Transform vector from frame 1 to frame 2
multipleResolve1	Transform several vectors from frame 2 to frame 1
multipleResolve2	Transform several vectors from frame 1 to frame 2
nullRotation	Return quaternion orientation object that does not rotate a frame
inverseRotation	Return inverse quaternions orientation object
relativeRotation	Return relative quaternions orientation object
absoluteRotation	Return absolute quaternions orientation object from another absolute and a relative quaternions orientation object
planarRotation	Return quaternion orientation object of a planar rotation
smallRotation	Return rotation angles valid for a small rotation
from_T	Return quaternion orientation object Q from transformation matrix T
from_T_inv	Return quaternion orientation object Q from inverse transformation matrix T_inv
to_T	Return transformation matrix T from quaternion orientation object Q
to_T_inv	Return inverse transformation matrix T_inv from quaternion orientation object Q

Types and constants

  type Orientation 
  "Orientation type defining rotation from a frame 1 into a frame 2 with quaternions {p1,p2,p3,p0}"

    extends Internal.QuaternionBase;

    encapsulated function equalityConstraint 
    "Return the constraint residues to express that two frames have the same quaternion orientation"

      import Modelica;
      import Modelica.Mechanics.MultiBody.Frames.Quaternions;
      extends Modelica.Icons.Function;
      input Quaternions.Orientation Q1 
      "Quaternions orientation object to rotate frame 0 into frame 1";
      input Quaternions.Orientation Q2 
      "Quaternions orientation object to rotate frame 0 into frame 2";
      output Real residue[3] 
      "The half of the rotation angles around x-, y-, and z-axis of frame 1 to rotate frame 1 into frame 2 for a small rotation (shall be zero)";
    algorithm 
      residue := [Q1[4], Q1[3], -Q1[2], -Q1[1]; -Q1[3], Q1[4], Q1[1], -Q1[2];
         Q1[2], -Q1[1], Q1[4], -Q1[3]]*Q2;
    end equalityConstraint;

  end Orientation;

  type der_Orientation = Real[4] (each unit="1/s") 
  "First time derivative of Quaternions.Orientation";

Modelica.Mechanics.MultiBody.Frames.Quaternions.orientationConstraint

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2

Outputs

Type	Name	Description
Real	residue[1]	Residue constraint (shall be zero)

Modelica definition

function orientationConstraint 
  "Return residues of orientation constraints (shall be zero)"
  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  output Real residue[1] "Residue constraint (shall be zero)";
algorithm 
  residue := {Q*Q - 1};
end orientationConstraint;

Modelica.Mechanics.MultiBody.Frames.Quaternions.angularVelocity1

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2
der_Orientation	der_Q		Derivative of Q [1/s]

Outputs

Type	Name	Description
AngularVelocity	w[3]	Angular velocity resolved in frame 1 [rad/s]

Modelica definition

function angularVelocity1 
  "Compute angular velocity resolved in frame 1 from quaternion orientation object and its derivative"

  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  input der_Orientation der_Q "Derivative of Q";
  output Modelica.SIunits.AngularVelocity w[3] 
    "Angular velocity resolved in frame 1";
algorithm 
  w := 2*([Q[4], -Q[3], Q[2], -Q[1]; Q[3], Q[4], -Q[1], -Q[2]; -Q[2], Q[1],
     Q[4], -Q[3]]*der_Q);
end angularVelocity1;

Modelica.Mechanics.MultiBody.Frames.Quaternions.angularVelocity2

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2
der_Orientation	der_Q		Derivative of Q [1/s]

Outputs

Type	Name	Description
AngularVelocity	w[3]	Angular velocity of frame 2 with respect to frame 1 resolved in frame 2 [rad/s]

Modelica definition

function angularVelocity2 
  "Compute angular velocity resolved in frame 2 from quaternions orientation object and its derivative"

  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  input der_Orientation der_Q "Derivative of Q";
  output Modelica.SIunits.AngularVelocity w[3] 
    "Angular velocity of frame 2 with respect to frame 1 resolved in frame 2";
algorithm 
  w := 2*([Q[4], Q[3], -Q[2], -Q[1]; -Q[3], Q[4], Q[1], -Q[2]; Q[2], -Q[1],
     Q[4], -Q[3]]*der_Q);
end angularVelocity2;

Modelica.Mechanics.MultiBody.Frames.Quaternions.resolve1

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2
Real	v2[3]		Vector in frame 2

Outputs

Type	Name	Description
Real	v1[3]	Vector in frame 1

Modelica definition

function resolve1 "Transform vector from frame 2 to frame 1"
  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  input Real v2[3] "Vector in frame 2";
  output Real v1[3] "Vector in frame 1";
algorithm 
  v1 := 2*((Q[4]*Q[4] - 0.5)*v2 + (Q[1:3]*v2)*Q[1:3] + Q[4]*cross(Q[1:3],
    v2));
end resolve1;

Modelica.Mechanics.MultiBody.Frames.Quaternions.resolve2

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2
Real	v1[3]		Vector in frame 1

Outputs

Type	Name	Description
Real	v2[3]	Vector in frame 2

Modelica definition

function resolve2 "Transform vector from frame 1 to frame 2"
  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  input Real v1[3] "Vector in frame 1";
  output Real v2[3] "Vector in frame 2";
algorithm 
  v2 := 2*((Q[4]*Q[4] - 0.5)*v1 + (Q[1:3]*v1)*Q[1:3] - Q[4]*cross(Q[1:3],
    v1));
end resolve2;

Modelica.Mechanics.MultiBody.Frames.Quaternions.multipleResolve1

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2
Real	v2[3, :]		Vectors in frame 2

Outputs

Type	Name	Description
Real	v1[3, size(v2, 2)]	Vectors in frame 1

Modelica definition

function multipleResolve1 
  "Transform several vectors from frame 2 to frame 1"
  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  input Real v2[3, :] "Vectors in frame 2";
  output Real v1[3, size(v2, 2)] "Vectors in frame 1";
algorithm 
  v1 := ((2*Q[4]*Q[4] - 1)*identity(3) + 2*([Q[1:3]]*transpose([Q[1:3]]) +
    Q[4]*skew(Q[1:3])))*v2;
end multipleResolve1;

Modelica.Mechanics.MultiBody.Frames.Quaternions.multipleResolve2

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2
Real	v1[3, :]		Vectors in frame 1

Outputs

Type	Name	Description
Real	v2[3, size(v1, 2)]	Vectors in frame 2

Modelica definition

function multipleResolve2 
  "Transform several vectors from frame 1 to frame 2"
  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  input Real v1[3, :] "Vectors in frame 1";
  output Real v2[3, size(v1, 2)] "Vectors in frame 2";
algorithm 
  v2 := ((2*Q[4]*Q[4] - 1)*identity(3) + 2*([Q[1:3]]*transpose([Q[1:3]]) -
    Q[4]*skew(Q[1:3])))*v1;
end multipleResolve2;

Modelica.Mechanics.MultiBody.Frames.Quaternions.nullRotation

Information

Outputs

Type	Name	Description
Orientation	Q	Quaternions orientation object to rotate frame 1 into frame 2

Modelica definition

function nullRotation 
  "Return quaternion orientation object that does not rotate a frame"

  extends Modelica.Icons.Function;
  output Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
algorithm 
  Q := {0,0,0,1};
end nullRotation;

Modelica.Mechanics.MultiBody.Frames.Quaternions.inverseRotation

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2

Outputs

Type	Name	Description
Orientation	Q_inv	Quaternions orientation object to rotate frame 2 into frame 1

Modelica definition

function inverseRotation 
  "Return inverse quaternions orientation object"
  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  output Quaternions.Orientation Q_inv 
    "Quaternions orientation object to rotate frame 2 into frame 1";
algorithm 
  Q_inv := {-Q[1],-Q[2],-Q[3],Q[4]};
end inverseRotation;

Modelica.Mechanics.MultiBody.Frames.Quaternions.relativeRotation

Information

Inputs

Type	Name	Default	Description
Orientation	Q1		Quaternions orientation object to rotate frame 0 into frame 1
Orientation	Q2		Quaternions orientation object to rotate frame 0 into frame 2

Outputs

Type	Name	Description
Orientation	Q_rel	Quaternions orientation object to rotate frame 1 into frame 2

Modelica definition

function relativeRotation 
  "Return relative quaternions orientation object"
  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q1 
    "Quaternions orientation object to rotate frame 0 into frame 1";
  input Quaternions.Orientation Q2 
    "Quaternions orientation object to rotate frame 0 into frame 2";
  output Quaternions.Orientation Q_rel 
    "Quaternions orientation object to rotate frame 1 into frame 2";
algorithm 
  Q_rel := [Q1[4], Q1[3], -Q1[2], -Q1[1]; -Q1[3], Q1[4], Q1[1], -Q1[2]; Q1[
    2], -Q1[1], Q1[4], -Q1[3]; Q1[1], Q1[2], Q1[3], Q1[4]]*Q2;
end relativeRotation;

Modelica.Mechanics.MultiBody.Frames.Quaternions.absoluteRotation

Information

Inputs

Type	Name	Default	Description
Orientation	Q1		Quaternions orientation object to rotate frame 0 into frame 1
Orientation	Q_rel		Quaternions orientation object to rotate frame 1 into frame 2

Outputs

Type	Name	Description
Orientation	Q2	Quaternions orientation object to rotate frame 0 into frame 2

Modelica definition

function absoluteRotation 
  "Return absolute quaternions orientation object from another absolute and a relative quaternions orientation object"

  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q1 
    "Quaternions orientation object to rotate frame 0 into frame 1";
  input Quaternions.Orientation Q_rel 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  output Quaternions.Orientation Q2 
    "Quaternions orientation object to rotate frame 0 into frame 2";
algorithm 
  Q2 := [Q_rel[4], Q_rel[3], -Q_rel[2], Q_rel[1]; -Q_rel[3], Q_rel[4],
    Q_rel[1], Q_rel[2]; Q_rel[2], -Q_rel[1], Q_rel[4], Q_rel[3]; -Q_rel[1],
     -Q_rel[2], -Q_rel[3], Q_rel[4]]*Q1;
end absoluteRotation;

Modelica.Mechanics.MultiBody.Frames.Quaternions.planarRotation

Information

Inputs

Type	Name	Default	Description
Real	e[3]		Normalized axis of rotation (must have length=1) [1]
Angle	angle		Rotation angle to rotate frame 1 into frame 2 along axis e [rad]

Outputs

Type	Name	Description
Orientation	Q	Quaternions orientation object to rotate frame 1 into frame 2 along axis e

Modelica definition

function planarRotation 
  "Return quaternion orientation object of a planar rotation"
  import Modelica.Math;
  extends Modelica.Icons.Function;
  input Real e[3](each final unit="1") 
    "Normalized axis of rotation (must have length=1)";
  input Modelica.SIunits.Angle angle 
    "Rotation angle to rotate frame 1 into frame 2 along axis e";
  output Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2 along axis e";
algorithm 
  Q := vector([e*Math.sin(angle/2); Math.cos(angle/2)]);
end planarRotation;

Modelica.Mechanics.MultiBody.Frames.Quaternions.smallRotation

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2

Outputs

Type	Name	Description
Angle	phi[3]	The rotation angles around x-, y-, and z-axis of frame 1 to rotate frame 1 into frame 2 for a small relative rotation [rad]

Modelica definition

function smallRotation 
  "Return rotation angles valid for a small rotation"
  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  output Modelica.SIunits.Angle phi[3] 
    "The rotation angles around x-, y-, and z-axis of frame 1 to rotate frame 1 into frame 2 for a small relative rotation";
algorithm 
  phi := 2*{Q[1],Q[2],Q[3]};
end smallRotation;

Modelica.Mechanics.MultiBody.Frames.Quaternions.from_T

Information

Inputs

Type	Name	Default	Description
Real	T[3, 3]		Transformation matrix to transform vector from frame 1 to frame 2 (v2=T*v1)
Orientation	Q_guess	nullRotation()	Guess value for Q (there are 2 solutions; the one close to Q_guess is used

Outputs

Type	Name	Description
Orientation	Q	Quaternions orientation object to rotate frame 1 into frame 2 (Q and -Q have same transformation matrix)

Modelica definition

function from_T 
  "Return quaternion orientation object Q from transformation matrix T"

  extends Modelica.Icons.Function;
  input Real T[3, 3] 
    "Transformation matrix to transform vector from frame 1 to frame 2 (v2=T*v1)";
  input Quaternions.Orientation Q_guess=nullRotation() 
    "Guess value for Q (there are 2 solutions; the one close to Q_guess is used";
  output Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2 (Q and -Q have same transformation matrix)";
protected 
  Real paux;
  Real paux4;
  Real c1;
  Real c2;
  Real c3;
  Real c4;
  constant Real p4limit=0.1;
  constant Real c4limit=4*p4limit*p4limit;
algorithm 
  /*
   Note, for quaternions, Q and -Q have the same transformation matrix.
   Calculation of quaternions from transformation matrix T:
   It is guaranteed that c1>=0, c2>=0, c3>=0, c4>=0 and
   that not all of them can be zero at the same time
   (e.g., if 3 of them are zero, the 4th variable is 1).
   Since the sqrt(..) has to be performed on one of these variables,
   it is applied on a variable which is far enough from zero.
   This guarantees that the sqrt(..) is never taken near zero
   and therefore the derivative of sqrt(..) can never be infinity.
   There is an ambiguity for quaternions, since Q and -Q
   lead to the same transformation matrix. The ambiguity
   is resolved here by selecting the Q that is closer to
   the input argument Q_guess.
*/
  c1 := 1 + T[1, 1] - T[2, 2] - T[3, 3];
  c2 := 1 + T[2, 2] - T[1, 1] - T[3, 3];
  c3 := 1 + T[3, 3] - T[1, 1] - T[2, 2];
  c4 := 1 + T[1, 1] + T[2, 2] + T[3, 3];

  if c4 > c4limit or (c4 > c1 and c4 > c2 and c4 > c3) then
    paux := sqrt(c4)/2;
    paux4 := 4*paux;
    Q := {(T[2, 3] - T[3, 2])/paux4,(T[3, 1] - T[1, 3])/paux4,(T[1, 2] - T[
      2, 1])/paux4,paux};

  elseif c1 > c2 and c1 > c3 and c1 > c4 then
    paux := sqrt(c1)/2;
    paux4 := 4*paux;
    Q := {paux,(T[1, 2] + T[2, 1])/paux4,(T[1, 3] + T[3, 1])/paux4,(T[2, 3]
       - T[3, 2])/paux4};

  elseif c2 > c1 and c2 > c3 and c2 > c4 then
    paux := sqrt(c2)/2;
    paux4 := 4*paux;
    Q := {(T[1, 2] + T[2, 1])/paux4,paux,(T[2, 3] + T[3, 2])/paux4,(T[3, 1]
       - T[1, 3])/paux4};

  else
    paux := sqrt(c3)/2;
    paux4 := 4*paux;
    Q := {(T[1, 3] + T[3, 1])/paux4,(T[2, 3] + T[3, 2])/paux4,paux,(T[1, 2]
       - T[2, 1])/paux4};
  end if;

  if Q*Q_guess < 0 then
    Q := -Q;
  end if;
end from_T;

Modelica.Mechanics.MultiBody.Frames.Quaternions.from_T_inv

Information

Inputs

Type	Name	Default	Description
Real	T_inv[3, 3]		Inverse transformation matrix to transform vector from frame 2 to frame 1 (v1=T_inv*v2)
Orientation	Q_guess	nullRotation()	Guess value for output Q (there are 2 solutions; the one closer to Q_guess is used

Outputs

Type	Name	Description
Orientation	Q	Quaternions orientation object to rotate frame 1 into frame 2 (Q and -Q have same transformation matrix)

Modelica definition

function from_T_inv 
  "Return quaternion orientation object Q from inverse transformation matrix T_inv"

  extends Modelica.Icons.Function;
  input Real T_inv[3, 3] 
    "Inverse transformation matrix to transform vector from frame 2 to frame 1 (v1=T_inv*v2)";
  input Quaternions.Orientation Q_guess=nullRotation() 
    "Guess value for output Q (there are 2 solutions; the one closer to Q_guess is used";
  output Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2 (Q and -Q have same transformation matrix)";
algorithm 
  Q := from_T(transpose(T_inv), Q_guess);
end from_T_inv;

Modelica.Mechanics.MultiBody.Frames.Quaternions.to_T

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2

Outputs

Type	Name	Description
Real	T[3, 3]	Transformation matrix to transform vector from frame 1 to frame 2 (v2=T*v1)

Modelica definition

function to_T 
  "Return transformation matrix T from quaternion orientation object Q"

  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  output Real T[3, 3] 
    "Transformation matrix to transform vector from frame 1 to frame 2 (v2=T*v1)";
algorithm 
  /*
  T := (2*Q[4]*Q[4] - 1)*identity(3) + 2*([Q[1:3]]*transpose([Q[1:3]]) - Q[4]*
    skew(Q[1:3]));
*/
  T := [2*(Q[1]*Q[1] + Q[4]*Q[4]) - 1, 2*(Q[1]*Q[2] + Q[3]*Q[4]), 2*(Q[1]*Q[
    3] - Q[2]*Q[4]); 2*(Q[2]*Q[1] - Q[3]*Q[4]), 2*(Q[2]*Q[2] + Q[4]*Q[4])
     - 1, 2*(Q[2]*Q[3] + Q[1]*Q[4]); 2*(Q[3]*Q[1] + Q[2]*Q[4]), 2*(Q[3]*Q[2]
     - Q[1]*Q[4]), 2*(Q[3]*Q[3] + Q[4]*Q[4]) - 1];
end to_T;

Modelica.Mechanics.MultiBody.Frames.Quaternions.to_T_inv

Information

Inputs

Type	Name	Default	Description
Orientation	Q		Quaternions orientation object to rotate frame 1 into frame 2

Outputs

Type	Name	Description
Real	T_inv[3, 3]	Transformation matrix to transform vector from frame 2 to frame 1 (v1=T*v2)

Modelica definition

function to_T_inv 
  "Return inverse transformation matrix T_inv from quaternion orientation object Q"

  extends Modelica.Icons.Function;
  input Quaternions.Orientation Q 
    "Quaternions orientation object to rotate frame 1 into frame 2";
  output Real T_inv[3, 3] 
    "Transformation matrix to transform vector from frame 2 to frame 1 (v1=T*v2)";
algorithm 
  /*
  T_inv := (2*Q[4]*Q[4] - 1)*identity(3) + 2*([Q[1:3]]*transpose([Q[1:3]]) + Q[
    4]*skew(Q[1:3]));
*/
  T_inv := [2*(Q[1]*Q[1] + Q[4]*Q[4]) - 1, 2*(Q[2]*Q[1] - Q[3]*Q[4]), 2*(Q[
    3]*Q[1] + Q[2]*Q[4]); 2*(Q[1]*Q[2] + Q[3]*Q[4]), 2*(Q[2]*Q[2] + Q[4]*Q[
    4]) - 1, 2*(Q[3]*Q[2] - Q[1]*Q[4]); 2*(Q[1]*Q[3] - Q[2]*Q[4]), 2*(Q[2]*
    Q[3] + Q[1]*Q[4]), 2*(Q[3]*Q[3] + Q[4]*Q[4]) - 1];
end to_T_inv;