Planar revolute - revolute - prismatic joint aggregation (no constraints, no potential states). This component consists of 2 revolute joints with parallel
axes of rotation that and a prismatic joint with a translational
axis that is orthogonal to the revolute joint axes, see the default
animation in the following figure (the axes vectors are not part of the
default animation):

within Modelica.Mechanics.MultiBody.Joints.Assemblies;
model JointRRP
  "Planar revolute - revolute - prismatic joint aggregation (no constraints, no potential states)"

  import Modelica.Mechanics.MultiBody.Types;
  import Modelica.Units.Conversions.to_unit1;

  extends Interfaces.PartialTwoFramesDoubleSize;
  Modelica.Mechanics.MultiBody.Interfaces.Frame_a frame_ia
    "Coordinate system at origin of frame_a fixed at connecting rod of revolute joints"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_ib
    "Coordinate system at origin of frame_b fixed at connecting rod of revolute and prismatic joint"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_im
    "Coordinate system at origin of revolute joint in the middle fixed at connecting rod of revolute and prismatic joint"
  Modelica.Mechanics.Translational.Interfaces.Flange_a axis
    "1-dim. translational flange that drives the prismatic joint"
  Modelica.Mechanics.Translational.Interfaces.Flange_b bearing
    "1-dim. translational flange of the drive bearing of the prismatic joint"

  parameter Boolean animation=true "= true, if animation shall be enabled";
  parameter Modelica.Mechanics.MultiBody.Types.Axis n_a={0,0,1}
    "Axes of the two revolute joints resolved in frame_a (both axes are parallel to each other)"
  parameter Modelica.Mechanics.MultiBody.Types.Axis n_b={-1,0,0}
    "Axis of prismatic joint fixed and resolved in frame_b (must be orthogonal to revolute joint axes)"
  parameter SI.Position rRod1_ia[3]={1,0,0}
    "Vector from origin of frame_a to revolute joint in the middle, resolved in frame_ia"
  parameter SI.Position rRod2_ib[3]={-1,0,0}
    "Vector from origin of frame_ib to revolute joint in the middle, resolved in frame_ib (frame_ib is parallel to frame_b)";
  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.Distance cylinderLength=world.defaultJointLength
    "Length of cylinders representing the revolute joints"
  parameter SI.Distance cylinderDiameter=world.defaultJointWidth
    "Diameter of cylinders representing the revolute joints"
  input Types.Color cylinderColor=Modelica.Mechanics.MultiBody.Types.Defaults.JointColor
    "Color of cylinders representing the revolute joints"
  parameter Types.Axis boxWidthDirection={0,1,0}
    "Vector in width direction of prismatic joint, resolved in frame_b"
  parameter SI.Distance boxWidth=world.defaultJointWidth
    "Width of prismatic joint box"
  parameter SI.Distance boxHeight=boxWidth "Height of prismatic joint box"
  input Types.Color boxColor=cylinderColor "Color of prismatic joint box"
  parameter SI.Diameter rodDiameter=1.1*cylinderDiameter
    "Diameter of the two rods connecting the joints"
  input Types.Color rodColor=Modelica.Mechanics.MultiBody.Types.Defaults.RodColor
    "Color of the two rods connecting the joints"
  input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"
  parameter Boolean checkTotalPower=false
    "= true, if total power flowing into this component shall be determined (must be zero)"
  final parameter Real e_a[3](each final unit="1")=Modelica.Math.Vectors.normalizeWithAssert(
                                               n_a)
    "Unit vector along axes of rotations, resolved in frame_a";
  final parameter Real e_ia[3](each final unit="1")=jointUSP.e2_ia
    "Unit vector along axes of rotations, resolved in frame_ia";
  final parameter Real e_im[3](each final unit="1", each fixed=false)
    "Unit vector along axes of rotations, resolved in frame_im";
  final parameter Real e_b[3](each final unit="1")=jointUSP.prismatic.e
    "Unit vector along axes of translation of the prismatic joint, resolved in frame_b and frame_ib";
  SI.Power totalPower=jointUSP.totalPower
    "Total power flowing into this element, if checkTotalPower=true (otherwise dummy)";

  JointUSP jointUSP(
    animation=false,
    showUniversalAxes=false,
    n1_a=n_a,
    n_b=n_b,
    s_offset=s_offset,
    s_guess=s_guess,
    rRod1_ia=rRod1_ia,
    rRod2_ib=rRod2_ib,
    checkTotalPower=checkTotalPower)

