RelativeSensor

model RelativeSensor "Measure relative kinematic quantities between two frame connectors"
    extends Modelica.Mechanics.MultiBody.Sensors.Internal.PartialRelativeSensor;

    Interfaces.Frame_resolve frame_resolve if resolveInFrame == Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB.frame_resolve or resolveInFrameAfterDifferentiation == Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB.frame_resolve "If resolveInFrame = Types.ResolveInFrameAB.frame_resolve, the output signals are resolved in this frame"
        annotation (Placement(transformation(extent = {
            {84, 64}, 
            {116, 96}})));
    parameter Boolean animation = true "= true, if animation shall be enabled (show arrow)";
    parameter Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB resolveInFrame = Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB.frame_a "Frame in which vectors are resolved before differentiation (world, frame_a, frame_b, or frame_resolve)";
    parameter Boolean get_r_rel = false "= true, to measure the relative position vector from the origin of frame_a to frame_b"
        annotation (
            HideResult = true,
            choices(checkBox = true));
    parameter Boolean get_v_rel = false "= true, to measure the relative velocity of the origin of frame_b with respect to frame_a"
        annotation (
            HideResult = true,
            choices(checkBox = true));
    parameter Boolean get_a_rel = false "= true, to measure the relative acceleration of the origin of frame_b with respect to frame_a"
        annotation (
            HideResult = true,
            choices(checkBox = true));
    parameter Boolean get_w_rel = false "= true, to measure the relative angular velocity of frame_b with respect to frame_a"
        annotation (
            HideResult = true,
            choices(checkBox = true));
    parameter Boolean get_z_rel = false "= true, to measure the relative angular acceleration of frame_b with respect to frame_a"
        annotation (
            HideResult = true,
            choices(checkBox = true));
    parameter Boolean get_angles = false "= true, to measure the 3 rotation angles"
        annotation (
            HideResult = true,
            choices(checkBox = true),
            Dialog(group = "3 angles to rotate frame_a into frame_b along the axes defined in \"sequence\""));
    parameter Types.RotationSequence sequence(min = {1, 1, 1}, max = {3, 3, 3}) = {1, 2, 3} "If get_angles=true: Angles are returned to rotate frame_a around axes sequence[1], sequence[2] and finally sequence[3] into frame_b"
        annotation (
            HideResult = true,
            Evaluate = true,
            Dialog(
                group = "3 angles to rotate frame_a into frame_b along the axes defined in \"sequence\"",
                enable = get_angles));
    parameter SI.Angle guessAngle1 = 0 "If get_angles=true: Select angles[1] such that abs(angles[1] - guessAngle1) is a minimum"
        annotation (
            HideResult = true,
            Dialog(
                group = "3 angles to rotate frame_a into frame_b along the axes defined in \"sequence\"",
                enable = get_angles));
    input SI.Diameter arrowDiameter = world.defaultArrowDiameter "Diameter of relative arrow from frame_a to frame_b"
        annotation (Dialog(
            tab = "Animation",
            group = "if animation = true",
            enable = animation));
    input Types.Color arrowColor = Modelica.Mechanics.MultiBody.Types.Defaults.SensorColor "Color of relative arrow from frame_a to frame_b"
        annotation (Dialog(
            colorSelector = true,
            tab = "Animation",
            group = "if animation = true",
            enable = animation));
    input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient "Reflection of ambient light (= 0: light is completely absorbed)"
        annotation (Dialog(
            tab = "Animation",
            group = "if animation = true",
            enable = animation));
    parameter Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB resolveInFrameAfterDifferentiation = resolveInFrame "Frame in which vectors are resolved after differentiation (world, frame_a, frame_b, or frame_resolve)"
        annotation (Dialog(
            tab = "Advanced",
            group = "if get_v_rel or get_a_rel or get_z_rel",
            enable = get_v_rel or get_a_rel or get_z_rel));
    Blocks.Interfaces.RealOutput r_rel[3](each final quantity = "Length", each final unit = "m") if get_r_rel "Relative position vector frame_b.r_0 - frame_a.r_0 resolved in frame defined by resolveInFrame"
        annotation (Placement(transformation(
            origin = {-100, -110},
            extent = {
                {10, -10}, 
                {-10, 10}},
            rotation = 90)));
    Blocks.Interfaces.RealOutput v_rel[3](each final quantity = "Velocity", each final unit = "m/s") if get_v_rel "Relative velocity vector"
        annotation (Placement(transformation(
            origin = {-60, -110},
            extent = {
                {10, -10}, 
                {-10, 10}},
            rotation = 90)));
    Blocks.Interfaces.RealOutput a_rel[3](each final quantity = "Acceleration", each final unit = "m/s2") if get_a_rel "Relative acceleration vector"
        annotation (Placement(transformation(
            origin = {-20, -110},
            extent = {
                {10, -10}, 
                {-10, 10}},
            rotation = 90)));
    Blocks.Interfaces.RealOutput angles[3](each final quantity = "Angle", each final unit = "rad", each displayUnit = "deg") if get_angles "Angles to rotate frame_a into frame_b via 'sequence'"
        annotation (Placement(transformation(
            origin = {20, -110},
            extent = {
                {10, -10}, 
                {-10, 10}},
            rotation = 90)));
    Blocks.Interfaces.RealOutput w_rel[3](each final quantity = "AngularVelocity", each final unit = "1/s") if get_w_rel "Relative angular velocity vector"
        annotation (Placement(transformation(
            origin = {60, -110},
            extent = {
                {10, -10}, 
                {-10, 10}},
            rotation = 90)));
    Blocks.Interfaces.RealOutput z_rel[3](each final quantity = "AngularAcceleration", each final unit = "1/s2") if get_z_rel "Relative angular acceleration vector"
        annotation (Placement(transformation(
            origin = {100, -110},
            extent = {
                {10, -10}, 
                {-10, 10}},
            rotation = 90)));
