model RevoluteWithLengthConstraint "Revolute joint where the rotation angle is computed from a length constraint (1 degree-of-freedom, no potential state)"
extends Modelica.Mechanics.MultiBody.Interfaces.PartialTwoFrames;
Modelica.Mechanics.Rotational.Interfaces.Flange_a axis "1-dim. rotational flange that drives the joint"
annotation (Placement(transformation(extent = {
{10, 90},
{-10, 110}})));
Modelica.Mechanics.Rotational.Interfaces.Flange_b bearing "1-dim. rotational flange of the drive bearing"
annotation (Placement(transformation(extent = {
{-50, 90},
{-70, 110}})));
Modelica.Blocks.Interfaces.RealInput position_a[3](each final quantity = "Length", each final unit = "m") "Position vector from frame_a to frame_a side of length constraint, resolved in frame_a of revolute joint"
annotation (Placement(transformation(extent = {
{-140, -80},
{-100, -40}})));
Modelica.Blocks.Interfaces.RealInput position_b[3](each final quantity = "Length", each final unit = "m") "Position vector from frame_b to frame_b side of length constraint, resolved in frame_b of revolute joint"
annotation (Placement(transformation(extent = {
{140, -80},
{100, -40}})));
parameter Boolean animation = true "= true, if animation shall be enabled";
parameter SI.Position lengthConstraint(start = 1) "Fixed length of length constraint";
parameter Modelica.Mechanics.MultiBody.Types.Axis n = {0, 0, 1} "Axis of rotation resolved in frame_a (= same as in frame_b)"
annotation (Evaluate = true);
parameter Cv.NonSIunits.Angle_deg phi_offset = 0 "Relative angle offset (angle = phi + from_deg(phi_offset))";
parameter Cv.NonSIunits.Angle_deg phi_guess = 0 "Select the configuration such that at initial time |phi - from_deg(phi_guess)| is minimal";
parameter SI.Distance cylinderLength = world.defaultJointLength "Length of cylinder representing the joint axis"
annotation (Dialog(
tab = "Animation",
group = "if animation = true",
enable = animation));
parameter SI.Distance cylinderDiameter = world.defaultJointWidth "Diameter of cylinder representing the joint axis"
annotation (Dialog(
tab = "Animation",
group = "if animation = true",
enable = animation));
input Types.Color cylinderColor = Modelica.Mechanics.MultiBody.Types.Defaults.JointColor "Color of cylinder representing the joint axis"
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));
final parameter Boolean positiveBranch(fixed = false) "Based on phi_guess, selection of one of the two solutions of the non-linear constraint equation";
final parameter Real e[3](each final unit = "1") = Modelica.Math.Vectors.normalizeWithAssert(n) "Unit vector in direction of rotation axis, resolved in frame_a";
SI.Angle phi "Rotation angle of revolute joint";
Frames.Orientation R_rel "Relative orientation object from frame_a to frame_b";
SI.Angle angle "= phi + from_deg(phi_offset) (relative rotation angle between frame_a and frame_b)";
SI.Torque tau "= axis.tau (driving torque in the axis)";
protected
SI.Position r_a[3] = position_a "Position vector from frame_a to frame_a side of length constraint, resolved in frame_a of revolute joint";
SI.Position r_b[3] = position_b "Position vector from frame_b to frame_b side of length constraint, resolved in frame_b of revolute joint";
Real e_r_a "Projection of r_a on e";
Real e_r_b "Projection of r_b on e";
Real A "Coefficient A of equation: A*cos(phi) + B*sin(phi) + C = 0";
Real B "Coefficient B of equation: A*cos(phi) + B*sin(phi) + C = 0";
Real C "Coefficient C of equation: A*cos(phi) + B*sin(phi) + C = 0";
Real k1 "Constant of quadratic equation";
Real k2 "Constant of quadratic equation";
Real k1a(start = 1);
Real k1b;
Real kcos_angle "= k1*cos(angle)";
Real ksin_angle "= k1*sin(angle)";
Visualizers.Advanced.Shape cylinder(shapeType = "cylinder", color = cylinderColor, specularCoefficient = specularCoefficient, length = cylinderLength, width = cylinderDiameter, height = cylinderDiameter, lengthDirection = e, widthDirection = {0, 1, 0}, r_shape = -e * (0.5 * cylinderLength), r = frame_a.r_0, R = frame_a.R) if world.enableAnimation and animation;
function selectBranch "Determine branch which is closest to initial angle=0"
extends Modelica.Icons.Function;
input SI.Length L "Length of length constraint";
input Real e[3](each final unit = "1") "Unit vector along axis of rotation, resolved in frame_a (= same in frame_b)";
input SI.Angle angle_guess "Select the configuration such that at initial time |angle-angle_guess| is minimal (angle=0: frame_a and frame_b coincide)";
input SI.Position r_a[3] "Position vector from frame_a to frame_a side of length constraint, resolved in frame_a of revolute joint";
input SI.Position r_b[3] "Position vector from frame_b to frame_b side of length constraint, resolved in frame_b of revolute joint";
output Boolean positiveBranch "Branch of the initial solution";
protected
Real e_r_a "Projection of r_a on e";
Real e_r_b "Projection of r_b on e";
Real A "Coefficient A of equation: A*cos(phi) + B*sin(phi) + C = 0";
Real B "Coefficient B of equation: A*cos(phi) + B*sin(phi) + C = 0";
Real C "Coefficient C of equation: A*cos(phi) + B*sin(phi) + C = 0";
Real k1 "Constant of quadratic equation";
Real k2 "Constant of quadratic equation";
Real k1a;
Real k1b;
Real kcos1 "k1*cos(angle1)";
Real ksin1 "k1*sin(angle1)";
Real kcos2 "k2*cos(angle2)";
Real ksin2 "k2*sin(angle2)";
SI.Angle angle1 "solution 1 of nonlinear equation";
SI.Angle angle2 "solution 2 of nonlinear equation";
algorithm
e_r_a := e * r_a;
e_r_b := e * r_b;
A := -2 * (r_b * r_a - e_r_b * e_r_a);
B := 2 * r_b * cross(e, r_a);
C := r_a * r_a + r_b * r_b - L * L - 2 * e_r_b * e_r_a;
k1 := A * A + B * B;
k1a := k1 - C * C;
assert(1e-10 < k1a, "\nSingular position of loop (either no or two analytic solutions;\nthe mechanism has lost one-degree-of freedom in this position).\nTry first to use another Modelica.Mechanics.MultiBody.Joints.Assemblies.JointXXX component.\nIn most cases it is best that the joints outside of the JointXXX\ncomponent are revolute and NOT prismatic joints. If this also\nlead to singular positions, it could be that this kinematic loop\ncannot be solved analytically. In this case you have to build\nup the loop with basic joints (NO aggregation JointXXX components)\nand rely on dynamic state selection, i.e., during simulation\nthe states will be dynamically selected in such a way that in no\nposition a degree of freedom is lost.\n");
k1b := max(k1a, 1e-12);
k2 := sqrt(k1b);
kcos1 := -A * C + B * k2;
ksin1 := -B * C - A * k2;
angle1 := Modelica.Math.atan2(ksin1, kcos1);
kcos2 := -A * C - B * k2;
ksin2 := -B * C + A * k2;
angle2 := Modelica.Math.atan2(ksin2, kcos2);
if abs(angle1 - angle_guess) <= abs(angle2 - angle_guess) then
positiveBranch := true;
else
positiveBranch := false;
end if;
end selectBranch;
initial equation
positiveBranch = selectBranch(lengthConstraint, e, Cv.from_deg(phi_offset + phi_guess), r_a, r_b);
equation
assert(1e-10 < k1a, "\nSingular position of loop (either no or two analytic solutions;\nthe mechanism has lost one-degree-of freedom in this position).\nTry first to use another Modelica.Mechanics.MultiBody.Joints.Assemblies.JointXXX component.\nIn most cases it is best that the joints outside of the JointXXX\ncomponent are revolute and NOT prismatic joints. If this also\nlead to singular positions, it could be that this kinematic loop\ncannot be solved analytically. In this case you have to build\nup the loop with basic joints (NO aggregation JointXXX components)\nand rely on dynamic state selection, i.e., during simulation\nthe states will be dynamically selected in such a way that in no\nposition a degree of freedom is lost.\n");
Connections.branch(frame_a.R, frame_b.R);
A = -2 * (r_b * r_a - e_r_b * e_r_a);
B = 2 * r_b * cross(e, r_a);
C = r_a * r_a + r_b * r_b - lengthConstraint * lengthConstraint - 2 * e_r_b * e_r_a;
R_rel = Frames.planarRotation(e, angle, der(angle));
angle = Cv.from_deg(phi_offset) + phi;
angle = Modelica.Math.atan2(ksin_angle, kcos_angle);
e_r_a = e * r_a;
e_r_b = e * r_b;
k1 = A * A + B * B;
k1a = k1 - C * C;
k1b = Frames.Internal.maxWithoutEvent(k1a, 1e-12);
k2 = sqrt(k1b);
kcos_angle = -A * C + (if positiveBranch then B else -B) * k2;
ksin_angle = -B * C + (if positiveBranch then -A else A) * k2;
axis.phi = phi;
axis.tau = tau;
bearing.phi = 0;
frame_b.R = Frames.absoluteRotation(frame_a.R, R_rel);
frame_b.r_0 = frame_a.r_0;
zeros(3) = frame_a.f + Frames.resolve1(R_rel, frame_b.f);
zeros(3) = frame_a.t + Frames.resolve1(R_rel, frame_b.t);
annotation (
Icon(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Rectangle(
extent = {
{-30, 10},
{10, -10}},
lineColor = {64, 64, 64},
fillColor = {192, 192, 192},
fillPattern = FillPattern.Solid),
Rectangle(
extent = {
{-100, -60},
{-30, 60}},
lineColor = {64, 64, 64},
fillPattern = FillPattern.HorizontalCylinder,
fillColor = {255, 255, 255},
radius = 10),
Rectangle(
extent = {
{30, -60},
{100, 60}},
lineColor = {64, 64, 64},
fillPattern = FillPattern.HorizontalCylinder,
fillColor = {255, 255, 255},
radius = 10),
Text(
extent = {
{-139, -168},
{137, -111}},
textString = "%name",
lineColor = {0, 0, 255}),
Rectangle(
extent = {
{-100, 60},
{-30, -60}},
lineColor = {64, 64, 64},
radius = 10),
Rectangle(
extent = {
{30, 60},
{100, -60}},
lineColor = {64, 64, 64},
radius = 10),
Text(
extent = {
{-142, -108},
{147, -69}},
textString = "n=%n"),
Line(points = {
{-60, 60},
{-60, 90}}),
Line(points = {
{-20, 70},
{-60, 70}}),
Line(points = {
{-20, 80},
{-20, 60}}),
Line(points = {
{20, 80},
{20, 60}}),
Line(points = {
{20, 70},
{41, 70}}),
Polygon(
points = {
{-9, 30},
{10, 30},
{30, 50},
{-29, 50},
{-9, 30}},
lineColor = {64, 64, 64},
fillColor = {192, 192, 192},
fillPattern = FillPattern.Solid),
Polygon(
points = {
{10, 30},
{30, 50},
{30, -51},
{10, -31},
{10, 30}},
lineColor = {64, 64, 64},
fillColor = {192, 192, 192},
fillPattern = FillPattern.Solid),
Rectangle(
extent = {
{-10, 90},
{10, 50}},
lineColor = {64, 64, 64},
fillPattern = FillPattern.VerticalCylinder,
fillColor = {192, 192, 192})}),
Diagram(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Rectangle(
extent = {
{-100, -60},
{-30, 60}},
lineColor = {64, 64, 64},
fillPattern = FillPattern.HorizontalCylinder,
fillColor = {255, 255, 255},
radius = 10),
Rectangle(
extent = {
{-100, -60},
{-30, 60}},
lineColor = {64, 64, 64},
radius = 10),
Rectangle(
extent = {
{-30, 10},
{10, -10}},
lineColor = {64, 64, 64},
fillColor = {192, 192, 192},
fillPattern = FillPattern.Solid),
Rectangle(
extent = {
{30, -60},
{100, 60}},
lineColor = {64, 64, 64},
fillPattern = FillPattern.HorizontalCylinder,
fillColor = {255, 255, 255},
radius = 10),
Rectangle(
extent = {
{30, -60},
{100, 60}},
lineColor = {64, 64, 64},
radius = 10),
Line(points = {
{-60, 60},
{-60, 96}}),
Line(points = {
{-20, 70},
{-60, 70}}),
Line(points = {
{-20, 80},
{-20, 60}}),
Line(points = {
{20, 80},
{20, 60}}),
Line(points = {
{20, 70},
{41, 70}}),
Polygon(
points = {
{-9, 30},
{10, 30},
{30, 50},
{-29, 50},
{-9, 30}},
lineColor = {64, 64, 64},
fillColor = {192, 192, 192},
fillPattern = FillPattern.Solid),
Polygon(
points = {
{10, 30},
{30, 50},
{30, -51},
{10, -31},
{10, 30}},
lineColor = {64, 64, 64},
fillColor = {192, 192, 192},
fillPattern = FillPattern.Solid),
Rectangle(
extent = {
{-10, 50},
{10, 100}},
lineColor = {64, 64, 64},
fillPattern = FillPattern.VerticalCylinder,
fillColor = {192, 192, 192})}),
Documentation(info = "<html>\n<p>\nJoint where frame_b rotates around axis n which is fixed in frame_a.\nThe two frames coincide when \"phi + phi_offset = 0\", where\n\"phi_offset\" is a parameter with a zero default\nand \"phi\" is the rotation angle.\n</p>\n<p>\nThis variant of the revolute joint is designed to work together\nwith a length constraint in a kinematic loop. This means that the\nangle of the revolute joint, phi, is computed such that the\nlength constraint is fulfilled.\n</p>\n<p>\n<strong>Usually, this joint should not be used by a user of the MultiBody\nlibrary. It is only provided to built-up the Modelica.Mechanics.MultiBody.Joints.Assemblies.JointXYZ\njoints.</strong>\n</p>\n\n<p>\nIn releases before version 3.0 of the Modelica Standard Library, it was possible\nto activate the torque projection equation (= cut-torque projected to the rotation\naxis must be identical to the drive torque of flange axis) via parameter\n<strong>axisTorqueBalance</strong>. This is no longer possible, since otherwise this\nmodel would not be \"balanced\" (= same number of unknowns as equations).\nInstead, when using this model in version 3.0 and later versions,\nthe force in the length constraint component (Joints.SphericalSpherical or\nJoints.UniversalSpherical) must be calculated such that the driving torque\nin direction of the rotation\naxis is (RC shall be the name of the instance of RevoluteWithLengthConstraint):\n</p>\n<pre>\n 0 = RC.axis.tau + RC.e*RC.frame_b.t;\n</pre>\n<p>\nIf this equation is used, usually the force in the length constraint\nand the second derivative of the revolute angle will be part of a linear\nalgebraic system of equations. In some cases it is possible to solve\nthis system of equations locally, i.e., provide the rod force directly\nas function of the revolute constraint torque. In any case, this projection\nequation or an equivalent one has to be provided via variable \"constraintResidue\" in the \"Advanced\"\nmenu of \"Joints.SphericalSpherical\" or \"Joints.UniversalSpherical\".\n</p>\n\n</html>"));
end RevoluteWithLengthConstraint;