protected
  Visualizers.Advanced.Shape shape_rev1(
    shapeType="cylinder",
    color=cylinderColor,
    specularCoefficient=specularCoefficient,
    length=cylinderLength,
    width=cylinderDiameter,
    height=cylinderDiameter,
    lengthDirection=e_a,
    widthDirection={0,1,0},
    r_shape=-e_a*(cylinderLength/2),
    r=frame_a.r_0,
    R=frame_a.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape shape_rev2(
    shapeType="cylinder",
    color=cylinderColor,
    specularCoefficient=specularCoefficient,
    length=cylinderLength,
    width=cylinderDiameter,
    height=cylinderDiameter,
    lengthDirection=e_im,
    widthDirection={0,1,0},
    r_shape=-e_im*(cylinderLength/2),
    r=frame_im.r_0,
    R=frame_im.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape shape_prism(
    shapeType="box",
    color=boxColor,
    specularCoefficient=specularCoefficient,
    length=jointUSP.prismatic.distance,
    width=boxWidth,
    height=boxHeight,
    lengthDirection=e_b,
    widthDirection=e_im,
    r=frame_b.r_0,
    R=frame_b.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape shape_rod1(
    shapeType="cylinder",
    color=rodColor,
    specularCoefficient=specularCoefficient,
    length=Modelica.Math.Vectors.length(
                         rRod1_ia),
    width=rodDiameter,
    height=rodDiameter,
    lengthDirection = to_unit1(rRod1_ia),
    widthDirection=e_ia,
    r=frame_ia.r_0,
    R=frame_ia.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape shape_rod2(
    shapeType="cylinder",
    color=rodColor,
    specularCoefficient=specularCoefficient,
    length=Modelica.Math.Vectors.length(
                         rRod2_ib),
    width=rodDiameter,
    height=rodDiameter,
    lengthDirection = to_unit1(rRod2_ib),
    widthDirection=e_b,
    r=frame_ib.r_0,
    R=frame_ib.R) if world.enableAnimation and animation;
initial equation
  e_im = Frames.resolve2(frame_im.R, Frames.resolve1(frame_a.R, e_a));

equation
  connect(jointUSP.frame_a, frame_a)
  connect(jointUSP.frame_b, frame_b)
  connect(jointUSP.frame_ia, frame_ia)
  connect(jointUSP.frame_im, frame_im)
  connect(jointUSP.frame_ib, frame_ib)
  connect(jointUSP.axis, axis)
  connect(jointUSP.bearing, bearing)
end JointRRP;


Planar revolute - revolute - revolute joint aggregation (no constraints, no potential states). This component consists of 3 revolute joints with parallel
axes of rotation that are connected together by two rods, see the default
animation in the following figure (the axes vectors are not part of the
default animation):

within Modelica.Mechanics.MultiBody.Joints.Assemblies;
model JointRRR
  "Planar revolute - revolute - revolute joint aggregation (no constraints, no potential states)"

  import Modelica.Mechanics.MultiBody.Types;
  import Modelica.Units.Conversions.to_unit1;

  extends Interfaces.PartialTwoFramesDoubleSize;

  Modelica.Mechanics.MultiBody.Interfaces.Frame_a frame_ia
    "Coordinate system at origin of frame_a fixed at connecting rod of left and middle revolute joint"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_ib
    "Coordinate system at origin of frame_b fixed at connecting rod of middle and right revolute joint"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_im
    "Coordinate system at origin of revolute joint in the middle fixed at connecting rod of middle and right revolute joint"
  Modelica.Mechanics.Rotational.Interfaces.Flange_a axis
    "1-dim. rotational flange that drives the right revolute joint at frame_b"
  Modelica.Mechanics.Rotational.Interfaces.Flange_b bearing
    "1-dim. rotational flange of the drive bearing of the right revolute joint at frame_b"

  parameter Boolean animation=true "= true, if animation shall be enabled";
  parameter Modelica.Mechanics.MultiBody.Types.Axis n_a={0,0,1}
    "Axes of revolute joints resolved in frame_a (all axes are parallel to each other)"
  final parameter Real n_b[3](each final unit="1",each fixed=false, start = {0,0,1})
    "Axis of revolute joint fixed and resolved in frame_b";
  parameter SI.Position rRod1_ia[3]={1,0,0}
    "Vector from origin of frame_a to revolute joint in the middle, resolved in frame_ia"
  parameter SI.Position rRod2_ib[3]={-1,0,0}
    "Vector from origin of frame_ib to revolute joint in the middle, resolved in frame_ib";
  parameter Modelica.Units.NonSI.Angle_deg phi_offset=0
    "Relative angle offset of revolute joint at frame_b (angle = phi(t) + from_deg(phi_offset))";
  parameter Modelica.Units.NonSI.Angle_deg phi_guess=0
    "Select the configuration such that at initial time |phi(t0) - from_deg(phi_guess)| is minimal";
  parameter SI.Distance cylinderLength=world.defaultJointLength
    "Length of cylinders representing the revolute joints"
  parameter SI.Distance cylinderDiameter=world.defaultJointWidth
    "Diameter of cylinders representing the revolute joints"
  input Types.Color cylinderColor=Modelica.Mechanics.MultiBody.Types.Defaults.JointColor
    "Color of cylinders representing the revolute joints"
  parameter SI.Diameter rodDiameter=1.1*cylinderDiameter
    "Diameter of the two rods connecting the revolute joints"
  input Types.Color rodColor=Modelica.Mechanics.MultiBody.Types.Defaults.RodColor
    "Color of the two rods connecting the revolute joint"
  input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"

  parameter Boolean checkTotalPower=false
    "= true, if total power flowing into this component shall be determined (must be zero)"
  final parameter Real e_a[3](each final unit="1")=Modelica.Math.Vectors.normalizeWithAssert(
                                               n_a)
    "Unit vector along axes of rotations, resolved in frame_a";
  final parameter Real e_ia[3](each final unit="1")=jointUSR.e2_ia
    "Unit vector along axes of rotations, resolved in frame_ia";
  final parameter Real e_b[3](each final unit="1")=jointUSR.revolute.e
    "Unit vector along axes of rotations, resolved in frame_b, frame_ib and frame_im";
  SI.Power totalPower=jointUSR.totalPower
    "Total power flowing into this element, if checkTotalPower=true (otherwise dummy)";

  JointUSR jointUSR(
    animation=false,
    n1_a=n_a,
    n_b=n_b,
    phi_offset=phi_offset,
    rRod2_ib=rRod2_ib,
    showUniversalAxes=false,
    rRod1_ia=rRod1_ia,
    checkTotalPower=checkTotalPower,
    phi_guess=phi_guess)

protected
 Visualizers.Advanced.Shape shape_rev1(
    shapeType="cylinder",
    color=cylinderColor,
    specularCoefficient=specularCoefficient,
    length=cylinderLength,
    width=cylinderDiameter,
    height=cylinderDiameter,
    lengthDirection=e_a,
    widthDirection={0,1,0},
    r_shape=-e_a*(cylinderLength/2),
    r=frame_a.r_0,
    R=frame_a.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape shape_rev2(
    shapeType="cylinder",
    color=cylinderColor,
    specularCoefficient=specularCoefficient,
    length=cylinderLength,
    width=cylinderDiameter,
    height=cylinderDiameter,
    lengthDirection=e_b,
    widthDirection={0,1,0},
    r_shape=-e_b*(cylinderLength/2),
    r=frame_im.r_0,
    R=frame_im.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape shape_rev3(
    shapeType="cylinder",
    color=cylinderColor,
    specularCoefficient=specularCoefficient,
    length=cylinderLength,
    width=cylinderDiameter,
    height=cylinderDiameter,
    lengthDirection=e_b,
    widthDirection={0,1,0},
    r_shape=-e_b*(cylinderLength/2),
    r=frame_b.r_0,
    R=frame_b.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape shape_rod1(
    shapeType="cylinder",
    color=rodColor,
    specularCoefficient=specularCoefficient,
    length=Modelica.Math.Vectors.length(
                         rRod1_ia),
    width=rodDiameter,
    height=rodDiameter,
    lengthDirection = to_unit1(rRod1_ia),
    widthDirection=e_ia,
    r=frame_ia.r_0,
    R=frame_ia.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape shape_rod2(
    shapeType="cylinder",
    color=rodColor,
    specularCoefficient=specularCoefficient,
    length=Modelica.Math.Vectors.length(
                         rRod2_ib),
    width=rodDiameter,
    height=rodDiameter,
    lengthDirection = to_unit1(rRod2_ib),
    widthDirection=e_b,
    r=frame_ib.r_0,
    R=frame_ib.R) if world.enableAnimation and animation;
initial equation
  n_b = Frames.resolve2(frame_b.R, Frames.resolve1(frame_a.R, n_a));

equation
  connect(jointUSR.frame_a, frame_a)
  connect(jointUSR.frame_b, frame_b)
  connect(jointUSR.frame_ia, frame_ia)
  connect(jointUSR.frame_im, frame_im)
  connect(jointUSR.frame_ib, frame_ib)
  connect(jointUSR.axis, axis)
  connect(jointUSR.bearing, bearing)
end JointRRR;


Spherical - spherical - prismatic joint aggregation with mass (no constraints, no potential states). This component consists of a spherical joint 1 at frame_a, a prismatic
joint at frame_b and a spherical joint 2 which is connected via rod 1
to the spherical joint 1 and via rod 2 to the prismatic joint, see the default
animation in the following figure (the axes vectors are not part of the
default animation):

within Modelica.Mechanics.MultiBody.Joints.Assemblies;
model JointSSP
  "Spherical - spherical - prismatic joint aggregation with mass (no constraints, no potential states)"

  import Modelica.Mechanics.MultiBody.Types;

  extends Interfaces.PartialTwoFramesDoubleSize;
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_ib
    "Coordinate system at origin of frame_b fixed at connecting rod of spherical and prismatic joint"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_im
    "Coordinate system at origin of spherical joint in the middle fixed at connecting rod of spherical and prismatic joint"
  Modelica.Mechanics.Translational.Interfaces.Flange_a axis
    "1-dim. translational flange that drives the prismatic joint"
  Modelica.Mechanics.Translational.Interfaces.Flange_b bearing
    "1-dim. translational flange of the drive bearing of the prismatic joint"

  parameter Boolean animation=true "= true, if animation shall be enabled";
  parameter Boolean showMass=true
    "= true, if point mass on rod 1 shall be shown (provided animation = true and rod1Mass > 0)";
  parameter SI.Length rod1Length(min=Modelica.Constants.eps, start = 1)
    "Distance between the origins of the two spherical joints";
  parameter SI.Mass rod1Mass(min=0)=0
    "Mass of rod 1 (= point mass located in middle of rod connecting the two spherical joints)";
  parameter Modelica.Mechanics.MultiBody.Types.Axis n_b={0,0,1}
    "Axis of prismatic joint fixed and resolved in frame_b";
  parameter SI.Position rRod2_ib[3]={1,0,0}
    "Vector from origin of frame_ib to spherical joint in the middle, resolved in frame_ib";
  parameter SI.Position s_offset=0
    "Relative distance offset of prismatic joint (distance between frame_b and frame_ib = 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 two spherical joints"
  input Types.Color sphereColor=Modelica.Mechanics.MultiBody.Types.Defaults.
       JointColor
    "Color of the spheres representing the two spherical joints"
  parameter SI.Diameter rod1Diameter=sphereDiameter/Types.Defaults.
      JointRodDiameterFraction
    "Diameter of rod 1 connecting the two spherical joints"
  input Types.Color rod1Color=Modelica.Mechanics.MultiBody.Types.Defaults.
      RodColor "Color of rod 1 connecting the two spherical joint"

  parameter SI.Diameter rod2Diameter=rod1Diameter
    "Diameter of rod 2 connecting the revolute joint and spherical joint 2"
  input Types.Color rod2Color=rod1Color
    "Color of rod 2 connecting the revolute joint and spherical joint 2"

  parameter Types.Axis boxWidthDirection={0,1,0}
    "Vector in width direction of prismatic joint box, resolved in frame_b"
  parameter SI.Distance boxWidth=world.defaultJointWidth
    "Width of prismatic joint box"
  parameter SI.Distance boxHeight=boxWidth "Height of prismatic joint box"
  input Types.Color boxColor=Modelica.Mechanics.MultiBody.Types.Defaults.JointColor
    "Color of prismatic joint box"
  input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"
  parameter Boolean checkTotalPower=false
    "= true, if total power flowing into this component shall be determined (must be zero)"
  Real aux
    "Denominator used to compute force in rod connecting 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=rod1Length,
    n=n_b,
    s_offset=s_offset,
    s_guess=s_guess,
    boxWidthDirection=boxWidthDirection,
    boxWidth=boxWidth,
    boxHeight=boxHeight,
    specularCoefficient=specularCoefficient,
    boxColor=boxColor)
  Modelica.Mechanics.MultiBody.Joints.SphericalSpherical rod1(
    animation=animation,
    showMass=showMass,
    m=rod1Mass,
    rodLength=rod1Length,
    rodDiameter=rod1Diameter,
    sphereDiameter=sphereDiameter,
    rodColor=rod1Color,
    kinematicConstraint=false,
    specularCoefficient=specularCoefficient,
    sphereColor=sphereColor,
    constraintResidue=rod1.f_rod - f_rod)
  Modelica.Mechanics.MultiBody.Parts.FixedTranslation rod2(
    animation=animation,
    width=rod2Diameter,
    height=rod2Diameter,
    specularCoefficient=specularCoefficient,
    color=rod2Color,
    r=rRod2_ib)
  Sensors.RelativePosition relativePosition(resolveInFrame=Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB.frame_a)
  Modelica.Blocks.Sources.Constant position_b[3](k=rRod2_ib)
equation
  /* Compute the unknown force in the rod of the rod1 joint
     by a force balance:
       0 = frame_b.f + frame_ib.f + frame_im.f +
           Frames.resolve2(rod1.R_rel, rod1.f_rod*rod1.eRod_a)
     The condition is that the projection of the force in the prismatic
     joint along the axis of the prismatic joint is equal to the driving
     axis force in the flange:
       -prismatic.f = prismatic.e*frame_b.f
     Therefore, we have with e=prismatic.e and f=prismatic.f
        f = e*(frame_ib.f + frame_im.f +
               Frames.resolve2(rod1.R_rel, rod1.f_rod*rod1.eRod_a))
          = e*(frame_ib.f + frame_im.f +
               rod1.f_rod*Frames.resolve2(rod1.R_rel, rod1.eRod_a))
     Solving this equation for f_rod results in
       rod1.f_rod = (f - e*(frame_ib.f + frame_im.f))
                    / (e*Frames.resolve2(rod1.R_rel, rod1.eRod_a))
     Additionally, a guard against division by zero is introduced
  */
  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))/
    noEvent(if abs(aux) < 1e-10 then 1e-10 else aux);

  // Measure power for test purposes
  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_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_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) + (-rod1Mass)*(der(rod1.v_CM_0) -
      world.gravityAcceleration(rod1.r_CM_0))*rod1.v_CM_0;
  else
    totalPower = 0;
  end if;

  connect(prismatic.frame_b, rod2.frame_a)
  connect(rod2.frame_b, rod1.frame_b)
  connect(prismatic.frame_a, frame_b)
  connect(rod2.frame_a, frame_ib)
  connect(rod1.frame_a, frame_a)
  connect(relativePosition.frame_b, frame_a)
  connect(relativePosition.frame_a, frame_b)
  connect(position_b.y, prismatic.position_b)
  connect(prismatic.axis, axis)
  connect(prismatic.bearing, bearing)
  connect(rod2.frame_b, frame_im)
  connect(relativePosition.r_rel, prismatic.position_a)
end JointSSP;


Spherical - spherical - revolute joint aggregation with mass (no constraints, no potential states). This component consists of a spherical joint 1 at frame_a, a revolute
joint at frame_b and a spherical joint 2 which is connected via rod 1
to the spherical joint 1 and via rod 2 to the revolute joint, see the default
animation in the following figure (the axes vectors are not part of the
default animation):

within Modelica.Mechanics.MultiBody.Joints.Assemblies;
model JointSSR
  "Spherical - spherical - revolute joint aggregation with mass (no constraints, no potential states)"

  import Modelica.Mechanics.MultiBody.Types;

  extends Interfaces.PartialTwoFramesDoubleSize;
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_ib
    "Coordinate system at origin of frame_b fixed at connecting rod of spherical and revolute joint"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_im
    "Coordinate system at origin of spherical joint in the middle fixed at connecting rod of spherical and revolute joint"
  Modelica.Mechanics.Rotational.Interfaces.Flange_a axis
    "1-dim. rotational flange that drives the revolute joint"
  Modelica.Mechanics.Rotational.Interfaces.Flange_b bearing
    "1-dim. rotational flange of the drive bearing of the revolute joint"

  parameter Boolean animation=true "= true, if animation shall be enabled";
  parameter Boolean showMass=true
    "= true, if point mass on rod 1 shall be shown (provided animation = true and rod1Mass > 0)";
  parameter SI.Length rod1Length(min=Modelica.Constants.eps, start = 1)
    "Distance between the origins of the two spherical joints";
  parameter SI.Mass rod1Mass(min=0)=0
    "Mass of rod 1 (= point mass located in middle of rod connecting the two spherical joints)";
  parameter Modelica.Mechanics.MultiBody.Types.Axis n_b={0,0,1}
    "Axis of revolute joint fixed and resolved in frame_b";
  parameter SI.Position rRod2_ib[3]={1,0,0}
    "Vector from origin of frame_ib to spherical joint in the middle, resolved in frame_ib";
  parameter Modelica.Units.NonSI.Angle_deg phi_offset=0
    "Relative angle offset of revolute joint (angle = phi(t) + from_deg(phi_offset))";
  parameter Modelica.Units.NonSI.Angle_deg phi_guess=0
    "Select the configuration such that at initial time |phi(t0) - from_deg(phi_guess)| is minimal";
  parameter SI.Diameter sphereDiameter=world.defaultJointLength
    "Diameter of the spheres representing the two spherical joints"
  input Types.Color sphereColor=Modelica.Mechanics.MultiBody.Types.Defaults.
       JointColor
    "Color of the spheres representing the two spherical joints"
  parameter SI.Diameter rod1Diameter=sphereDiameter/Types.Defaults.
      JointRodDiameterFraction
    "Diameter of rod 1 connecting the two spherical joints"
  input Types.Color rod1Color=Modelica.Mechanics.MultiBody.Types.Defaults.
      RodColor "Color of rod 1 connecting the two spherical joint"
  parameter SI.Diameter rod2Diameter=rod1Diameter
    "Diameter of rod 2 connecting the revolute joint and spherical joint 2"
  input Types.Color rod2Color=rod1Color
    "Color of rod 2 connecting the revolute joint and spherical joint 2"
  parameter SI.Diameter revoluteDiameter=world.defaultJointWidth
    "Diameter of cylinder representing the revolute joint"
  parameter SI.Distance revoluteLength=world.defaultJointLength
    "Length of cylinder representing the revolute joint"
  input Types.Color revoluteColor=Modelica.Mechanics.MultiBody.Types.
      Defaults.JointColor
    "Color of cylinder representing the revolute joint"
  input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"
  parameter Boolean checkTotalPower=false
    "= true, if total power flowing into this component shall be determined (must be zero)"
  SI.Position aux
    "Denominator used to compute force in rod connecting 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.RevoluteWithLengthConstraint revolute(
    animation=animation,
    lengthConstraint=rod1Length,
    n=n_b,
    phi_offset=phi_offset,
    phi_guess=phi_guess,
    cylinderDiameter=revoluteDiameter,
    cylinderLength=revoluteLength,
    cylinderColor=revoluteColor,
    specularCoefficient=specularCoefficient)
  Modelica.Mechanics.MultiBody.Joints.SphericalSpherical rod1(
    animation=animation,
    showMass=showMass,
    m=rod1Mass,
    rodLength=rod1Length,
    rodDiameter=rod1Diameter,
    sphereDiameter=sphereDiameter,
    rodColor=rod1Color,
    specularCoefficient=specularCoefficient,
    kinematicConstraint=false,
    sphereColor=sphereColor,
    constraintResidue=rod1.f_rod - f_rod)
  Modelica.Mechanics.MultiBody.Parts.FixedTranslation rod2(
    animation=animation,
    width=rod2Diameter,
    height=rod2Diameter,
    color=rod2Color,
    specularCoefficient=specularCoefficient,
    r=rRod2_ib)
  Sensors.RelativePosition relativePosition(resolveInFrame=Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB.frame_a)
  Modelica.Blocks.Sources.Constant position_b[3](k=rRod2_ib)
equation
  /* Compute the unknown force in the rod of the rod1 joint
     by a torque balance at the revolute joint:
       0 = frame_b.t + frame_ib.t + frame_im.t + cross(rRod2_ib, frame_im.f)
           + cross(rRod2_ib, -rod1.f_b_a1)
           + cross(rRod2_ib, Frames.resolve2(rod1.R_rel, rod1.f_rod*rod1.eRod_a))
     The condition is that the projection of the torque in the revolute
     joint along the axis of the revolute joint is equal to the driving
     axis torque in the flange:
       -revolute.tau = revolute.e*frame_b.t
     Therefore, we have with e=revolute.e and tau=revolute.tau
        tau = e*(frame_ib.t  + frame_im.t + cross(rRod2_ib, frame_im.f)
              + cross(rRod2_ib, -rod1.f_b_a1))
              + e*cross(rRod2_ib, Frames.resolve2(rod1.R_rel, rod1.f_rod*rod1.eRod_a))
            = e*(frame_ib.t + frame_im.t + cross(rRod2_ib, frame_im.f)
              + cross(rRod2_ib, -rod.f_b_a1))
              + rod1.f_rod*e*cross(rRod2_ib, Frames.resolve2(rod1.R_rel, rod1.eRod_a))
     Solving this equation for f_rod results in
       rod1.f_rod = (tau - e*(frame_ib.t + frame_im.t + cross(rRod2_ib, frame_im.f)
                   + cross(rRod2_ib, -rod1.f_b_a1)))
                   / (cross(e,rRod2_ib)*Frames.resolve2(rod1.R_rel, rod1.eRod_a))
     Additionally, a guard against division by zero is introduced
  */

  aux = cross(revolute.e, rRod2_ib)*Frames.resolveRelative(rod1.eRod_a,
    rod1.frame_a.R, rod1.frame_b.R);
  f_rod = (-revolute.tau - revolute.e*(frame_ib.t + frame_im.t + cross(
    rRod2_ib, frame_im.f) - cross(rRod2_ib, 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);

  // Measure power for test purposes
  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_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_ib.t*
      Frames.angularVelocity2(frame_ib.R) + frame_im.t*
      Frames.angularVelocity2(frame_im.R) + axis.tau*der(axis.phi) +
      bearing.tau*der(bearing.phi) + (-rod1Mass)*(der(rod1.v_CM_0) -
      world.gravityAcceleration(rod1.r_CM_0))*rod1.v_CM_0;
  else
    totalPower = 0;
  end if;

  connect(revolute.frame_b, rod2.frame_a)
  connect(rod2.frame_b, rod1.frame_b)
  connect(revolute.frame_a, frame_b)
  connect(rod2.frame_a, frame_ib)
  connect(rod1.frame_a, frame_a)
  connect(relativePosition.frame_b, frame_a)
  connect(relativePosition.frame_a, frame_b)
  connect(position_b.y, revolute.position_b)
  connect(revolute.axis, axis)
  connect(rod2.frame_b, frame_im)
  connect(relativePosition.r_rel, revolute.position_a)
  connect(revolute.bearing, bearing)
end JointSSR;


Universal - prismatic - spherical joint aggregation (no constraints, no potential states). This component consists of a universal joint at frame_a,
a spherical joint at frame_b and a prismatic joint along the
line connecting the origin of frame_a and the origin of frame_b,
see the default animation in the following figure (the axes vectors
are not part of the default animation):

within Modelica.Mechanics.MultiBody.Joints.Assemblies;
model JointUPS
  "Universal - prismatic - spherical 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 prismatic joint"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_ib
    "Coordinate system at origin of frame_b fixed at prismatic joint"
  Modelica.Mechanics.Translational.Interfaces.Flange_a axis
    "1-dim. translational flange that drives the prismatic joint"
  Modelica.Mechanics.Translational.Interfaces.Flange_b bearing
    "1-dim. translational flange of the drive bearing of the prismatic joint"

  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 resolved in frame_a (axis 2 is orthogonal to axis 1 and to line from universal to spherical joint)"
  parameter SI.Position nAxis_ia[3]={1,0,0}
    "Axis vector along line from origin of frame_a to origin of frame_b, resolved in frame_ia"
  parameter SI.Position s_offset=0
    "Relative distance offset (distance between frame_a and frame_b = s(t) + s_offset)";
  parameter SI.Diameter sphereDiameter=world.defaultJointLength
    "Diameter of spheres representing the spherical joints"
  input Types.Color sphereColor=Modelica.Mechanics.MultiBody.Types.Defaults.JointColor
    "Color of spheres representing the spherical joints"
  parameter SI.Diameter axisDiameter=sphereDiameter/Types.Defaults.
      JointRodDiameterFraction
    "Diameter of cylinder on the connecting line from frame_a to frame_b"
  input Types.Color axisColor=Modelica.Mechanics.MultiBody.Types.Defaults.SensorColor
    "Color of cylinder on the connecting line from frame_a to frame_b"
  input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"
  parameter SI.Distance cylinderLength=world.defaultJointLength
    "Length of cylinders representing the two universal joint axes"
  parameter SI.Distance cylinderDiameter=world.defaultJointWidth
    "Diameter of cylinders representing the two universal joint axes"
 input Types.Color cylinderColor=Modelica.Mechanics.MultiBody.Types.Defaults.JointColor
    "Color of cylinders representing the two universal joint axes"

  parameter Boolean checkTotalPower=false
    "= true, if total power flowing into this component shall be determined (must be zero)"
  final parameter Real eAxis_ia[3](each final unit="1")=Modelica.Math.Vectors.normalizeWithAssert(nAxis_ia)
    "Unit vector from origin of frame_a to origin of frame_b, resolved in frame_ia";
  final parameter Real e2_ia[3](each final unit="1")=Modelica.Math.Vectors.normalizeWithAssert(
                                                 cross(n1_a, eAxis_ia))
    "Unit vector in direction of second rotation axis of universal joint, resolved in frame_ia";
  final parameter Real e3_ia[3](each final unit="1")=cross(eAxis_ia, e2_ia)
    "Unit vector perpendicular to eAxis_ia and e2_ia, resolved in frame_ia";
  SI.Position s
    "Relative distance between frame_a and frame_b along axis nAxis = s + s_offset";
  SI.Force f "= axis.f (driving force in the axis; = -bearing.f)";
  SI.Length axisLength "Distance between frame_a and frame_b";
  SI.Power totalPower
    "Total power flowing into this element, if checkTotalPower=true (otherwise dummy)";

protected
  SI.Force f_c_a[3] "frame_ia.f resolved in frame_a";
  SI.Torque t_cd_a[3] "frame_ia.t + frame_ib.t resolved in frame_a";
  SI.Force f_bd_a[3] "frame_b.f + frame_ib.f resolved in frame_a";
  SI.Position rAxis_0[3]
    "Position vector from origin of frame_a to origin of frame_b resolved in world frame";
  SI.Position rAxis_a[3]
    "Position vector from origin of frame_a to origin of frame_b resolved in frame_a";
  Real eAxis_a[3](each final unit="1")
    "Unit vector in direction of rAxis_a, resolved in frame_a";
  Real e2_a[3](each final unit="1")
    "Unit vector in direction of second rotation axis of universal joint, resolved in frame_a";
  Real e3_a[3](each final unit="1")
    "Unit vector perpendicular to eAxis_a and e2_a, resolved in frame_a";
  Real n2_a[3](each final unit="1")
    "Vector in direction of second rotation axis of universal joint, resolved in frame_a";
  Real length2_n2_a(unit="1") "Square of length of vector n2_a";
  Real length_n2_a(unit="1") "Length of vector n2_a";
  Real der_rAxis_a_L[3](each unit="1/s") "= der(rAxis_a)/axisLength";
  SI.AngularVelocity w_rel_ia1[3];
  Frames.Orientation R_ia1_a;
  Frames.Orientation R_ia2_a;
  Frames.Orientation R_ia_a "Rotation from frame_a to frame_ia";
  // Real T_ia_a[3, 3] "Transformation matrix from frame_a to frame_ia";

  Visualizers.Advanced.Shape axisCylinder(
    shapeType="cylinder",
    color=axisColor,
    specularCoefficient=specularCoefficient,
    length=axisLength,
    width=axisDiameter,
    height=axisDiameter,
    lengthDirection=eAxis_ia,
    widthDirection=e2_ia,
    r=frame_ia.r_0,
    R=frame_ia.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape sphericalShape_b(
    shapeType="sphere",
    color=sphereColor,
    specularCoefficient=specularCoefficient,
    length=sphereDiameter,
    width=sphereDiameter,
    height=sphereDiameter,
    lengthDirection={1,0,0},
    widthDirection={0,1,0},
    r_shape={-0.5,0,0}*sphereDiameter,
    r=frame_b.r_0,
    R=frame_b.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape sphericalShape_a(
    shapeType="sphere",
    color=sphereColor,
    specularCoefficient=specularCoefficient,
    length=sphereDiameter,
    width=sphereDiameter,
    height=sphereDiameter,
    lengthDirection={1,0,0},
    widthDirection={0,1,0},
    r_shape={-0.5,0,0}*sphereDiameter,
    r=frame_a.r_0,
    R=frame_a.R) if world.enableAnimation and animation;
  Visualizers.Advanced.Shape universalShape1(
    shapeType="cylinder",
    color=cylinderColor,
    specularCoefficient=specularCoefficient,
    length=cylinderLength,
    width=cylinderDiameter,
    height=cylinderDiameter,
    lengthDirection=n1_a,
    widthDirection={0,1,0},
    r_shape=-n1_a*(cylinderLength/2),
    r=frame_a.r_0,
    R=frame_a.R) if world.enableAnimation and animation and showUniversalAxes;
  Visualizers.Advanced.Shape universalShape2(
    shapeType="cylinder",
    color=cylinderColor,
    specularCoefficient=specularCoefficient,
    length=cylinderLength,
    width=cylinderDiameter,
    height=cylinderDiameter,
    lengthDirection=e2_ia,
    widthDirection={0,1,0},
    r_shape=-e2_ia*(cylinderLength/2),
    r=frame_ia.r_0,
    R=frame_ia.R) if world.enableAnimation and animation and showUniversalAxes;
equation
  Connections.branch(frame_a.R, frame_ia.R);
  Connections.branch(frame_ia.R, frame_ib.R);

  // Translational flanges
  axisLength = s + s_offset;
  bearing.s = 0;
  axis.s = s;
  axis.f = f;

  // Position vector rAxis from frame_a to frame_b
  rAxis_0 = frame_b.r_0 - frame_a.r_0;
  rAxis_a = Frames.resolve2(frame_a.R, rAxis_0);

  /* Determine relative Rotation R_rel_c from frame_a to frame_ia
     and absolute rotation of frame_a.R.
  */
  axisLength = sqrt(rAxis_0*rAxis_0);
  assert(axisLength > 1.0e-15, "
Distance between frame_a and frame_b of a JointUPS joint
became zero. This is not allowed. If this occurs during
initialization, the initial conditions are probably wrong.");

  eAxis_a = rAxis_a/axisLength;
  n2_a = cross(n1_a, eAxis_a);
  length2_n2_a = n2_a*n2_a;
  assert(noEvent(length2_n2_a > 1e-10), "
A Modelica.Mechanics.MultiBody.Joints.Assemblies.JointUPS joint (consisting of
a universal, prismatic and spherical joint) is in the singular
configuration of the universal joint. This means that axis 1 of
the universal joint defined via parameter \"n1_a\" is parallel to vector
\"eAxis_ia\" that is directed from the origin of frame_a to the
origin of frame_b. You may try to use another \"n1_a\" vector.
");
  length_n2_a = sqrt(length2_n2_a);
  e2_a = n2_a/length_n2_a;
  e3_a = cross(eAxis_a, e2_a);

  /* The statements below are an efficient implementation of the
     original equations:
       T_ia_a = [eAxis_ia, e2_ia, e3_ia]*transpose([eAxis_a, e2_a, e3_a]);
       R_ia_a = Frames.from_T(T_ia_a,
                     Frames.TransformationMatrices.angularVelocity2(T_ia_a, der(T_ia_a)));
   To perform this, the rotation is split into two parts:
     R_ia_a : Rotation object from frame_a to frame_ia
     R_ia1_a: Rotation object from frame_a to frame_ia1
                (frame that is fixed in frame_ia such that x-axis
                is along the rod axis)
                T = transpose([eAxis_a, e2_a, e3_a]; w = w_rel_ia1
     R_ia2_a: Fixed rotation object from frame_ia1 to frame_ia
                T = [eAxis_a, e2_ia, e3_ia]; w = zeros(3)

   The difficult part is to compute w_rel_ia1:
      w_rel_ia1 = [  e3_a*der(e2_a);
                    -e3_a*der(eAxis_a);
                     e2_a*der(eAxis_a)]
   der(eAxis_a) is directly given, since eAxis_a is a function
   of translational quantities only.
      der(eAxis_a) = (der(rAxis_a) - eAxis_a*(eAxis_a*der(rAxis_a)))/axisLength
      der(n2_a)    = cross(n1_a, der(eAxis_a))
      der(e2_a)    = (der(n2_a) - e2_a*(e2_a*der(n2_a)))/length_n2_a
   Inserting these equations in w_rel_ia1 results in:
      e3_a*der(eAxis_a) = e3_a*der(rAxis_a)/axisLength       // e3_a*eAxis_a = 0
      e2_a*der(eAxis_a) = e2_a*der(rAxis_a)/axisLength       // e2_a*eAxis_a = 0
      e3_a*der(e2_a)    = e3_a*der(n2_a)/length_n2_a       // e3_a*e2_a = 0
                        = e3_a*cross(n1_a, der(eAxis_a))/length_n2_a
                        = e3_a*cross(n1_a, der(rAxis_a) - eAxis_a*(eAxis_a*der(rAxis_a)))/(length_n2_a*axisLength)
                        = e3_a*cross(n1_a, der(rAxis_a))/(length_n2_a*axisLength)
   Furthermore, we have:
     rAxis_a      = resolve2(frame_a.R, rAxis_0);
     der(rAxis_a) = resolve2(frame_a.R, der(rAxis_0)) - cross(frame_a.R.w, rAxis_a));
*/
  der_rAxis_a_L = (Frames.resolve2(frame_a.R, der(rAxis_0)) - cross(frame_a.
     R.w, rAxis_a))/axisLength;
  w_rel_ia1 = {e3_a*cross(n1_a, der_rAxis_a_L)/length_n2_a,-e3_a*
    der_rAxis_a_L,e2_a*der_rAxis_a_L};
  R_ia1_a = Frames.from_T(transpose([eAxis_a, e2_a, e3_a]), w_rel_ia1);
  R_ia2_a = Frames.from_T([eAxis_ia, e2_ia, e3_ia], zeros(3));
  R_ia_a = Frames.absoluteRotation(R_ia1_a, R_ia2_a);
  /*
  T_ia_a = [eAxis_ia, e2_ia, e3_ia]*transpose([eAxis_a, e2_a, e3_a]);
  R_ia_a = Frames.from_T(T_ia_a, Frames.TransformationMatrices.angularVelocity2
    (T_ia_a, der(T_ia_a)));
*/

  // Compute kinematic quantities of frame_ia and frame_ib
  frame_ia.r_0 = frame_a.r_0;
  frame_ib.r_0 = frame_b.r_0;
  frame_ia.R = Frames.absoluteRotation(frame_a.R, R_ia_a);
  frame_ib.R = frame_ia.R;

  /* In the following formulas f_a, f_b, f_ia, f_ib, t_a, t_b, t_ia, t_ib are
     the forces and torques at frame_a, frame_b, frame_ia, frame_ib respectively,
     resolved in frame_a. eAxis, e2, e3 are the unit vectors resolved in frame_a.
     Torque balance at the rod around the origin of frame_a:
       0 = t_a + t_ia + t_ib + cross(rAxis, (f_b+f_ib))
     with
         rAxis = axisLength*eAxis
         f_bd  = f_b + f_ib
         f_bd  = f*eAxis + f_bd[2]*e2 + f_bd[3]*e3
     follows:
         0 = t_a + t_ia + axisLength*(f_bd[2]*e_z - f_bd[3]*e_y)
     The projection of t_a with respect to universal joint axes vanishes:
       e1*t_a = 0
       e2*t_a = 0
     Therefore:
        0 = e1*(t_ia + t_ib) + axisLength*f_bd[2]*(e1*e3)
        0 = e2*(t_ia + t_ib) - axisLength*f_bd[3]
     or
        f_bd = f*eAxis - e2*(e1*(t_ia+t_ib))/(axisLength*(e1*e3)) +
                e3*(e2*(t_ia+t_ib))/axisLength
     Force balance:
        0 = f_a + f_bd + f_ia
  */
  f_c_a = Frames.resolve1(R_ia_a, frame_ia.f);
  t_cd_a = Frames.resolve1(R_ia_a, frame_ia.t + frame_ib.t);
  f_bd_a = -eAxis_a*f - e2_a*((n1_a*t_cd_a)/(axisLength*(n1_a*e3_a))) +
    e3_a*((e2_a*t_cd_a)/axisLength);
  zeros(3) = frame_b.f + Frames.resolveRelative(frame_ib.f, frame_ib.R,
    frame_b.R) - Frames.resolveRelative(f_bd_a, frame_a.R, frame_b.R);
  zeros(3) = frame_b.t;
  zeros(3) = frame_a.f + f_c_a + f_bd_a;
  zeros(3) = frame_a.t + t_cd_a + cross(rAxis_a, f_bd_a);

  // Measure power for test purposes
  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_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) + axis.f*der(axis.s) + bearing.f*
      der(bearing.s);
  else
    totalPower = 0;
  end if;
end JointUPS;


Universal - spherical - prismatic joint aggregation (no constraints, no potential states). This component consists of a universal joint at frame_a, a prismatic
joint at frame_b and a spherical joint which is connected via rod1
to the universal and via rod2 to the prismatic joint, see the default
animation in the following figure (the axes vectors are not part of the
default animation):

within Modelica.Mechanics.MultiBody.Joints.Assemblies;
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"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_ib
    "Coordinate system at origin of frame_b fixed at connecting rod of spherical and prismatic joint"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_im
    "Coordinate system at origin of spherical joint fixed at connecting rod of spherical and prismatic joint"
  Modelica.Mechanics.Translational.Interfaces.Flange_a axis
    "1-dim. translational flange that drives the prismatic joint"
  Modelica.Mechanics.Translational.Interfaces.Flange_b bearing
    "1-dim. translational flange of the drive bearing of the prismatic joint"

  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)"
  parameter Modelica.Mechanics.MultiBody.Types.Axis n_b={-1,0,0}
    "Axis of prismatic joint fixed and resolved in frame_b"
  parameter SI.Position rRod1_ia[3]={1,0,0}
    "Vector from origin of frame_a to spherical joint, resolved in frame_ia"
  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)"
  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"
  input Types.Color sphereColor=Modelica.Mechanics.MultiBody.Types.Defaults.JointColor
    "Color of the spheres representing the universal and the spherical joint"
  parameter SI.Diameter rod1Diameter=sphereDiameter/Types.Defaults.
      JointRodDiameterFraction
    "Diameter of rod 1 connecting the universal and the spherical joint"
  input Types.Color rod1Color=Modelica.Mechanics.MultiBody.Types.Defaults.RodColor
    "Color of rod 1 connecting the universal and the spherical joint"
  parameter SI.Diameter rod2Diameter=rod1Diameter
    "Diameter of rod 2 connecting the prismatic and the spherical joint"
  input Types.Color rod2Color=rod1Color
    "Color of rod 2 connecting the prismatic and the spherical joint"
  parameter Types.Axis boxWidthDirection={0,1,0}
    "Vector in width direction of prismatic joint, resolved in frame_b"
  parameter SI.Distance boxWidth=world.defaultJointWidth
    "Width of prismatic joint box"
  parameter SI.Distance boxHeight=boxWidth "Height of prismatic joint box"
  input Types.Color boxColor=sphereColor "Color of prismatic joint box"
  input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"
  parameter SI.Distance cylinderLength=world.defaultJointLength
    "Length of cylinders representing the two universal joint axes"
  parameter SI.Distance cylinderDiameter=world.defaultJointWidth
    "Diameter of cylinders representing the two universal joint axes"
  input Types.Color cylinderColor=Modelica.Mechanics.MultiBody.Types.Defaults.JointColor
    "Color of cylinders representing the two universal joint axes"
  parameter Boolean checkTotalPower=false
    "= true, if total power flowing into this component shall be determined (must be zero)"
  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)
  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)
  Modelica.Mechanics.MultiBody.Parts.FixedTranslation rod2(
    animation=animation,
    r=rRod2_ib,
    width=rod2Diameter,
    height=rod2Diameter,
    specularCoefficient=specularCoefficient,
    color=rod2Color)
  Sensors.RelativePosition relativePosition(resolveInFrame=Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB.frame_a)
  Modelica.Blocks.Sources.Constant position_b[3](k=rRod2_ib)
protected
  Real aux
    "Denominator used to compute force in rod connecting universal and spherical joint";
equation
  /* Compute the unknown force in rod1 connecting the universal and
     the spherical joint by a force balance at the prismatic joint
        0 = -prismatic.frame_b.f + frame_ib.f + frame_im.f - rod1.frame_b.f
     The force at rod1.frame_b is split into two parts:
        rod1.frame_b.f = Frames.resolve2(rod1.R_rel, rod1.f_b_a1 - rod1.f_rod*rod1.eRod_a)
     where rod1.f_rod is the unknown force in rod1.
     The condition is that the projection of the force in the prismatic
     joint along the axis of its translation axis is equal to the driving
     axis force in the flange:
       -prismatic.f = prismatic.e*prismatic.frame_b.f
     Therefore, we have with e=prismatic.e and f=prismatic.f
       -f = e*(frame_ib.f + frame_im.f
               - Frames.resolve2(rod1.R_rel, rod1.f_b_a1 - rod1.f_rod*rod1.eRod_a))
          = e*(frame_ib.f + frame_im.f - Frames.resolve2(rod1.R_rel, rod1.f_b_a1)
              + rod1.f_rod*Frames.resolve2(rod1.R_rel, rod1.eRod_a))
     Solving this equation for f_rod results in
       rod1.f_rod = -(f+e*(frame_ib.f + frame_im.f - Frames.resolve2(rod1.R_rel, rod1.f_b_a1)))
                   /(e*Frames.resolve2(rod1.R_rel, rod1.eRod_a))
     Additionally, a guard against division by zero is introduced
  */
  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);
  // Measure power for test purposes
  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.frame_b, rod2.frame_a)
  connect(rod2.frame_b, rod1.frame_b)
  connect(prismatic.frame_a, frame_b)
  connect(rod2.frame_a, frame_ib)
  connect(rod1.frame_a, frame_a)
  connect(relativePosition.frame_b, frame_a)
  connect(relativePosition.frame_a, frame_b)
  connect(rod2.frame_b, frame_im)
  connect(rod1.frame_ia, frame_ia)
  connect(position_b.y, prismatic.position_b)
  connect(prismatic.axis, axis)
  connect(prismatic.bearing, bearing)
  connect(relativePosition.r_rel, prismatic.position_a)
  Modelica.Mechanics.MultiBody.Visualizers.FixedShape     shape(shapeType       = \"box\",
                                             lengthDirection = jointUSP.eRod1_ia,
                                             widthDirection  = jointUSP.e2_ia,
                                             length          = jointUSP.rod1Length/2,
                                             width           = jointUSP.rod1Length/10);
equation
  connect(jointUSP.frame_ia, shape.frame_a);

"),
    Icon(coordinateSystem(
        preserveAspectRatio=true,
        extent={{-100,-100},{100,100}},
        initialScale=0.2), graphics={
        Rectangle(
          extent={{10,6},{52,-6}},
          fillPattern=FillPattern.HorizontalCylinder,
          fillColor={192,192,192}),
        Ellipse(
          extent={{-34,-24},{14,24}},
          startAngle=-110,
          endAngle=110,
          lineColor={0,0,0},
          fillColor={192,192,192},
          fillPattern=FillPattern.Solid,
          closure=EllipseClosure.Radial),
        Ellipse(
          extent={{-28,-18},{8,18}},
          fillColor={255,255,255},
          fillPattern=FillPattern.Solid,
          startAngle=-115,
          endAngle=115,
          lineColor={255,255,255}),
          Ellipse(
          extent={{-28,-18},{8,18}},
          startAngle=-110,
          endAngle=110,
          lineColor={0,0,0},
          closure=EllipseClosure.None),
        Ellipse(
          extent={{-100,-30},{-40,30}},
          startAngle=90,
          endAngle=270,
          lineColor={64,64,64},
          fillColor={192,192,192},
          fillPattern=FillPattern.Solid,
          closure=EllipseClosure.Radial),
        Ellipse(
          extent={{-94,-24},{-46,24}},
          fillColor={255,255,255},
          fillPattern=FillPattern.Solid,
          startAngle=85,
          endAngle=275,
          lineColor={255,255,255}),
          Ellipse(
          extent={{-94,-24},{-46,24}},
          startAngle=90,
          endAngle=270,
          lineColor={64,64,64},
          closure=EllipseClosure.None),
        Rectangle(
          extent={{-50,5},{-10,-5}},
          fillPattern=FillPattern.HorizontalCylinder,
          fillColor={192,192,192}),
          Ellipse(
            extent={{-20,10},{0,-10}},
            fillPattern=FillPattern.Sphere,
            fillColor={192,192,192}),
        Line(
          points={{-70,28},{-70,-30}},
          thickness=0.5),
        Ellipse(
          extent={{-92,-14},{-48,14}},
          fillPattern=FillPattern.Solid,
          fillColor={192,192,192},
          startAngle=-123,
          endAngle=57,
          lineColor={64,64,64}),
        Line(
          points={{-50,0},{-50,80},{-80,80},{-80,100}},
          color={95,95,95},
          thickness=0.5),
        Line(
          points={{14,0},{14,80},{0,80},{0,100}},
          color={95,95,95},
          thickness=0.5),
        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),
        Line(
          points={{50,6},{50,80},{80,80},{80,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),
        Text(
          extent={{-124,110},{-95,90}},
          textColor={128,128,128},
          textString="ia"),
        Text(
          extent={{-43,110},{-10,90}},
          textColor={128,128,128},
          textString="im"),
        Text(
          extent={{40,110},{77,90}},
          textColor={128,128,128},
          textString="ib"),
        Ellipse(
          extent={{-86,-8},{-54,8}},
          lineColor={255,255,255},
          fillColor={255,255,255},
          fillPattern=FillPattern.Solid,
          startAngle=0,
          endAngle=360),
        Ellipse(
          extent={{-86,-8},{-54,8}},
          lineColor={64,64,64},
          fillColor={255,255,255},
          fillPattern=FillPattern.Solid,
          startAngle=-114,
          endAngle=61,
          closure=EllipseClosure.None),
        Line(
          points={{-70,30},{-70,-8}},
          thickness=0.5),
        Line(
          points={{-56,14},{-84,-14}},
          thickness=0.5),
        Text(
          extent={{-75,-50},{75,-70}},
          textColor={0,0,255},
          textString="%name")}),
    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}},
          textColor={128,128,128},
          textString="rRod1_ia"),
        Text(
          extent={{-27,40},{0,33}},
          textColor={128,128,128},
          textString="rRod2_ib"),
        Line(
          points={{3,30},{-43,30}},
          color={128,128,128},
          arrow={Arrow.None,Arrow.Filled})}));
