record Orientation "Orientation object defining rotation from a frame 1 into a frame 2"
extends Modelica.Icons.Record;
Real T[3,3] "Transformation matrix from world frame to local frame";
SI.AngularVelocity w[3] "Absolute angular velocity of local frame, resolved in local frame";
encapsulated function equalityConstraint "Return the constraint residues to express that two frames have the same orientation"
import Modelica;
import Modelica.Mechanics.MultiBody.Frames;
extends Modelica.Icons.Function;
input Frames.Orientation R1 "Orientation object to rotate frame 0 into frame 1";
input Frames.Orientation R2 "Orientation object to rotate frame 0 into frame 2";
output Real residue[3] "The rotation angles around x-, y-, and z-axis of frame 1 to rotate frame 1 into frame 2 for a small rotation (should be zero)";
algorithm
residue := {Modelica.Math.atan2(cross(R1.T[1,:], R1.T[2,:]) * R2.T[2,:], R1.T[1,:] * R2.T[1,:]), Modelica.Math.atan2(-cross(R1.T[1,:], R1.T[2,:]) * R2.T[1,:], R1.T[2,:] * R2.T[2,:]), Modelica.Math.atan2(R1.T[2,:] * R2.T[1,:], R1.T[3,:] * R2.T[3,:])};
annotation (
Inline = true,
Documentation(info = "<html>\n<h4>Syntax</h4>\n<blockquote><pre>\nresidue = Orientation.<strong>equalityConstraint</strong>(R1, R2);\n</pre></blockquote>\n\n<h4>Description</h4>\n<p>\nThe function call <code>Orientation.<strong>equalityConstrain</strong>(R1,R2)</code> returns the Real residue vector\nwith 3 elements. This vector has zero elements if orientation objects R1 and R2 are identical\n(= describe the same orientation). The residue vector is determined by computing the relative\norientation object between R1 and R2 and using the outer-diagonal elements of this matrix to\nformulate the residue in such a way that only identical orientation objects lead to a zero residue vector.\n</p>\n</html>"));
end equalityConstraint;
annotation (Documentation(info = "<html>\n<p>\nThis object describes the <strong>rotation</strong> from a <strong>frame 1</strong> into a <strong>frame 2</strong>.\nAn instance of this type should never be directly accessed but\nonly with the access functions provided\nin package Modelica.Mechanics.MultiBody.Frames. As a consequence, it is not necessary to know\nthe internal representation of this object as described in the next paragraphs.\n</p>\n<p>\n\"Orientation\" is defined to be a record consisting of two\nelements: \"Real T[3,3]\", the transformation matrix to rotate frame 1\ninto frame 2 and \"Real w[3]\", the angular velocity of frame 2 with\nrespect to frame 1, resolved in frame 2. Element \"T\"\nhas the following interpretation:\n</p>\n\n<blockquote><pre>\nOrientation R;\n<strong>R.T</strong> = [<strong>e</strong><sub>x</sub>, <strong>e</strong><sub>y</sub>, <strong>e</strong><sub>z</sub>];\n e.g., <strong>R.T</strong> = [1,0,0; 0,1,0; 0,0,1]\n</pre></blockquote>\n\n<p>\nwhere <strong>e</strong><sub>x</sub>,<strong>e</strong><sub>y</sub>,<strong>e</strong><sub>z</sub>\nare unit vectors in the direction of the x-axis, y-axis, and z-axis\nof frame 1, resolved in frame 2, respectively. Therefore, if <strong>v</strong><sub>1</sub>\nis vector <strong>v</strong> resolved in frame 1 and <strong>v</strong><sub>2</sub> is\nvector <strong>v</strong> resolved in frame 2, the following relationship holds:\n</p>\n\n<blockquote><pre>\n<strong>v</strong><sub>2</sub> = <strong>R.T</strong> * <strong>v</strong><sub>1</sub>\n</pre></blockquote>\n\n<p>\nThe <strong>inverse</strong> orientation\n<strong>R_inv.T</strong> = <strong>R.T</strong><sup>T</sup> describes the rotation\nfrom frame 2 into frame 1.\n</p>\n<p>\nSince the orientation is described by 9 variables, there are\n6 constraints between these variables. These constraints\nare defined in function <strong>Frames.orientationConstraint</strong>.\n</p>\n<p>\nR.w is the angular velocity of frame 2 with respect to frame 1, resolved\nin frame 2. Formally, R.w is defined as:<br>\n<strong>skew</strong>(R.w) = R.T*<strong>der</strong>(transpose(R.T))\nwith\n</p>\n<blockquote><pre>\n | 0 -w[3] w[2] |\n<strong>skew</strong>(w) = | w[3] 0 -w[1] |\n | -w[2] w[1] 0 |\n</pre></blockquote>\n</html>"));
end Orientation;