protected
    RelativePosition relativePosition(resolveInFrame = resolveInFrame) if get_r_rel or get_v_rel or get_a_rel annotation (Placement(transformation(extent = {
        {-80, -10}, 
        {-60, 10}})));
    Blocks.Continuous.Der der1[3] if get_v_rel or get_a_rel annotation (Placement(transformation(
        extent = {
            {-10, -10}, 
            {0, 0}},
        rotation = -90,
        origin = {-55, -30})));
    Blocks.Continuous.Der der2[3] if get_a_rel annotation (Placement(transformation(
        extent = {
            {0, 0}, 
            {10, 10}},
        rotation = -90,
        origin = {-25, -40})));
    Modelica.Mechanics.MultiBody.Sensors.RelativeAngles relativeAngles(sequence = sequence, guessAngle1 = guessAngle1) if get_angles annotation (Placement(transformation(extent = {
        {20, -25}, 
        {40, -5}})));
    RelativeAngularVelocity relativeAngularVelocity(resolveInFrame = resolveInFrame) if get_w_rel or get_z_rel annotation (Placement(transformation(extent = {
        {50, -40}, 
        {70, -20}})));
    Blocks.Continuous.Der der3[3] if get_z_rel annotation (Placement(transformation(
        extent = {
            {-10, -10}, 
            {0, 0}},
        rotation = -90,
        origin = {95, -58})));
    Internal.ZeroForceAndTorque zeroForce1 annotation (Placement(transformation(extent = {
        {-80, 40}, 
        {-60, 60}})));
    Internal.ZeroForceAndTorque zeroForce2 annotation (Placement(transformation(extent = {
        {80, 40}, 
        {60, 60}})));
    Internal.ZeroForceAndTorque zeroForce3 if resolveInFrame == Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB.frame_resolve annotation (Placement(transformation(extent = {
        {40, 70}, 
        {20, 90}})));
    Modelica.Mechanics.MultiBody.Sensors.TransformRelativeVector transformVector_v_rel(frame_r_in = resolveInFrame, frame_r_out = resolveInFrameAfterDifferentiation) if get_v_rel annotation (Placement(transformation(extent = {
        {-70, -64}, 
        {-50, -44}})));
    Modelica.Mechanics.MultiBody.Sensors.TransformRelativeVector transformVector_a_rel(frame_r_in = resolveInFrame, frame_r_out = resolveInFrameAfterDifferentiation) if get_a_rel annotation (Placement(transformation(extent = {
        {-30, -78}, 
        {-10, -58}})));
    Modelica.Mechanics.MultiBody.Sensors.TransformRelativeVector transformVector_z_rel(frame_r_in = resolveInFrame, frame_r_out = resolveInFrameAfterDifferentiation) if get_z_rel annotation (Placement(transformation(extent = {
        {80, -94}, 
        {100, -74}})));
    outer Modelica.Mechanics.MultiBody.World world;
    Modelica.Mechanics.MultiBody.Visualizers.Advanced.Arrow arrow(r = frame_a.r_0, r_head = frame_b.r_0 - frame_a.r_0, diameter = arrowDiameter, color = arrowColor) if world.enableAnimation and animation;