end JointUSP;


Universal - spherical - revolute joint aggregation (no constraints, no potential states). This component consists of a universal joint at frame_a, a revolute
joint at frame_b and a spherical joint which is connected via rod1
to the universal and via rod2 to the revolute joint, see the default
animation in the following figure (the axes vectors are not part of the
default animation):

within Modelica.Mechanics.MultiBody.Joints.Assemblies;
model JointUSR
  "Universal - spherical - revolute 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"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_ib
    "Coordinate system at origin of frame_b fixed at connecting rod of spherical and revolute joint"
  Modelica.Mechanics.MultiBody.Interfaces.Frame_b frame_im
    "Coordinate system at origin of spherical joint fixed at connecting rod of spherical and revolute joint"
  Modelica.Mechanics.Rotational.Interfaces.Flange_a axis
    "1-dim. rotational flange that drives the revolute joint"
  Modelica.Mechanics.Rotational.Interfaces.Flange_b bearing
    "1-dim. rotational flange of the drive bearing of the revolute joint"

  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)"
  parameter Modelica.Mechanics.MultiBody.Types.Axis n_b={0,0,1}
    "Axis of revolute joint fixed and resolved in frame_b"
  parameter SI.Position rRod1_ia[3]={1,0,0}
    "Vector from origin of frame_a to spherical joint, resolved in frame_ia"
  parameter SI.Position rRod2_ib[3]={-1,0,0}
    "Vector from origin of frame_ib to spherical joint, resolved in frame_ib";
  parameter Modelica.Units.NonSI.Angle_deg phi_offset=0
    "Relative angle offset of revolute joint (angle = phi(t) + from_deg(phi_offset))";
  parameter Modelica.Units.NonSI.Angle_deg phi_guess=0
    "Select the configuration such that at initial time |phi(t0) - from_deg(phi_guess)| is minimal";
  parameter SI.Diameter sphereDiameter=world.defaultJointLength
    "Diameter of the spheres representing the universal and the spherical joint"
  input Types.Color sphereColor=Modelica.Mechanics.MultiBody.Types.Defaults.
       JointColor
    "Color of the spheres representing the universal and the spherical joint"
  parameter SI.Diameter rod1Diameter=sphereDiameter/Types.Defaults.
      JointRodDiameterFraction
    "Diameter of rod 1 connecting the universal and the spherical joint"
  input Types.Color rod1Color=Modelica.Mechanics.MultiBody.Types.Defaults.
      RodColor
    "Color of rod 1 connecting the universal and the spherical joint"

  parameter SI.Diameter rod2Diameter=rod1Diameter
    "Diameter of rod 2 connecting the revolute and the spherical joint"
  input Types.Color rod2Color=rod1Color
    "Color of rod 2 connecting the revolute and the spherical joint"
  parameter SI.Diameter revoluteDiameter=world.defaultJointWidth
    "Diameter of cylinder representing the revolute joint"
  parameter SI.Distance revoluteLength=world.defaultJointLength
    "Length of cylinder representing the revolute joint"
  input Types.Color revoluteColor=Modelica.Mechanics.MultiBody.Types.
      Defaults.JointColor
    "Color of cylinder representing the revolute joint"
  input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"
  parameter SI.Distance cylinderLength=world.defaultJointLength
    "Length of cylinders representing the two universal joint axes"
  parameter SI.Distance cylinderDiameter=world.defaultJointWidth
    "Diameter of cylinders representing the two universal joint axes"
  input Types.Color cylinderColor=Modelica.Mechanics.MultiBody.Types.Defaults.JointColor
    "Color of cylinders representing the two universal joint axes"
  parameter Boolean checkTotalPower=false
    "= true, if total power flowing into this component shall be determined (must be zero)"
  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.Power totalPower
    "Total power flowing into this element, if checkTotalPower=true (otherwise dummy)";
  SI.Position aux
    "Denominator used to compute force in rod connecting universal and spherical joint";
  SI.Force f_rod
    "Constraint force in direction of the rod (positive, if rod is pressed)";

  Modelica.Mechanics.MultiBody.Joints.Internal.RevoluteWithLengthConstraint revolute(
    animation=animation,
    lengthConstraint=rod1Length,
    n=n_b,
    phi_offset=phi_offset,
    phi_guess=phi_guess,
    cylinderDiameter=revoluteDiameter,
    cylinderLength=revoluteLength,
    cylinderColor=revoluteColor,
    specularCoefficient=specularCoefficient)
  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,
    cylinderLength=cylinderLength,
    cylinderDiameter=cylinderDiameter,
    cylinderColor=cylinderColor,
    specularCoefficient=specularCoefficient,
    kinematicConstraint=false,
    constraintResidue=rod1.f_rod - f_rod)
  Modelica.Mechanics.MultiBody.Parts.FixedTranslation rod2(
    animation=animation,
    width=rod2Diameter,
    height=rod2Diameter,
    color=rod2Color,
    specularCoefficient=specularCoefficient,
    r=rRod2_ib)
  Sensors.RelativePosition relativePosition(resolveInFrame=Modelica.Mechanics.MultiBody.Types.ResolveInFrameAB.frame_a)
  Modelica.Blocks.Sources.Constant position_b[3](k=rRod2_ib)
