model AbsoluteAngles "Measure absolute angles between frame connector and the world frame"
extends Internal.PartialAbsoluteSensor;
Modelica.Blocks.Interfaces.RealOutput angles[3](each final quantity = "Angle", each final unit = "rad", each displayUnit = "deg") "Angles to rotate world frame into frame_a via 'sequence'"
annotation (Placement(transformation(
origin = {110, 0},
extent = {
{-10, -10},
{10, 10}})));
parameter MultiBody.Types.RotationSequence sequence(min = {1, 1, 1}, max = {3, 3, 3}) = {1, 2, 3} "Angles are returned to rotate world frame around axes sequence[1], sequence[2] and finally sequence[3] into frame_a"
annotation (Evaluate = true);
parameter SI.Angle guessAngle1 = 0 "Select angles[1] such that abs(angles[1] - guessAngle1) is a minimum";
equation
angles = MultiBody.Frames.axesRotationsAngles(frame_a.R, sequence, guessAngle1);
frame_a.f = zeros(3);
frame_a.t = zeros(3);
annotation (
Icon(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Text(
extent = {
{-132, 76},
{129, 124}},
textString = "%name",
lineColor = {0, 0, 255}),
Line(
points = {
{70, 0},
{100, 0}},
color = {0, 0, 127}),
Text(
extent = {
{62, -22},
{172, -44}},
textString = "angles")}),
Documentation(info = "<html>\n<p>\nThis model determines the 3 angles to rotate the world frame\ninto frame_a along the axes defined by parameter <strong>sequence</strong>.\nFor example, if sequence = {3,1,2} then the world frame is\nrotated around angles[1] along the z-axis, afterwards it is rotated\naround angles[2] along the x-axis, and finally it is rotated around\nangles[3] along the y-axis and is then identical to frame_a.\nThe 3 angles are returned in the range\n</p>\n<pre>\n -<font face=\"Symbol\">p</font> <= angles[i] <= <font face=\"Symbol\">p</font>\n</pre>\n<p>\nThere are <strong>two solutions</strong> for \"angles[1]\" in this range.\nVia parameter <strong>guessAngle1</strong> (default = 0) the\nreturned solution is selected such that |angles[1] - guessAngle1| is\nminimal. The transformation matrix between the world frame and\nframe_a may be in a singular configuration with respect to \"sequence\", i.e.,\nthere is an infinite number of angle values leading to the same relative\ntransformation matrix. In this case, the returned solution is\nselected by setting angles[1] = guessAngle1. Then angles[2]\nand angles[3] can be uniquely determined in the above range.\n</p>\n<p>\nThe parameter <strong>sequence</strong> has the restriction that\nonly values 1,2,3 can be used and that sequence[1] ≠ sequence[2]\nand sequence[2] ≠ sequence[3]. Often used values are:\n</p>\n<pre>\n sequence = <strong>{1,2,3}</strong> // Cardan or Tait-Bryan angle sequence\n = <strong>{3,1,3}</strong> // Euler angle sequence\n = <strong>{3,2,1}</strong>\n</pre>\n</html>"));
end AbsoluteAngles;