model RelativeAngles "Measure relative angles between two frame connectors"
extends Internal.PartialRelativeSensor annotation (IconMap(primitivesVisible = false));
extends Modelica.Icons.RotationalSensor;
Modelica.Blocks.Interfaces.RealOutput angles[3](each final quantity = "Angle", each final unit = "rad", each displayUnit = "deg") "Angles to rotate frame_a into frame_b via 'sequence'"
annotation (Placement(transformation(
origin = {0, -110},
extent = {
{-10, -10},
{10, 10}},
rotation = 270)));
parameter MultiBody.Types.RotationSequence sequence(min = {1, 1, 1}, max = {3, 3, 3}) = {1, 2, 3} "Angles are returned to rotate frame_a around axes sequence[1], sequence[2] and finally sequence[3] into frame_b"
annotation (Evaluate = true);
parameter SI.Angle guessAngle1 = 0 "Select angles[1] such that abs(angles[1] - guessAngle1) is a minimum";
Modelica.Mechanics.MultiBody.Frames.Orientation R_rel "Relative orientation object from frame_a to frame_b";
equation
R_rel = Modelica.Mechanics.MultiBody.Frames.relativeRotation(frame_a.R, frame_b.R);
angles = MultiBody.Frames.axesRotationsAngles(R_rel, sequence, guessAngle1);
frame_a.f = zeros(3);
frame_a.t = zeros(3);
frame_b.f = zeros(3);
frame_b.t = zeros(3);
annotation (
Icon(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Line(
points = {
{0, -70},
{0, -100}},
color = {0, 0, 127}),
Line(points = {
{-70, 0},
{-96, 0},
{-96, 0}}),
Line(points = {
{96, 0},
{70, 0},
{70, 0}}),
Text(
extent = {
{-132, 90},
{129, 138}},
textString = "%name",
lineColor = {0, 0, 255}),
Text(
extent = {
{0, -76},
{110, -98}},
textString = "angles"),
Text(
extent = {
{-108, 43},
{-72, 18}},
lineColor = {128, 128, 128},
textString = "a"),
Text(
extent = {
{72, 43},
{108, 18}},
lineColor = {128, 128, 128},
textString = "b")}),
Documentation(info = "<html>\n<p>\nThis model determines the 3 angles to rotate frame_a\ninto frame_b along the axes defined by parameter <strong>sequence</strong>.\nFor example, if sequence = {3,1,2} then frame_a 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_b.\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 relative transformation matrix between frame_a and\nframe_b 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 RelativeAngles;