equation
 // Connections.root(frame_ib.R);

  /* Compute the unknown force in the rod of the rod1 joint
     by a torque balance at the revolute joint:
       0 = revolute.frame_b.t + frame_ib.t + frame_im.t + cross(rRod2_ib, frame_im.f)
           + cross(r_ib, -rod1.f_b_a1)
           + cross(r_ib, Frames.resolve2(rod1.R_rel, rod1.f_rod*rod1.eRod1_ia))
     The condition is that the projection of the torque in the revolute
     joint along the axis of the revolute joint is equal to the driving
     axis torque in the flange:
       -revolute.tau = revolute.e*frame_b.t
     Therefore, we have
        tau = e*(frame_ib.t  + frame_im.t + cross(rRod2_ib, frame_im.f)
              + cross(rRod2_ib, -rod1.f_b_a1))
              + e*cross(rRod2_ib, Frames.resolve2(rod1.R_rel, rod1.f_rod*rod1.eRod_a))
            = e*(frame_ib.t + frame_im.t + cross(rRod2_ib, frame_im.f)
              + cross(rRod2_ib, -rod.f_b_a1))
              + rod1.f_rod*e*cross(rRod2_ib, Frames.resolve2(rod1.R_rel, rod1.eRod_a))
     Solving this equation for f_rod results in
       f_rod = (-tau - e*(frame_ib.t + frame_im.t + cross(rRod2_ib, frame_im.f)
               + cross(rRod2_ib, -rod1.f_b_a1)))
               / (cross(e,rRod2_ib)*Frames.resolve2(rod1.R_rel, rod1.eRod_a))
     Additionally, a guard against division by zero is introduced

     f_rod is passed to component JointsUSR.rod1 via variable "constraintResidue" in the Advanced menu
  */
  aux = cross(revolute.e, rRod2_ib)*Frames.resolveRelative(rod1.eRod_a,
    rod1.frame_a.R, rod1.frame_b.R);
  f_rod = (-revolute.tau - revolute.e*(frame_ib.t + frame_im.t + cross(
    rRod2_ib, frame_im.f) - cross(rRod2_ib, 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);

  // Measure power for test purposes
  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.tau*der(axis.phi) +
      bearing.tau*der(bearing.phi);
  else
    totalPower = 0;
  end if;

  connect(revolute.frame_b, rod2.frame_a)
  connect(rod2.frame_b, rod1.frame_b)
  connect(revolute.frame_a, frame_b)
  connect(rod2.frame_a, frame_ib)
  connect(rod1.frame_a, frame_a)
  connect(relativePosition.frame_b, frame_a)
  connect(relativePosition.frame_a, frame_b)
  connect(position_b.y, revolute.position_b)
  connect(rod2.frame_b, frame_im)
  connect(rod1.frame_ia, frame_ia)
  connect(revolute.axis, axis)
  connect(relativePosition.r_rel, revolute.position_a)
  connect(revolute.bearing, bearing)
  Modelica.Mechanics.MultiBody.Visualizers.FixedShape     shape(shapeType       = \"box\",
                                             lengthDirection = jointUSR.eRod1_ia,
                                             widthDirection  = jointUSR.e2_ia,
                                             length          = jointUSR.rod1Length/2,
                                             width           = jointUSR.rod1Length/10);
equation
  connect(jointUSP.frame_ia, shape.frame_a);

"),
    Icon(coordinateSystem(
        preserveAspectRatio=true,
        extent={{-100,-100},{100,100}},
        initialScale=0.2), graphics={
        Rectangle(
          extent={{72,10},{88,-10}},
          fillColor={192,192,192},
          fillPattern=FillPattern.Solid),
        Rectangle(
          extent={{10,6},{61,-6}},
          fillPattern=FillPattern.HorizontalCylinder,
          fillColor={192,192,192}),
        Ellipse(
          extent={{-34,-24},{14,24}},
          startAngle=-110,
          endAngle=110,
          lineColor={0,0,0},
          fillColor={192,192,192},
          fillPattern=FillPattern.Solid,
          closure=EllipseClosure.Radial),
        Ellipse(
          extent={{-28,-18},{8,18}},
          fillColor={255,255,255},
          fillPattern=FillPattern.Solid,
          startAngle=-115,
          endAngle=115,
          lineColor={255,255,255}),
          Ellipse(
          extent={{-28,-18},{8,18}},
          startAngle=-110,
          endAngle=110,
          lineColor={0,0,0},
          closure=EllipseClosure.None),
        Ellipse(
          extent={{-100,-30},{-40,30}},
          startAngle=90,
          endAngle=270,
          lineColor={64,64,64},
          fillColor={192,192,192},
          fillPattern=FillPattern.Solid,
          closure=EllipseClosure.Radial),
        Ellipse(
          extent={{-94,-24},{-46,24}},
          fillColor={255,255,255},
          fillPattern=FillPattern.Solid,
          startAngle=85,
          endAngle=275,
          lineColor={255,255,255}),
          Ellipse(
          extent={{-94,-24},{-46,24}},
          startAngle=90,
          endAngle=270,
          lineColor={64,64,64},
          closure=EllipseClosure.None),
        Rectangle(
          extent={{-50,5},{-10,-5}},
          fillPattern=FillPattern.HorizontalCylinder,
          fillColor={192,192,192}),
        Rectangle(
          extent={{60,-30},{76,30}},
          fillPattern=FillPattern.HorizontalCylinder,
          fillColor={192,192,192}),
        Rectangle(
          extent={{85,-30},{100,30}},
          fillPattern=FillPattern.HorizontalCylinder,
          fillColor={192,192,192}),
        Rectangle(extent={{60,30},{76,-30}}),
        Rectangle(extent={{85,30},{100,-30}}),
        Line(
          points={{68,30},{68,80},{80,80},{80,100}},
          color={95,95,95},
          thickness=0.5),
        Line(
          points={{80,80},{101,80}},
          color={128,128,128},
          thickness=0.5),
        Line(
          points={{90,30},{90,40},{95,40}},
          color={95,95,95},
          thickness=0.5),
          Ellipse(
            extent={{-20,10},{0,-10}},
            fillPattern=FillPattern.Sphere,
            fillColor={192,192,192}),
        Line(
          points={{-70,28},{-70,-30}},
          thickness=0.5),
        Ellipse(
          extent={{-92,-14},{-48,14}},
          fillPattern=FillPattern.Solid,
          fillColor={192,192,192},
          startAngle=-123,
          endAngle=57,
          lineColor={64,64,64}),
        Ellipse(
          extent={{-86,-8},{-54,8}},
          lineColor={255,255,255},
          fillColor={255,255,255},
          fillPattern=FillPattern.Solid,
          startAngle=0,
          endAngle=360),
        Line(
          points={{-50,0},{-50,80},{-80,80},{-80,100}},
          color={95,95,95},
          thickness=0.5),
        Line(
          points={{-70,30},{-70,-8}},
          thickness=0.5),
        Ellipse(
          extent={{-86,-8},{-54,8}},
          lineColor={64,64,64},
          fillColor={255,255,255},
          fillPattern=FillPattern.Solid,
          startAngle=-114,
          endAngle=61,
          closure=EllipseClosure.None),
        Line(
          points={{-56,14},{-84,-14}},
          thickness=0.5),
        Line(
          points={{14,0},{14,80},{0,80},{0,100}},
          color={95,95,95},
          thickness=0.5),
        Text(
          extent={{40,110},{77,90}},
          textColor={128,128,128},
          textString="ib"),
        Text(
          extent={{-124,110},{-95,90}},
          textColor={128,128,128},
          textString="ia"),
        Text(
          extent={{-43,110},{-10,90}},
          textColor={128,128,128},
          textString="im"),
        Text(
          extent={{-75,-50},{75,-70}},
          textColor={0,0,255},
          textString="%name")}));
end JointUSR;


within Modelica.Mechanics.MultiBody.Joints;
package Assemblies "Components that aggregate several joints for analytic loop handling"
  extends Modelica.Icons.Package;
end Assemblies;


within Modelica.Mechanics.MultiBody.Joints;
package Constraints "Components that define joints by constraints"
  extends Modelica.Icons.Package;
end Constraints;


Prismatic cut-joint and translational directions may be constrained or released. This model does not use explicit variables e.g. state variables in order to describe the relative motion of frame_b with respect to frame_a, but defines kinematic constraints between the frame_a and frame_b. The forces and torques at both frames are then evaluated in such a way that the constraints are satisfied.  Sometimes this type of formulation is also called an implicit joint in literature.

within Modelica.Mechanics.MultiBody.Joints.Constraints;
model Prismatic
  "Prismatic cut-joint and translational directions may be constrained or released"
  extends Modelica.Mechanics.MultiBody.Interfaces.PartialTwoFrames;

  parameter Boolean x_locked=true
    "= true: constraint force in x-direction, resolved in frame_a"
  parameter Boolean y_locked=true
    "= true: constraint force in y-direction, resolved in frame_a"
  parameter Boolean z_locked=true
    "= true: constraint force in z-direction, resolved in frame_a"

  parameter Boolean animation=true
    "= true, if animation shall be enabled (show sphere)";
  parameter SI.Distance sphereDiameter=world.defaultJointLength /3
    "Diameter of sphere representing the spherical joint"
  input Types.Color sphereColor=Types.Defaults.JointColor
    "Color of sphere representing the spherical joint"
  input Types.SpecularCoefficient specularCoefficient=world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"

protected
  Frames.Orientation R_rel
    "Dummy or relative orientation object from frame_a to frame_b";
  SI.Position r_rel_a[3]
    "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a";
  SI.InstantaneousPower P;

public
  Visualizers.Advanced.Shape sphere(
    shapeType="sphere",
    color=sphereColor,
    specularCoefficient=specularCoefficient,
    length=sphereDiameter,
    width=sphereDiameter,
    height=sphereDiameter,
    lengthDirection={1,0,0},
    widthDirection={0,1,0},
    r_shape={-0.5,0,0}*sphereDiameter,
    r=frame_a.r_0,
    R=frame_a.R) if world.enableAnimation and animation;
equation
  // Determine relative position vector resolved in frame_a
  R_rel = Frames.relativeRotation(frame_a.R, frame_b.R);
  r_rel_a = Frames.resolve2(frame_a.R, frame_b.r_0 - frame_a.r_0);

  // Constraint equations concerning rotations
  // Same logic as for overdetermined connection graph loops to get good residuals.
  zeros(3)=Modelica.Mechanics.MultiBody.Frames.Orientation.equalityConstraint(frame_a.R, frame_b.R);

  // Constraint equations concerning translations
  if x_locked then
    r_rel_a[1]=0;
  else
    frame_a.f[1]=0;
  end if;

  if y_locked then
    r_rel_a[2]=0;
  else
    frame_a.f[2]=0;
  end if;

  if z_locked then
    r_rel_a[3]=0;
  else
    frame_a.f[3]=0;
  end if;

  zeros(3) = frame_a.t + Frames.resolve1(R_rel, frame_b.t) + cross(r_rel_a,
    Frames.resolve1(R_rel, frame_b.f));
  zeros(3) = Frames.resolve1(R_rel, frame_b.f) + frame_a.f;
  P = frame_a.t*Frames.angularVelocity2(frame_a.R) + frame_b.t*
    Frames.angularVelocity2(frame_b.R) + frame_b.f*Frames.resolve2(frame_b.R,
    der(frame_b.r_0)) + frame_a.f*Frames.resolve2(frame_a.R, der(frame_a.r_0));
end Prismatic;


Revolute cut-joint and translational directions may be constrained or released. This model does not use explicit variables e.g. state variables in order to describe the relative motion of frame_b with respect to frame_a, but defines kinematic constraints between the frame_a and frame_b. The forces and torques at both frames are then evaluated in such a way that the constraints are satisfied. Sometimes this type of formulation is also called an implicit joint in literature.

within Modelica.Mechanics.MultiBody.Joints.Constraints;
model Revolute
  "Revolute cut-joint and translational directions may be constrained or released"
  extends Modelica.Mechanics.MultiBody.Interfaces.PartialTwoFrames;

  parameter Boolean x_locked=true
    "= true: constraint force in x-direction, resolved in frame_a"
  parameter Boolean y_locked=true
    "= true: constraint force in y-direction, resolved in frame_a"
  parameter Boolean z_locked=true
    "= true: constraint force in z-direction, resolved in frame_a"

  parameter Boolean animation=true
    "= true, if animation shall be enabled (show sphere)";
  parameter Types.Axis n={0,1,0}
    "Axis of rotation resolved in frame_a (= same as in frame_b)"

  parameter SI.Distance sphereDiameter=world.defaultJointLength /3
    "Diameter of sphere representing the spherical joint"
  input Types.Color sphereColor=Types.Defaults.JointColor
    "Color of sphere representing the spherical joint"
  input Types.SpecularCoefficient specularCoefficient=world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"

protected
  Frames.Orientation R_rel
    "Dummy or relative orientation object from frame_a to frame_b";
  SI.Position r_rel_a[3]
    "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a";
  SI.InstantaneousPower P;
  parameter Real e[3](each final unit="1")=Modelica.Math.Vectors.normalizeWithAssert(
                                       n)
    "Unit vector in direction of rotation axis, resolved in frame_a (= same as in frame_b)";

  parameter Real nnx_a[3](each final unit="1")=if abs(e[1]) > 0.1 then {0,1,0} else (if abs(e[2])
       > 0.1 then {0,0,1} else {1,0,0})
    "Arbitrary vector that is not aligned with rotation axis n"
      parameter Real ey_a[3](each final unit="1")=Modelica.Math.Vectors.normalizeWithAssert(
                                          cross(e, nnx_a))
    "Unit vector orthogonal to axis n of revolute joint, resolved in frame_a"
  parameter Real ex_a[3](each final unit="1")=cross(ey_a, e)
    "Unit vector orthogonal to axis n of revolute joint and to ey_a, resolved in frame_a";

public
  Visualizers.Advanced.Shape sphere(
    shapeType="sphere",
    color=sphereColor,
    specularCoefficient=specularCoefficient,
    length=sphereDiameter,
    width=sphereDiameter,
    height=sphereDiameter,
    lengthDirection={1,0,0},
    widthDirection={0,1,0},
    r_shape={-0.5,0,0}*sphereDiameter,
    r=frame_a.r_0,
    R=frame_a.R) if world.enableAnimation and animation;

equation
  // Determine relative position vector resolved in frame_a
  R_rel = Frames.relativeRotation(frame_a.R, frame_b.R);
  r_rel_a = Frames.resolve2(frame_a.R, frame_b.r_0 - frame_a.r_0);

  // Constraint equations concerning translations
  if x_locked then
    r_rel_a[1]=0;
  else
    frame_a.f[1]=0;
  end if;

  if y_locked then
    r_rel_a[2]=0;
  else
    frame_a.f[2]=0;
  end if;

  if z_locked then
    r_rel_a[3]=0;
  else
    frame_a.f[3]=0;
  end if;

  // Constraint equations concerning rotations
  0 = ex_a*R_rel.T*e;
  0 = ey_a*R_rel.T*e;
  frame_a.t*n=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) - cross(r_rel_a,
    frame_a.f);
  P = frame_a.t*Frames.angularVelocity2(frame_a.R) + frame_b.t*
    Frames.angularVelocity2(frame_b.R) + Frames.resolve1(frame_b.R, frame_b.f)
    *der(frame_b.r_0) + Frames.resolve1(frame_a.R, frame_a.f)*der(frame_a.r_0);
