model JointUSP "Universal - spherical - prismatic joint aggregation (no constraints, no potential states)"
import Modelica.Mechanics.MultiBody.Types;
extends Interfaces.PartialTwoFramesDoubleSize;
Modelica.Mechanics.MultiBody.Interfaces.Frame_a frame_ia "Coordinate system at origin of frame_a fixed at connecting rod of universal and spherical joint"
annotation (Placement(transformation(
origin = {-80, 100},
extent = {
{-8, -8},
{8, 8}},
rotation = 90)));
Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_ib "Coordinate system at origin of frame_b fixed at connecting rod of spherical and prismatic joint"
annotation (Placement(transformation(
origin = {80, 100},
extent = {
{-8, 8},
{8, -8}},
rotation = 270)));
Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_im "Coordinate system at origin of spherical joint fixed at connecting rod of spherical and prismatic joint"
annotation (Placement(transformation(
origin = {0, 100},
extent = {
{8, -8},
{-8, 8}},
rotation = 270)));
Modelica.Mechanics.Translational.Interfaces.Flange_a axis "1-dim. translational flange that drives the prismatic joint"
annotation (Placement(transformation(extent = {
{95, 75},
{105, 85}})));
Modelica.Mechanics.Translational.Interfaces.Flange_b bearing "1-dim. translational flange of the drive bearing of the prismatic joint"
annotation (Placement(transformation(extent = {
{105, 35},
{95, 45}})));
parameter Boolean animation = true "= true, if animation shall be enabled";
parameter Boolean showUniversalAxes = true "= true, if universal joint shall be visualized with two cylinders, otherwise with a sphere (provided animation=true)";
parameter Modelica.Mechanics.MultiBody.Types.Axis n1_a = {0, 0, 1} "Axis 1 of universal joint fixed and resolved in frame_a (axis 2 is orthogonal to axis 1 and to rod 1)"
annotation (Evaluate = true);
parameter Modelica.Mechanics.MultiBody.Types.Axis n_b = {-1, 0, 0} "Axis of prismatic joint fixed and resolved in frame_b"
annotation (Evaluate = true);
parameter SI.Position rRod1_ia[3] = {1, 0, 0} "Vector from origin of frame_a to spherical joint, resolved in frame_ia"
annotation (Evaluate = true);
parameter SI.Position rRod2_ib[3] = {-1, 0, 0} "Vector from origin of frame_ib to spherical joint, resolved in frame_ib (frame_ib is parallel to frame_b)"
annotation (Evaluate = true);
parameter SI.Position s_offset = 0 "Relative distance offset of prismatic joint (distance between the prismatic joint frames = s(t) + s_offset)";
parameter SI.Position s_guess = 0 "Select the configuration such that at initial time |s(t0)-s_guess| is minimal";
parameter SI.Diameter sphereDiameter = world.defaultJointLength "Diameter of the spheres representing the universal and the spherical joint"
annotation (Dialog(
tab = "Animation",
group = "if animation = true",
enable = animation));
input Types.Color sphereColor = Modelica.Mechanics.MultiBody.Types.Defaults.JointColor "Color of the spheres representing the universal and the spherical joint"
annotation (Dialog(
colorSelector = true,
tab = "Animation",
group = "if animation = true",
enable = animation));
parameter SI.Diameter rod1Diameter = sphereDiameter / Types.Defaults.JointRodDiameterFraction "Diameter of rod 1 connecting the universal and the spherical joint"
annotation (Dialog(
tab = "Animation",
group = "if animation = true",
enable = animation));
input Types.Color rod1Color = Modelica.Mechanics.MultiBody.Types.Defaults.RodColor "Color of rod 1 connecting the universal and the spherical joint"
annotation (Dialog(
colorSelector = true,
tab = "Animation",
group = "if animation = true",
enable = animation));
parameter SI.Diameter rod2Diameter = rod1Diameter "Diameter of rod 2 connecting the prismatic and the spherical joint"
annotation (Dialog(
tab = "Animation",
group = "if animation = true",
enable = animation));
input Types.Color rod2Color = rod1Color "Color of rod 2 connecting the prismatic and the spherical joint"
annotation (Dialog(
colorSelector = true,
tab = "Animation",
group = "if animation = true",
enable = animation));
parameter Types.Axis boxWidthDirection = {0, 1, 0} "Vector in width direction of prismatic joint, resolved in frame_b"
annotation (
Evaluate = true,
Dialog(
tab = "Animation",
group = "if animation = true",
enable = animation));
parameter SI.Distance boxWidth = world.defaultJointWidth "Width of prismatic joint box"
annotation (Dialog(
tab = "Animation",
group = "if animation = true",
enable = animation));
parameter SI.Distance boxHeight = boxWidth "Height of prismatic joint box"
annotation (Dialog(
tab = "Animation",
group = "if animation = true",
enable = animation));
input Types.Color boxColor = sphereColor "Color of prismatic joint box"
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 SI.Distance cylinderLength = world.defaultJointLength "Length of cylinders representing the two universal joint axes"
annotation (Dialog(
tab = "Animation",
group = "if animation = true and showUniversalAxes",
enable = animation and showUniversalAxes));
parameter SI.Distance cylinderDiameter = world.defaultJointWidth "Diameter of cylinders representing the two universal joint axes"
annotation (Dialog(
tab = "Animation",
group = "if animation = true and showUniversalAxes",
enable = animation and showUniversalAxes));
input Types.Color cylinderColor = Modelica.Mechanics.MultiBody.Types.Defaults.JointColor "Color of cylinders representing the two universal joint axes"
annotation (Dialog(
colorSelector = true,
tab = "Animation",
group = "if animation = true and showUniversalAxes",
enable = animation and showUniversalAxes));
parameter Boolean checkTotalPower = false "= true, if total power flowing into this component shall be determined (must be zero)"
annotation (Dialog(tab = "Advanced"));
final parameter Real eRod1_ia[3](each final unit = "1") = rod1.eRod_ia "Unit vector from origin of frame_a to origin of spherical joint, resolved in frame_ia";
final parameter Real e2_ia[3](each final unit = "1") = rod1.e2_ia "Unit vector in direction of axis 2 of universal joint, resolved in frame_ia";
final parameter SI.Distance rod1Length = rod1.rodLength "Length of rod 1 (= distance between universal and spherical joint)";
SI.Force f_rod "Constraint force in direction of the rod (positive, if rod is pressed)";
SI.Power totalPower "Total power flowing into this element, if checkTotalPower=true (otherwise dummy)";
Modelica.Mechanics.MultiBody.Joints.Internal.PrismaticWithLengthConstraint prismatic(animation = animation, length = rod1.rodLength, n = n_b, s_offset = s_offset, s_guess = s_guess, boxWidthDirection = boxWidthDirection, boxWidth = boxWidth, boxHeight = boxHeight, boxColor = boxColor, specularCoefficient = specularCoefficient) annotation (Placement(transformation(extent = {
{76, -20},
{36, 20}})));
Modelica.Mechanics.MultiBody.Joints.UniversalSpherical rod1(animation = animation, showUniversalAxes = showUniversalAxes, rRod_ia = rRod1_ia, n1_a = n1_a, sphereDiameter = sphereDiameter, sphereColor = sphereColor, rodWidth = rod1Diameter, rodHeight = rod1Diameter, rodColor = rod1Color, specularCoefficient = specularCoefficient, cylinderLength = cylinderLength, cylinderDiameter = cylinderDiameter, cylinderColor = cylinderColor, kinematicConstraint = false, constraintResidue = rod1.f_rod - f_rod) annotation (Placement(transformation(extent = {
{-92, -20},
{-52, 20}})));
Modelica.Mechanics.MultiBody.Parts.FixedTranslation rod2(animation = animation, r = rRod2_ib, width = rod2Diameter, height = rod2Diameter, specularCoefficient = specularCoefficient, color = rod2Color) annotation (Placement(transformation(extent = {
{0, 20},
{-40, -20}})));
Sensors.RelativePosition relativePosition(resolveInFrame = Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB.frame_a) annotation (Placement(transformation(extent = {
{50, -70},
{30, -90}})));
Modelica.Blocks.Sources.Constant position_b[3](k = rRod2_ib) annotation (Placement(transformation(extent = {
{-20, -60},
{0, -40}})));
protected
Real aux "Denominator used to compute force in rod connecting universal and spherical joint";
equation
if checkTotalPower then
totalPower = frame_a.f * Frames.resolve2(frame_a.R, der(frame_a.r_0)) + frame_b.f * Frames.resolve2(frame_b.R, der(frame_b.r_0)) + frame_ia.f * Frames.resolve2(frame_ia.R, der(frame_ia.r_0)) + frame_ib.f * Frames.resolve2(frame_ib.R, der(frame_ib.r_0)) + frame_im.f * Frames.resolve2(frame_im.R, der(frame_im.r_0)) + frame_a.t * Frames.angularVelocity2(frame_a.R) + frame_b.t * Frames.angularVelocity2(frame_b.R) + frame_ia.t * Frames.angularVelocity2(frame_ia.R) + frame_ib.t * Frames.angularVelocity2(frame_ib.R) + frame_im.t * Frames.angularVelocity2(frame_im.R) + axis.f * der(axis.s) + bearing.f * der(bearing.s);
else
totalPower = 0;
end if;
connect(prismatic.axis,axis) annotation (Line(
points = {
{40, 14},
{40, 56},
{90, 56},
{90, 80},
{100, 80}},
color = {0, 191, 0}));
connect(prismatic.bearing,bearing) annotation (Line(
points = {
{64, 14},
{64, 40},
{100, 40}},
color = {0, 191, 0}));
connect(prismatic.frame_a,frame_b) annotation (Line(
points = {
{76, 0},
{100, 0}},
color = {95, 95, 95},
thickness = 0.5));
connect(prismatic.frame_b,rod2.frame_a) annotation (Line(
points = {
{36, 0},
{0, 0}},
color = {95, 95, 95},
thickness = 0.5));
connect(rod1.frame_a,frame_a) annotation (Line(
points = {
{-92, 0},
{-100, 0}},
color = {95, 95, 95},
thickness = 0.5));
connect(rod1.frame_ia,frame_ia) annotation (Line(
points = {
{-80, 20},
{-80, 100}},
color = {95, 95, 95},
thickness = 0.5));
connect(rod2.frame_a,frame_ib) annotation (Line(
points = {
{0, 0},
{7, 0},
{7, 70},
{80, 70},
{80, 100}},
color = {95, 95, 95},
thickness = 0.5));
connect(rod2.frame_b,frame_im) annotation (Line(
points = {
{-40, 0},
{-46, 0},
{-46, 80},
{0, 80},
{0, 100}},
color = {95, 95, 95},
thickness = 0.5));
connect(rod2.frame_b,rod1.frame_b) annotation (Line(
points = {
{-40, 0},
{-52, 0}},
thickness = 0.5));
connect(position_b.y,prismatic.position_b) annotation (Line(
points = {
{1, -50},
{10, -50},
{10, -12},
{32, -12}},
color = {0, 0, 127}));
connect(relativePosition.frame_a,frame_b) annotation (Line(
points = {
{50, -80},
{95, -80},
{95, 0},
{100, 0}},
color = {95, 95, 95},
pattern = LinePattern.Dot));
connect(relativePosition.frame_b,frame_a) annotation (Line(
points = {
{30, -80},
{-97, -80},
{-97, 0},
{-100, 0}},
color = {95, 95, 95},
pattern = LinePattern.Dot));
connect(relativePosition.r_rel,prismatic.position_a) annotation (Line(
points = {
{40, -69},
{40, -50},
{90, -50},
{90, -12},
{80, -12}},
color = {0, 0, 127}));
aux = prismatic.e * Frames.resolveRelative(rod1.eRod_a, rod1.frame_a.R, rod1.frame_b.R);
f_rod = (-prismatic.f - prismatic.e * (frame_ib.f + frame_im.f - Frames.resolveRelative(rod1.f_b_a1, rod1.frame_a.R, rod1.frame_b.R))) / noEvent(if abs(aux) < 1e-10 then 1e-10 else aux);
annotation (
Documentation(info = "<html>\n<p>\nThis component consists of a <strong>universal</strong> joint at frame_a, a <strong>prismatic</strong>\njoint at frame_b and a <strong>spherical</strong> joint which is connected via <strong>rod1</strong>\nto the universal and via <strong>rod2</strong> to the prismatic joint, see the default\nanimation in the following figure (the axes vectors are not part of the\ndefault animation):\n</p>\n\n<p>\n<img src=\"modelica://Modelica/Resources/Images/Mechanics/MultiBody/Joints/JointUSP.png\" alt=\"model Joints.Assemblies.JointUSP\">\n</p>\n\n<p>\nThis joint aggregation has no mass and no inertia and\nintroduces neither constraints nor potential state variables.\nIt should be used in kinematic loops whenever possible since\nthe non-linear system of equations introduced by this joint aggregation\nis solved <strong>analytically</strong> (i.e., a solution is always computed, if a\nunique solution exists).\n</p>\n<p>\nThe universal joint is defined in the following way:\n</p>\n<ul>\n<li> The rotation <strong>axis</strong> of revolute joint <strong>1</strong> is along parameter\n vector n1_a which is fixed in frame_a.</li>\n<li> The rotation <strong>axis</strong> of revolute joint <strong>2</strong> is perpendicular to\n axis 1 and to the line connecting the universal and the spherical joint\n (= rod 1).</li>\n</ul>\n<p>\nThe definition of axis 2 of the universal joint is performed according\nto the most often occurring case. In a future release, axis 2 might\nbe explicitly definable via a parameter. However, the treatment is much more\ncomplicated and the number of operations is considerably higher,\nif axis 2 is not orthogonal to axis 1 and to the connecting rod.\n</p>\n<p>\nNote, there is a <strong>singularity</strong> when axis 1 and the connecting rod are parallel\nto each other. Therefore, if possible n1_a should be selected in such a way that it\nis perpendicular to rRod1_ia in the initial configuration (i.e., the\ndistance to the singularity is as large as possible).\n</p>\n<p>\nThe rest of this joint aggregation is defined by the following parameters:\n</p>\n<ul>\n<li> The position of the spherical joint with respect to the universal\n joint is defined by vector <strong>rRod1_ia</strong>. This vector is directed from\n frame_a to the spherical joint and is resolved in frame_ia\n (it is most simple to select frame_ia such that it is parallel to\n frame_a in the reference or initial configuration).</li>\n<li> The position of the spherical joint with respect to the prismatic\n joint is defined by vector <strong>rRod2_ib</strong>. This vector is directed from\n the inner frame of the prismatic joint (frame_ib or prismatic.frame_a)\n to the spherical joint and is resolved in frame_ib (note, that frame_ib\n and frame_b are parallel to each other).</li>\n<li> The axis of translation of the prismatic joint is defined by axis\n vector <strong>n_b</strong>. It is fixed and resolved in frame_b.</li>\n<li> The two frames of the prismatic joint, i.e., frame_b and frame_ib,\n are parallel to each other.\n The distance between the origins of these two frames along axis n_b\n is equal to \"prismatic.s(t) + s_offset\", where \"prismatic.s(t)\" is\n a time varying variable and \"s_offset\" is a fixed, constant offset\n parameter.</li>\n<li> When specifying this joint aggregation with the definitions above, <strong>two</strong>\n different <strong>configurations</strong> are possible. Via parameter <strong>s_guess</strong>\n a guess value for prismatic.s(t0) at the initial time t0 is given. The configuration\n is selected that is closest to s_guess (|prismatic.s - s_guess| is minimal).</li>\n</ul>\n<p>\nAn additional <strong>frame_ia</strong> is present. It is <strong>fixed</strong> in the rod\nconnecting the universal and the spherical joint at the\norigin of <strong>frame_a</strong>. The placement of frame_ia on the rod\nis implicitly defined by the universal joint (frame_a and frame_ia coincide\nwhen the angles of the two revolute joints of the universal joint are zero)\nand by parameter vector <strong>rRod1_ia</strong>, the position vector\nfrom the origin of frame_a to the spherical joint, resolved in frame_<strong>ia</strong>.\n</p>\n<p>\nAn additional <strong>frame_ib</strong> is present. It is <strong>fixed</strong> in the rod\nconnecting the prismatic and the spherical joint at the side of the prismatic\njoint that is connected to this rod (= rod2.frame_a = prismatic.frame_a).\nIt is always parallel to <strong>frame_b</strong>.\n</p>\n<p>\nAn additional <strong>frame_im</strong> is present. It is <strong>fixed</strong> in the rod\nconnecting the prismatic and the spherical joint at the side of the spherical\njoint that is connected to this rod (= rod2.frame_b).\nIt is always parallel to <strong>frame_b</strong>.\n</p>\n<p>\nThe easiest way to define the parameters of this joint is by moving the\nMultiBody system in a <strong>reference configuration</strong> where <strong>all frames</strong>\nof all components are <strong>parallel</strong> to each other (alternatively,\nat least frame_a and frame_ia of the JointUSP joint\nshould be parallel to each other when defining an instance of this\ncomponent).\n</p>\n<p>\nIn the public interface of the JointUSP joint, the following\n(final) <strong>parameters</strong> are provided:\n</p>\n<pre>\n <strong>parameter</strong> Real rod1Length(unit=\"m\") \"Length of rod 1\";\n <strong>parameter</strong> Real eRod1_ia[3] \"Unit vector along rod 1, resolved in frame_ia\";\n <strong>parameter</strong> Real e2_ia [3] \"Unit vector along axis 2, resolved in frame_ia\";\n</pre>\n<p>\nThis allows a more convenient definition of data which is related to rod 1.\nFor example, if a box shall be connected at frame_ia directing from\nthe origin of frame_a to the middle of rod 1, this might be defined as:\n</p>\n<pre>\n Modelica.Mechanics.MultiBody.Joints.Assemblies.JointUSP jointUSP(rRod1_ia={1.2, 1, 0.2});\n Modelica.Mechanics.MultiBody.Visualizers.FixedShape shape(shapeType = \"box\",\n lengthDirection = jointUSP.eRod1_ia,\n widthDirection = jointUSP.e2_ia,\n length = jointUSP.rod1Length/2,\n width = jointUSP.rod1Length/10);\n <strong>equation</strong>\n <strong>connect</strong>(jointUSP.frame_ia, shape.frame_a);\n</pre>\n</html>"),
Icon(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}},
initialScale = 0.2),
graphics = {
Rectangle(
extent = {
{50, 20},
{80, -20}},
fillColor = {192, 192, 192},
fillPattern = FillPattern.Solid),
Rectangle(
extent = {
{80, 30},
{100, -30}},
fillColor = {192, 192, 192},
fillPattern = FillPattern.Solid),
Text(
extent = {
{-140, -45},
{140, -70}},
lineColor = {0, 0, 255},
textString = "%name"),
Ellipse(
extent = {
{-100, -30},
{-40, 30}},
fillPattern = FillPattern.Sphere,
fillColor = {192, 192, 192}),
Ellipse(
extent = {
{-93, -22},
{-48, 23}},
lineColor = {160, 160, 164},
fillColor = {255, 255, 255},
fillPattern = FillPattern.Solid),
Rectangle(
extent = {
{-70, 40},
{-39, -33}},
lineColor = {255, 255, 255},
fillColor = {255, 255, 255},
fillPattern = FillPattern.Solid),
Line(
points = {
{-70, 28},
{-70, -30}},
thickness = 0.5),
Ellipse(
extent = {
{-89, -18},
{-48, 18}},
fillPattern = FillPattern.Sphere,
fillColor = {192, 192, 192}),
Ellipse(
extent = {
{-84, -12},
{-53, 13}},
lineColor = {160, 160, 164},
fillColor = {255, 255, 255},
fillPattern = FillPattern.Solid),
Polygon(
points = {
{-81, -17},
{-92, -1},
{-83, 16},
{-57, 24},
{-81, -17}},
pattern = LinePattern.None,
fillColor = {255, 255, 255},
fillPattern = FillPattern.Solid,
lineColor = {0, 0, 255}),
Line(
points = {
{-70, 30},
{-70, -10}},
thickness = 0.5),
Line(
points = {
{-61, 16},
{-79, -15}},
thickness = 0.5),
Line(
points = {
{-50, 0},
{-50, 80},
{-80, 80},
{-80, 100}},
color = {95, 95, 95},
thickness = 0.5),
Ellipse(
extent = {
{-40, -30},
{20, 30}},
fillPattern = FillPattern.Sphere,
fillColor = {192, 192, 192}),
Ellipse(
extent = {
{-33, -22},
{12, 23}},
lineColor = {192, 192, 192},
fillColor = {255, 255, 255},
fillPattern = FillPattern.Solid),
Rectangle(
extent = {
{-44, 31},
{-14, -30}},
lineColor = {255, 255, 255},
fillColor = {255, 255, 255},
fillPattern = FillPattern.Solid),
Ellipse(
extent = {
{-23, 10},
{-3, -10}},
fillPattern = FillPattern.Solid),
Rectangle(
extent = {
{19, 6},
{50, -6}},
fillPattern = FillPattern.HorizontalCylinder,
fillColor = {192, 192, 192}),
Rectangle(
extent = {
{-50, 5},
{-21, -5}},
fillPattern = FillPattern.HorizontalCylinder,
fillColor = {192, 192, 192}),
Text(
extent = {
{37, 109},
{68, 90}},
lineColor = {128, 128, 128},
textString = "ib"),
Text(
extent = {
{-124, 110},
{-93, 90}},
lineColor = {128, 128, 128},
textString = "ia"),
Line(
points = {
{50, 6},
{50, 80},
{80, 80},
{80, 100}},
color = {95, 95, 95},
thickness = 0.5),
Text(
extent = {
{-44, 111},
{-8, 91}},
lineColor = {128, 128, 128},
textString = "im"),
Line(
points = {
{19, 6},
{19, 80},
{0, 80},
{0, 100}},
color = {95, 95, 95},
thickness = 0.5),
Rectangle(
extent = {
{80, 24},
{100, 30}},
pattern = LinePattern.None,
fillPattern = FillPattern.Solid,
lineColor = {0, 0, 255}),
Rectangle(
extent = {
{50, 14},
{80, 20}},
pattern = LinePattern.None,
fillPattern = FillPattern.Solid,
lineColor = {0, 0, 255}),
Line(
points = {
{95, 80},
{79, 80}},
color = {135, 135, 135},
thickness = 0.5),
Line(
points = {
{95, 40},
{90, 40},
{90, 30}},
color = {135, 135, 135},
thickness = 0.5)}),
Diagram(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}},
initialScale = 0.2),
graphics = {
Line(
points = {
{-78, 30},
{-50, 30}},
color = {128, 128, 128},
arrow = {Arrow.None, Arrow.Filled}),
Text(
extent = {
{-76, 39},
{-49, 32}},
lineColor = {128, 128, 128},
textString = "rRod1_ia"),
Text(
extent = {
{-27, 40},
{0, 33}},
lineColor = {128, 128, 128},
textString = "rRod2_ib"),
Line(
points = {
{3, 30},
{-43, 30}},
color = {128, 128, 128},
arrow = {Arrow.None, Arrow.Filled})}));
end JointUSP;