equation
    connect(frame_resolve,relativePosition.frame_resolve) annotation (Line(
        points = {
            {100, 80}, 
            {50, 80}, 
            {50, 20}, 
            {-30, 20}, 
            {-30, 8.1}, 
            {-60, 8.1}},
        color = {95, 95, 95},
        pattern = LinePattern.Dot));
    connect(frame_resolve,zeroForce3.frame_a) annotation (Line(
        points = {
            {100, 80}, 
            {40, 80}},
        color = {95, 95, 95},
        pattern = LinePattern.Dot));
    connect(der1.y,transformVector_v_rel.r_in) annotation (Line(
        points = {
            {-60, -30.5}, 
            {-60, -42}},
        color = {0, 0, 127}));
    connect(der2.u,der1.y) annotation (Line(
        points = {
            {-20, -39}, 
            {-20, -35}, 
            {-60, -35}, 
            {-60, -30.5}},
        color = {0, 0, 127}));
    connect(der2.y,transformVector_a_rel.r_in) annotation (Line(
        points = {
            {-20, -50.5}, 
            {-20, -56}},
        color = {0, 0, 127}));
    connect(der3.y,transformVector_z_rel.r_in) annotation (Line(
        points = {
            {90, -58.5}, 
            {90, -72}},
        color = {0, 0, 127}));
    connect(relativeAngles.angles,angles) annotation (Line(
        points = {
            {30, -26}, 
            {30, -92}, 
            {20, -92}, 
            {20, -110}},
        color = {0, 0, 127}));
    connect(relativeAngles.frame_a,frame_a) annotation (Line(
        points = {
            {20, -15}, 
            {10, -15}, 
            {10, -84}, 
            {-90, -84}, 
            {-90, 0}, 
            {-100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(relativeAngles.frame_b,frame_b) annotation (Line(
        points = {
            {40, -15}, 
            {50, -15}, 
            {50, 0}, 
            {100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(relativePosition.frame_a,frame_a) annotation (Line(
        points = {
            {-80, 0}, 
            {-100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(relativePosition.frame_b,frame_b) annotation (Line(
        points = {
            {-60, 0}, 
            {100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(relativePosition.r_rel,r_rel) annotation (Line(
        points = {
            {-70, -11}, 
            {-70, -15}, 
            {-80, -15}, 
            {-80, -30}, 
            {-100, -30}, 
            {-100, -110}},
        color = {0, 0, 127}));
    connect(relativePosition.r_rel,der1.u) annotation (Line(
        points = {
            {-70, -11}, 
            {-70, -15}, 
            {-60, -15}, 
            {-60, -19}},
        color = {0, 0, 127}));
    connect(zeroForce1.frame_a,frame_a) annotation (Line(
        points = {
            {-80, 50}, 
            {-90, 50}, 
            {-90, 0}, 
            {-100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(zeroForce2.frame_a,frame_b) annotation (Line(
        points = {
            {80, 50}, 
            {90, 50}, 
            {90, 0}, 
            {100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(relativeAngularVelocity.frame_a,frame_a) annotation (Line(
        points = {
            {50, -30}, 
            {10, -30}, 
            {10, -84}, 
            {-90, -84}, 
            {-90, 0}, 
            {-100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(relativeAngularVelocity.frame_b,frame_b) annotation (Line(
        points = {
            {70, -30}, 
            {80, -30}, 
            {80, 0}, 
            {100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(relativeAngularVelocity.w_rel,w_rel) annotation (Line(
        points = {
            {60, -41}, 
            {60, -110}},
        color = {0, 0, 127}));
    connect(relativeAngularVelocity.w_rel,der3.u) annotation (Line(
        points = {
            {60, -41}, 
            {60, -45}, 
            {90, -45}, 
            {90, -47}},
        color = {0, 0, 127}));
    connect(relativeAngularVelocity.frame_resolve,frame_resolve) annotation (Line(
        points = {
            {70, -21.9}, 
            {70, -21.9}, 
            {70, 20}, 
            {50, 20}, 
            {50, 80}, 
            {100, 80}},
        color = {95, 95, 95},
        pattern = LinePattern.Dot));
    connect(transformVector_a_rel.frame_a,frame_a) annotation (Line(
        points = {
            {-30, -68}, 
            {-40, -68}, 
            {-40, -84}, 
            {-90, -84}, 
            {-90, 0}, 
            {-100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(transformVector_a_rel.frame_b,frame_b) annotation (Line(
        points = {
            {-10, -68}, 
            {0, -68}, 
            {0, 0}, 
            {100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(transformVector_a_rel.r_out,a_rel) annotation (Line(
        points = {
            {-20, -79}, 
            {-20, -110}},
        color = {0, 0, 127}));
    connect(transformVector_a_rel.frame_resolve,frame_resolve) annotation (Line(
        points = {
            {-10, -60}, 
            {-10, -28}, 
            {-30, -28}, 
            {-30, 20}, 
            {50, 20}, 
            {50, 80}, 
            {100, 80}},
        color = {95, 95, 95},
        pattern = LinePattern.Dot));
    connect(transformVector_v_rel.frame_a,frame_a) annotation (Line(
        points = {
            {-70, -54}, 
            {-90, -54}, 
            {-90, 0}, 
            {-100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(transformVector_v_rel.frame_b,frame_b) annotation (Line(
        points = {
            {-50, -54}, 
            {-40, -54}, 
            {-40, 0}, 
            {100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(transformVector_v_rel.r_out,v_rel) annotation (Line(
        points = {
            {-60, -65}, 
            {-60, -110}},
        color = {0, 0, 127}));
    connect(transformVector_v_rel.frame_resolve,frame_resolve) annotation (Line(
        points = {
            {-50, -46}, 
            {-46, -46}, 
            {-46, -28}, 
            {-30, -28}, 
            {-30, 20}, 
            {50, 20}, 
            {50, 80}, 
            {100, 80}},
        color = {95, 95, 95},
        pattern = LinePattern.Dot));
    connect(transformVector_z_rel.frame_a,frame_a) annotation (Line(
        points = {
            {80, -84}, 
            {-90, -84}, 
            {-90, 0}, 
            {-100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(transformVector_z_rel.frame_b,frame_b) annotation (Line(
        points = {
            {100, -84}, 
            {110, -84}, 
            {110, 0}, 
            {100, 0}},
        color = {95, 95, 95},
        thickness = 0.5));
    connect(transformVector_z_rel.r_out,z_rel) annotation (Line(
        points = {
            {90, -95}, 
            {90, -100}, 
            {100, -100}, 
            {100, -110}},
        color = {0, 0, 127}));
    connect(transformVector_z_rel.frame_resolve,frame_resolve) annotation (Line(
        points = {
            {100, -76}, 
            {100, -20}, 
            {70, -20}, 
            {70, 20}, 
            {50, 20}, 
            {50, 80}, 
            {100, 80}},
        color = {95, 95, 95},
        pattern = LinePattern.Dot));

    annotation (
        Icon(
            coordinateSystem(
                preserveAspectRatio = true,
                extent = {
                    {-100, -100}, 
                    {100, 100}}),
            graphics = {
                Line(
                    visible = get_r_rel,
                    points = {
                        {-84, 0}, 
                        {-84, -60}, 
                        {-100, -60}, 
                        {-100, -100}},
                    color = {0, 0, 127}), 
                Line(
                    visible = get_a_rel,
                    points = {
                        {-20, -67}, 
                        {-20, -100}},
                    color = {0, 0, 127}), 
                Line(
                    visible = get_w_rel,
                    points = {
                        {60, -36}, 
                        {60, -100}},
                    color = {0, 0, 127}), 
                Line(
                    visible = get_z_rel,
                    points = {
                        {86, 0}, 
                        {86, -60}, 
                        {100, -60}, 
                        {100, -100}},
                    color = {0, 0, 127}), 
                Line(
                    visible = get_v_rel,
                    points = {
                        {-60, -36}, 
                        {-60, -100}},
                    color = {0, 0, 127}), 
                Line(
                    visible = get_angles,
                    points = {
                        {20, -67}, 
                        {20, -100}},
                    color = {0, 0, 127}), 
                Text(
                    extent = {
                        {-132, 90}, 
                        {129, 138}},
                    textString = "%name",
                    lineColor = {0, 0, 255}), 
                Text(
                    visible = get_r_rel,
                    extent = {
                        {-130, -74}, 
                        {-95, -90}},
                    textString = "r"), 
                Text(
                    visible = get_v_rel,
                    extent = {
                        {-95, -74}, 
                        {-60, -90}},
                    textString = "v"), 
                Text(
                    visible = get_a_rel,
                    extent = {
                        {-55, -74}, 
                        {-20, -90}},
                    textString = "a"), 
                Text(
                    visible = get_angles,
                    extent = {
                        {-71, -35}, 
                        {96, -54}},
                    textString = "angles"), 
                Text(
                    visible = get_w_rel,
                    extent = {
                        {63, -73}, 
                        {103, -90}},
                    textString = "w"), 
                Text(
                    visible = get_z_rel,
                    extent = {
                        {103, -71}, 
                        {149, -87}},
                    textString = "z")}),
        Documentation(info = "<html>\n<p>\nRelative kinematic quantities between frame_a and frame_b are\ndetermined and provided at the conditional output signal connectors.\nFor example, if parameter \"get_r_rel = <strong>true</strong>\", the connector\n\"r_rel\" is enabled and contains the relative vector from\nframe_a to frame_b. The following quantities can be provided\nas output signals:\n</p>\n\n<ol>\n<li> Relative position vector (= r_rel)</li>\n<li> Relative velocity vector (= v_rel)</li>\n<li> Relative acceleration vector (= a_rel)</li>\n<li> Three angles to rotate frame_a into frame_b (= angles)</li>\n<li> Relative angular velocity vector (= w_rel)</li>\n<li> Relative angular acceleration vector (= z_rel)</li>\n</ol>\n\n<p>\nVia parameter <strong>resolveInFrame</strong> it is defined, in which frame\na vector is resolved (before differentiation):\n</p>\n\n<table border=1 cellspacing=0 cellpadding=2>\n<tr><th><strong>resolveInFrame =<br>Types.ResolveInFrameAB.</strong></th><th><strong>Meaning</strong></th></tr>\n<tr><td>world</td>\n    <td>Resolve vectors in world frame</td></tr>\n\n<tr><td>frame_a</td>\n    <td>Resolve vectors in frame_a</td></tr>\n\n<tr><td>frame_b</td>\n    <td>Resolve vectors in frame_b</td></tr>\n\n<tr><td>frame_resolve</td>\n    <td>Resolve vectors in frame_resolve</td></tr>\n</table>\n\n<p>\nIf resolveInFrame = Types.ResolveInFrameAB.frame_resolve, the conditional connector\n\"frame_resolve\" is enabled and the vectors are resolved in the frame, to\nwhich frame_resolve is connected. Note, if this connector is enabled, it must\nbe connected.\n</p>\n\n<p>\nIn the following figure the animation of a RelativeSensor\ncomponent is shown. The light blue coordinate system is\nframe_a, the dark blue coordinate system is frame_b, and\nthe yellow arrow is the animated sensor.\n</p>\n\n<p>\n<img src=\"modelica://Modelica/Resources/Images/Mechanics/MultiBody/Sensors/RelativeSensor.png\">\n</p>\n\n<p>\nNote, derivatives\nof relative kinematic quantities are always performed with\nrespect to the frame, in which the vector to be differentiated\nis resolved. After differentiation, it is possible via parameter\n<strong>resolveInFrameAfterDifferentiation</strong> (in the \"Advanced\" menu)\nto resolve the differentiated\nvector in another frame.\n</p>\n<p>\nFor example, if resolveInFrame = <strong>Types.ResolveInFrameAB.frame_b</strong>, then\n</p>\n\n<pre>\n   r_rel = resolve2(frame_b.R, frame_b.r_0 - frame_a.r0);\n   v_rel = <strong>der</strong>(r_rel);\n</pre>\n\n<p>\nis returned (r_rel = resolve2(frame_b.R, frame_b.r_0 - frame_a.r0)), i.e.,\nthe derivative of the relative distance from frame_a to frame_b,\nresolved in frame_b. If\n<strong>resolveInFrameAfterDifferentiation</strong> = Types.ResolveInFrameAB.world, then\nv_rel is additionally transformed to:\n</p>\n\n<pre>\n   v_rel = resolve1(frame_b.R, <strong>der</strong>(r_rel))\n</pre>\n\n<p>\nThe cut-force and the cut-torque in frame_resolve are\nalways zero, whether frame_resolve is connected or not.\n</p>\n\n<p>\nIf <strong>get_angles</strong> = <strong>true</strong>, the 3 angles to rotate frame_a\ninto frame_b along the axes defined by parameter <strong>sequence</strong>\nare returned. For 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> &lt;= angles[i] &lt;= <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] &ne; sequence[2]\nand sequence[2] &ne; 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 RelativeSensor;