end Revolute;


Spherical cut joint and translational directions may be constrained or released. This model does not use explicit variables e.g. state variables in order to describe the relative motion of frame_b with to respect to frame_a, but defines kinematic constraints between the frame_a and frame_b. The forces and torques at both frames are then evaluated in such a way that the constraints are satisfied. Sometimes this type of formulation is also called an implicit joint in literature.

within Modelica.Mechanics.MultiBody.Joints.Constraints;
model Spherical
  "Spherical cut joint and translational directions may be constrained or released"
  extends Modelica.Mechanics.MultiBody.Interfaces.PartialTwoFrames;
  import MBS = Modelica.Mechanics.MultiBody;

  parameter Boolean x_locked=true
    "= true: constraint force in x-direction, resolved in frame_a"
  parameter Boolean y_locked=true
    "= true: constraint force in y-direction, resolved in frame_a"
  parameter Boolean z_locked=true
    "= true: constraint force in z-direction, resolved in frame_a"

  parameter Boolean animation=true
    "= true, if animation shall be enabled (show sphere)"
  parameter SI.Distance sphereDiameter=world.defaultJointLength /3
    "Diameter of sphere representing the spherical joint"
  input MBS.Types.Color sphereColor=MBS.Types.Defaults.JointColor
    "Color of sphere representing the spherical joint"
  input MBS.Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"

  Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape sphere(
    shapeType="sphere",
    color=sphereColor,
    specularCoefficient=specularCoefficient,
    length=sphereDiameter,
    width=sphereDiameter,
    height=sphereDiameter,
    lengthDirection={1,0,0},
    widthDirection={0,1,0},
    r_shape={-0.5,0,0}*sphereDiameter,
    r=frame_a.r_0,
    R=frame_a.R) if world.enableAnimation and animation;
protected
  MBS.Frames.Orientation R_rel
    "Dummy or relative orientation object from frame_a to frame_b";
  SI.Position r_rel_a[3]
    "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a";
  SI.InstantaneousPower P;

equation
  // Determine relative position vector resolved in frame_a
  R_rel = MBS.Frames.relativeRotation(frame_a.R, frame_b.R);
  r_rel_a = MBS.Frames.resolve2(frame_a.R, frame_b.r_0 - frame_a.r_0);

  // Constraint equations concerning translation
  if x_locked then
    r_rel_a[1]=0;
  else
    frame_a.f[1]=0;
  end if;

  if y_locked then
    r_rel_a[2]=0;
  else
    frame_a.f[2]=0;
  end if;

  if z_locked then
    r_rel_a[3]=0;
  else
    frame_a.f[3]=0;
  end if;

  //frame_a.t = zeros(3);
  frame_b.t = zeros(3);
  frame_b.f = -MBS.Frames.resolve2(R_rel, frame_a.f);
  zeros(3) = frame_a.t + MBS.Frames.resolve1(R_rel, frame_b.t) - cross(r_rel_a, frame_a.f);
  P= frame_a.t*MBS.Frames.angularVelocity2(frame_a.R)+frame_b.t*MBS.Frames.angularVelocity2(frame_b.R) + MBS.Frames.resolve1(frame_b.R,frame_b.f)*der(frame_b.r_0)+MBS.Frames.resolve1(frame_a.R,frame_a.f)*der(frame_a.r_0);
end Spherical;


Universal cut-joint and translational directions may be constrained or released. This model does not use explicit variables e.g. state variables in order to describe the relative motion of frame_b with respect to frame_a, but defines kinematic constraints between the frame_a and frame_b. The forces and torques at both frames are then evaluated in such a way that the constraints are satisfied. Sometimes this type of formulation is also called an implicit joint in literature.

within Modelica.Mechanics.MultiBody.Joints.Constraints;
model Universal
  "Universal cut-joint and translational directions may be constrained or released"
  extends Modelica.Mechanics.MultiBody.Interfaces.PartialTwoFrames;
  import MBS = Modelica.Mechanics.MultiBody;

  parameter MBS.Types.Axis n_a={1,0,0}
    "Axis of revolute joint 1 resolved in frame_a"
  parameter MBS.Types.Axis n_b={0,1,0}
    "Axis of revolute joint 2 resolved in frame_b"

  parameter Boolean x_locked=true
    "= true: constraint force in x-direction, resolved in frame_a"
  parameter Boolean y_locked=true
    "= true: constraint force in y-direction, resolved in frame_a"
  parameter Boolean z_locked=true
    "= true: constraint force in z-direction, resolved in frame_a"

  parameter Boolean animation=true
    "= true, if animation shall be enabled (show sphere)"
  parameter SI.Distance sphereDiameter=world.defaultJointLength /3
    "Diameter of sphere representing the spherical joint"
  input MBS.Types.Color sphereColor=MBS.Types.Defaults.JointColor
    "Color of sphere representing the spherical joint"
  input MBS.Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"
protected
  MBS.Frames.Orientation R_rel
    "Dummy or relative orientation object from frame_a to frame_b";
  Real w_rel[3];
  SI.Position r_rel_a[3]
    "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a";

  SI.InstantaneousPower P;
equation
  // Determine relative position vector resolved in frame_a
  R_rel = MBS.Frames.relativeRotation(frame_a.R, frame_b.R);
  w_rel = MBS.Frames.angularVelocity1(R_rel);
  r_rel_a = MBS.Frames.resolve2(frame_a.R, frame_b.r_0 - frame_a.r_0);

  // Constraint equations concerning translations
  if x_locked then
    r_rel_a[1]=0;
  else
    frame_a.f[1]=0;
  end if;

  if y_locked then
    r_rel_a[2]=0;
  else
    frame_a.f[2]=0;
  end if;

  if z_locked then
    r_rel_a[3]=0;
  else
    frame_a.f[3]=0;
  end if;

  // Constraint equations concerning rotations
  frame_a.t*n_a=0;
  frame_b.t*n_b=0;
  n_b*R_rel.T*n_a=0;
  assert(abs(n_a*n_b) < Modelica.Constants.eps, "The two axes that constitute the Constraints.Universal joint must be different");

  zeros(3)=frame_a.f + MBS.Frames.resolve1(R_rel, frame_b.f);
  zeros(3) = frame_a.t+MBS.Frames.resolve1(R_rel, frame_b.t)- cross(r_rel_a, frame_a.f);
  P = frame_a.t*MBS.Frames.angularVelocity2(frame_a.R)+frame_b.t*MBS.Frames.angularVelocity2(frame_b.R) + MBS.Frames.resolve1(frame_b.R,frame_b.f)*der(frame_b.r_0)+MBS.Frames.resolve1(frame_a.R,frame_a.f)*der(frame_a.r_0);
end Universal;


Internal model to initialize the angels for Joints.FreeMotionScalarInit. Compute three rotational angles angle for a given rotational sequence
sequence_start from a relative orientation from frame_a to frame_b.

within Modelica.Mechanics.MultiBody.Joints.Internal;
model InitAngle
 "Internal model to initialize the angels for Joints.FreeMotionScalarInit"
  extends Modelica.Blocks.Icons.Block;

  import Modelica.Mechanics.MultiBody.Frames;

  parameter Modelica.Mechanics.MultiBody.Types.RotationSequence sequence_start={1,2,3}
   "Sequence of angle rotations";

  Interfaces.Frame_a frame_a
  Interfaces.Frame_b frame_b

  Frames.Orientation R_rel
   "Relative orientation object to rotate from frame_a to frame_b";
  Frames.Orientation R_rel_inv
   "Relative orientation object to rotate from frame_b to frame_a";

  Blocks.Interfaces.RealOutput angle[3](each final quantity="Angle", each final unit="rad")
equation
  Connections.branch(frame_a.R, frame_b.R);
  R_rel = Frames.axesRotations(sequence_start,
                               {angle[1], angle[2], angle[3]},
                               {der(angle[1]), der(angle[2]), der(angle[3])});
  if Connections.rooted(frame_a.R) then
     R_rel_inv = Frames.nullRotation();
     frame_b.R = Frames.absoluteRotation(frame_a.R, R_rel);
  else
     R_rel_inv = Frames.inverseRotation(R_rel);
     frame_a.R = Frames.absoluteRotation(frame_b.R, R_rel_inv);
  end if;

  frame_a.f = zeros(3);
  frame_a.t = zeros(3);
  frame_b.f = zeros(3);
  frame_b.t = zeros(3);
end InitAngle;


Internal model to initialize w_rel_b for Joints.FreeMotionScalarInit. Compute relative angular velocity w_rel_b of an
orientation object R_b
relative to an
orientation object R_a,
resolved in orientation object R_b, i.e.

within Modelica.Mechanics.MultiBody.Joints.Internal;
model InitAngularVelocity
 "Internal model to initialize w_rel_b for Joints.FreeMotionScalarInit"
  extends Modelica.Blocks.Icons.Block;

  import Modelica.Mechanics.MultiBody.Frames;

  input Frames.Orientation R_a
  input Frames.Orientation R_b

  Modelica.Blocks.Interfaces.RealOutput w_rel_b[3](each final quantity="AngularVelocity", each final unit="rad/s")
equation
 Frames.angularVelocity2(R_b) = Frames.resolve2(R_b,Frames.angularVelocity1(R_a)) + w_rel_b;
end InitAngularVelocity;


Internal model to initialize r_rel_a for Joints.FreeMotionScalarInit. Compute relative position vector r_rel_a from a position vector r_a_0 to
a position vector r_b_0, resolved in frame given by an
orientation object R_a.

within Modelica.Mechanics.MultiBody.Joints.Internal;
model InitPosition
 "Internal model to initialize r_rel_a for Joints.FreeMotionScalarInit"
  extends Modelica.Blocks.Icons.Block;

  import Modelica.Mechanics.MultiBody.Frames;

  input SI.Position r_a_0[3]
  input SI.Position r_b_0[3]
  input Frames.Orientation R_a

  Modelica.Blocks.Interfaces.RealOutput r_rel_a[3](each final quantity="Length", each final unit="m")

equation
  r_b_0 = r_a_0 + Frames.resolve1(R_a, {r_rel_a[1], r_rel_a[2], r_rel_a[3]});
end InitPosition;


within Modelica.Mechanics.MultiBody.Joints;
package Internal "Components used for analytic solution of kinematic loops (use only if you know what you are doing)"

  extends Modelica.Icons.InternalPackage;
end Internal;


Prismatic joint where the translational distance is computed from a length constraint (1 degree-of-freedom, no potential state). Joint where frame_b is translated along axis n which is fixed in frame_a.
The two frames coincide when \"s + s_offset = 0\", where
\"s_offset\" is a parameter with a zero default
and \"s\" is the relative distance.

within Modelica.Mechanics.MultiBody.Joints.Internal;
model PrismaticWithLengthConstraint
  "Prismatic joint where the translational distance is computed from a length constraint (1 degree-of-freedom, no potential state)"

  extends Modelica.Mechanics.MultiBody.Interfaces.PartialTwoFrames;
  Modelica.Mechanics.Translational.Interfaces.Flange_a axis
    "1-dim. translational flange that drives the joint"
  Modelica.Mechanics.Translational.Interfaces.Flange_b bearing
    "1-dim. translational flange of the drive bearing"
  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 prismatic joint"
  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 prismatic joint"

  parameter Boolean animation=true "= true, if animation shall be enabled";
  parameter SI.Position length(start=1) "Fixed length of length constraint";
  parameter Modelica.Mechanics.MultiBody.Types.Axis n={1,0,0}
    "Axis of translation resolved in frame_a (= same as in frame_b)"
  parameter SI.Position s_offset=0
    "Relative distance offset (distance between frame_a and frame_b = 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 Types.Axis boxWidthDirection={0,1,0}
    "Vector in width direction of box, resolved in frame_a"
  parameter SI.Distance boxWidth=world.defaultJointWidth
    "Width of prismatic joint box"
  parameter SI.Distance boxHeight=boxWidth "Height of prismatic joint box"
  input Types.Color boxColor=Modelica.Mechanics.MultiBody.Types.Defaults.JointColor
    "Color of prismatic joint box"
  input Types.SpecularCoefficient specularCoefficient = world.defaultSpecularCoefficient
    "Reflection of ambient light (= 0: light is completely absorbed)"

  final parameter Boolean positiveBranch(fixed=false)
    "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 translation axis, resolved in frame_a";
  SI.Position s
    "Relative distance between frame_a and frame_b along axis n = s + s_offset)";
  SI.Position distance
    "Relative distance between frame_a and frame_b along axis n";
  SI.Position r_rel_a[3]
    "Position vector from frame_a to frame_b resolved in frame_a";
  SI.Force f "= axis.f (driving force 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 prismatic 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 prismatic joint";
  SI.Position rbra[3] "= rb - ra";
  Real B "Coefficient B of equation: s*s + B*s + C = 0";
  Real C "Coefficient C of equation: s*s + B*s + C = 0";
  Real k1 "Constant of quadratic equation solution";
  Real k2 "Constant of quadratic equation solution";
  Real k1a(start=1);
  Real k1b;

  Visualizers.Advanced.Shape box(
    shapeType="box",
    color=boxColor,
    specularCoefficient=specularCoefficient,
    length=if noEvent(abs(s + s_offset) > 1.e-6) then s + s_offset else 1.e-6,
    width=boxWidth,
    height=boxHeight,
    lengthDirection=e,
    widthDirection=boxWidthDirection,
    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 translation, resolved in frame_a (= same in frame_b)";
    input SI.Position d_guess
      "Select the configuration such that at initial time |d-d_guess| is minimal (d: distance between origin of frame_a and origin of frame_b)";
    input SI.Position r_a[3]
      "Position vector from frame_a to frame_a side of length constraint, resolved in frame_a of prismatic joint";
    input SI.Position r_b[3]
      "Position vector from frame_b to frame_b side of length constraint, resolved in frame_b of prismatic joint";
    output Boolean positiveBranch "Branch of the initial solution";
  protected
    SI.Position rbra[3] "= rb - ra";
    Real B "Coefficient B of equation: d*d + B*d + C = 0";
    Real C "Coefficient C of equation: d*d + B*d + C = 0";
    Real k1 "Constant of quadratic equation solution";
    Real k2 "Constant of quadratic equation solution";
    Real k1a;
    Real k1b;
    Real d1 "Solution 1 of quadratic equation";
    Real d2 "Solution 2 of quadratic equation";
  algorithm
    /* The position vector r_rel from frame_a to frame_b of the length constraint
       element, resolved in frame_b of the prismatic joint (frame_a and frame_b
       of the prismatic joint are parallel to each other) is given by:
          r_rel = d*e + r_b - r_a
       The length constraint can therefore be formulated as:
          r_rel*r_rel = L*L
       with
          (d*e + r_b - r_a)*(d*e + r_b - r_a)
                   = d*d + 2*d*e*(r_b - r_a) + (r_b - r_a)*(r_b - r_a)
       follows
          (1)  0 = d*d + d*2*e*(r_b - r_a) + (r_b - r_a)*(r_b - r_a) - L*L
       The vectors r_a, r_b and parameter L are NOT a function of
       the distance d of the prismatic joint. Therefore, (1) is a quadratic
       equation in the single unknown "d":
          (2) d*d + B*d + C = 0
              with   B = 2*e*(r_b - r_a)
                     C = (r_b - r_a)*(r_b - r_a) - L*L
       The solution is
          (3) d = - B/2 +/- sqrt(B*B/4 - C)
    */
    rbra := r_b - r_a;
    B := 2*(e*rbra);
    C := rbra*rbra - L*L;
    k1 := B/2;
    k1a :=k1*k1 - C;
  assert(noEvent(k1a > 1e-10), "
Singular position of loop (either no or two analytic solutions;
the mechanism has lost one-degree-of freedom in this position).
Try first to use another Modelica.Mechanics.MultiBody.Joints.Assemblies.JointXXX component.
If this also lead to singular positions, it could be that this
kinematic loop cannot be solved analytically with a fixed state
selection. In this case you have to build up the loop with
basic joints (NO aggregation JointXXX components) and rely on
dynamic state selection, i.e., during simulation the states will
be dynamically selected in such a way that in no position a
degree of freedom is lost.
"); k1b :=max(k1a, 1.0e-12);
    k2 :=sqrt(k1b);
    d1 := -k1 + k2;
    d2 := -k1 - k2;
    if abs(d1 - d_guess) <= abs(d2 - d_guess) then
      positiveBranch := true;
    else
      positiveBranch := false;
    end if;
  end selectBranch;
initial equation
  positiveBranch = selectBranch(length, e, s_offset + s_guess, r_a, r_b);
equation
  Connections.branch(frame_a.R, frame_b.R);

  axis.f = f;
  axis.s = s;
  bearing.s = 0;
  distance = s_offset + s;

  // relationships of frame_a and frame_b quantities
  r_rel_a = e*distance;
  frame_b.r_0 = frame_a.r_0 + Frames.resolve1(frame_a.R, r_rel_a);
  frame_b.R = frame_a.R;
  zeros(3) = frame_a.f + frame_b.f;
  zeros(3) = frame_a.t + frame_b.t + cross(r_rel_a, frame_b.f);

  // Compute translational distance (details, see function "selectBranch")
  rbra = r_b - r_a;
  B = 2*(e*rbra);
  C = rbra*rbra - length*length;
  k1 = B/2;
  k1a = k1*k1 - C;
  assert(noEvent(k1a > 1e-10), "
Singular position of loop (either no or two analytic solutions;
the mechanism has lost one-degree-of freedom in this position).
Try first to use another Modelica.Mechanics.MultiBody.Joints.Assemblies.JointXXX component.
If this also lead to singular positions, it could be that this
kinematic loop cannot be solved analytically with a fixed state
selection. In this case you have to build up the loop with
basic joints (NO aggregation JointXXX components) and rely on
dynamic state selection, i.e., during simulation the states will
be dynamically selected in such a way that in no position a
degree of freedom is lost.
");
  k1b = Frames.Internal.maxWithoutEvent(k1a, 1.0e-12);
  k2 = sqrt(k1b);
  distance = -k1 + (if positiveBranch then k2 else -k2);
end PrismaticWithLengthConstraint;