VehicleDynamics.Utilities.Joints

Special joints used in the chassis modelling.

VehicleDynamics.Utilities.Joints.JointUPS VehicleDynamics.Utilities.Joints.JointRSU VehicleDynamics.Utilities.Joints.JointUS VehicleDynamics.Utilities.Joints.DependentRevolute VehicleDynamics.Utilities.Joints.RelDistance2 VehicleDynamics.Utilities.Joints.FreeMotion VehicleDynamics.Utilities.Joints.FreeMotion4 VehicleDynamics.Utilities.Joints.FreeMotion3 VehicleDynamics.Utilities.Joints.Spherical VehicleDynamics.Utilities.Joints.FreeMotion2 VehicleDynamics.Utilities.Joints.RelDistance

NameDescription
JointUPS Universal - prismatic - spherical joint aggregation (no constraints and no states)
JointRSU Revolute - spherical - universal - joint aggregation (no constraints, no states)
JointUS Universal - Spherical joint aggregation (one constraint)
DependentRevolute Revolute joint where the rotation angle is computed from a length constraint
RelDistance2 Distance between flange_a and flange_b is converted to translational cuts.
FreeMotion Free motion joint (6 degrees-of-freedom, used in spanning tree)
FreeMotion4 Free motion joint (6 degrees-of-freedom, used in spanning tree)
FreeMotion3 Free motion joint (6 degrees-of-freedom, used in spanning tree)
Spherical Spherical joint described by three Cardan angles (3 degrees-of-freedom, used in spanning tree)
FreeMotion2 Free motion joint (6 degrees-of-freedom, used in spanning tree)
RelDistance Distance between flange_a and flange_b is converted to translational cuts.


VehicleDynamics.Utilities.Joints.FreeMotion VehicleDynamics.Utilities.Joints.FreeMotion

Free motion joint (6 degrees-of-freedom, used in spanning tree)

VehicleDynamics.Utilities.Joints.FreeMotion

Information

 

Joint which does not constrain the motion between frame_a and frame_b. Such a joint is just used to define the desired states to be used. The joint is realized in such a way, that a singularity cannot occur. This is achieved because the Cardan angles are defined between a frame_fix fixed in frame_a and frame_b. Whenever the Cardan angles are near a singularity, the integration is stopped and frame_fix is changed, such that the Cardan angles are far away from the singularity. The following state variables are used:

  r_rela[3]: Distance vector from the origin of frame_a to the origin
             of frame_b, resolved in frame_a in [m].
  phi[3]   : Cardan angles, also called Tait-Bryan angles, i.e.,
             rotate around 1-, 2-, 3-axis in [rad] from intermediate
             frame_fix, which is fixed in frame_a, to frame_b. Initially, frame_fix
             is identical to frame_a. If phi[2] is near its singularity (= pi/2 or -pi/2),
             the frame_fix and phi are changed, such that phi[2] is far away from 
             its singularity.
  v_rela[3]: = der(r_rela); relative velocity of frame_b with respect to frame_a
             resolved in frame_a in [m/s].
  w_rela[3]: Relative angular velocity of frame_b with respect to frame_a
             resolved in frame_a in [rad/s].

Parameters

NameDefaultDescription
enforceStatestrueif true, then all StateSelect.always is set for all 6 DOF

Modelica definition

model FreeMotion 
  "Free motion joint (6 degrees-of-freedom, used in spanning tree)" 
  
  parameter Boolean enforceStates=true 
    "|Advanced||if true, then all StateSelect.always is set for all 6 DOF";
  
  import SI = Modelica.SIunits;
  extends ModelicaAdditions.MultiBody.Interfaces.TreeJoint(r_rela(stateSelect=if 
          enforceStates then StateSelect.always else StateSelect.prefer), 
      v_rela(stateSelect=if enforceStates then StateSelect.always else 
          StateSelect.prefer));
  SI.Angle phi[3](stateSelect=if enforceStates then StateSelect.always else 
        StateSelect.prefer) 
    "Cardan angles from a frame fixed in frame_a to frame_b";
  SI.AngularVelocity phi_d[3](stateSelect=if enforceStates then StateSelect.
        always else StateSelect.prefer);
  SI.Velocity vaux[3];
  Real s1;
  Real s2;
  Real s3;
  Real c1;
  Real c2;
  Real c3;
  SI.AngularAcceleration phi_dd[3];
  SI.AngularVelocity phi_d12;
  SI.AngularVelocity phi_d13;
  SI.AngularVelocity phi_d23;
  
equation 
  
  /* Determine sines and cosines of the Cardan angles */
  s1 = Modelica.Math.sin(phi[1]);
  s2 = Modelica.Math.sin(phi[2]);
  s3 = Modelica.Math.sin(phi[3]);
  c1 = Modelica.Math.cos(phi[1]);
  c2 = Modelica.Math.cos(phi[2]);
  c3 = Modelica.Math.cos(phi[3]);
  
  /* Relative transformation matrix
       S_phi = [ c3, s3, 0; 
                -s3, c3, 0; 
                  0, 0, 1]*[c2, 0, -s2; 
                             0, 1, 0; 
                            s2, 0, c2]*[1, 0, 0; 
                                        0, c1, s1; 
                                        0, -s1, c1];
  */
  S_rel = [c2*c3, c1*s3 + s1*s2*c3, s1*s3 - c1*s2*c3; -c2*s3, c1*c3 - s1*s2*s3, 
     s1*c3 + c1*s2*s3; s2, -s1*c2, c1*c2];
  
  // Kinematic differential equations for translational motion
  der(r_rela) = v_rela;
  der(v_rela) = a_rela;
  
  // Kinematic differential equations for rotational motion      
  der(phi) = phi_d;
  der(phi_d) = phi_dd;
  
  /*
  phi_d = {w_rela[1] + (s1*w_rela[2] - c1*w_rela[3])*s2/c2,c1*w_rela[2] + s1*
    w_rela[3],(-s1*w_rela[2] + c1*w_rela[3])/c2};
*/
  w_rela = {phi_d[1] + s2*phi_d[3],c1*phi_d[2] - s1*c2*phi_d[3],s1*phi_d[2] + 
    c1*c2*phi_d[3]};
  
  phi_d12 = phi_d[1]*phi_d[2];
  phi_d13 = phi_d[1]*phi_d[3];
  phi_d23 = phi_d[2]*phi_d[3];
  z_rela = {phi_dd[1] + s2*phi_dd[3] + c2*phi_d23,c1*phi_dd[2] - s1*c2*phi_dd[3]
     - s1*phi_d12 - c1*c2*phi_d13 + s1*s2*phi_d23,s1*phi_dd[2] + c1*c2*phi_dd[3]
     + c1*phi_d12 - s1*c2*phi_d13 - c1*s2*phi_d23};
  
  // Kinematic relationships
  frame_b.S = frame_a.S*transpose(S_rel);
  frame_b.r0 = frame_a.r0 + frame_a.S*r_rela;
  
  vaux = cross(frame_a.w, r_rela);
  frame_b.v = S_rel*(frame_a.v + v_rela + vaux);
  frame_b.w = S_rel*(frame_a.w + w_rela);
  
  frame_b.a = S_rel*(frame_a.a + cross(frame_a.z, r_rela) + cross(frame_a.w, 
    vaux + 2*v_rela) + a_rela);
  frame_b.z = S_rel*(frame_a.z + cross(frame_a.w, w_rela) + z_rela);
  
  // cut-forces and cut-torques are zero
  frame_a.f = zeros(3);
  frame_a.t = zeros(3);
  frame_b.f = zeros(3);
  frame_b.t = zeros(3);
end FreeMotion;

VehicleDynamics.Utilities.Joints.Spherical VehicleDynamics.Utilities.Joints.Spherical

Spherical joint described by three Cardan angles (3 degrees-of-freedom, used in spanning tree)

VehicleDynamics.Utilities.Joints.Spherical

Information


Joint where the origins of frame_a and frame_b always coincide, and the frames are rotating against each other. The joint is realized in such a way, that a singularity cannot occur. This is achieved because the Cardan angles are defined between a frame_fix fixed in frame_a and frame_b. Whenever the Cardan angles are near a singularity, the integration is stopped and frame_fix is changed, such that the Cardan angles are far away from the singularity. The following state variables are used:

  phi[3]   : Cardan angles, also called Tait-Bryan angles, i.e.,
             rotate around 1-, 2-, 3-axis in [rad] from intermediate
             frame_fix, which is fixed in frame_a, to frame_b. Initially, frame_fix
             is identical to frame_a. If phi[2] is near its singularity (= pi/2 or -pi/2),
             the frame_fix and phi are changed, such that phi[2] is far away from
             its singularity.
  w_rela[3]: Relative angular velocity of frame_b with respect to frame_a
             resolved in frame_a in [rad/s].

Modelica definition

model Spherical 
  "Spherical joint described by three Cardan angles (3 degrees-of-freedom, used in spanning tree)"
   
  
  import Modelica.Math.*;
  
  // S_rel needs a correct start value, because pre(S_rel) is referenced below
  extends ModelicaAdditions.MultiBody.Interfaces.TreeJoint;
  //(S_rel(start=identity(3)));
  SI.Angle phi[3](stateSelect=StateSelect.never) 
    "Cardan angles from a frame fixed in frame_a to frame_b";
  
protected 
  constant SI.Angle phi2_critical_deg=80 
    "angle in [deg] too close to singularity. Redefine S_fix and phi";
  constant SI.Angle phi2_critical=phi2_critical_deg*Modelica.Constants.pi/180.0;
  constant Real c2_small=1.e-5 
    "if cos(phi[2]) < c2_small, c2_small is used as guard against zero division";
  Real s1;
  Real s2;
  Real s3;
  Real c1;
  Real c2;
  Real c2a;
  Real c3;
  //Boolean switch_state;
  Real S_phi[3, 3] "S_rel = S_phi(phi)*S_fix";
  Real S_fix[3, 3];
  //(start=identity(3)) "S_rel = S_phi(phi)*S_fix";
  SI.AngularVelocity w_fix[3] 
    "Relative angular velocity resolved in intermediate frame S_fix";
equation 
  
  /* Determine sines and cosines of the Cardan angles */
  s1 = sin(phi[1]);
  s2 = sin(phi[2]);
  s3 = sin(phi[3]);
  c1 = cos(phi[1]);
  c2a = cos(phi[2]);
  c3 = cos(phi[3]);
  
  /* Below, some expressions are divided by c2. By construction, it is not possible
     that c2=0, during continuous simulation. However, at initial time and when
     large numerical errors occur, c2=0 is possible, which would result in a division
     by zero. The following statement is a guard against this unlikely situation.
  */
  c2 = c2a;
  
  //if noEvent(c2a > c2_small or c2a < -c2_small) then c2a else if noEvent(
  //    c2a >= 0) then c2_small else -c2_small;
  
  /* Relative transformation matrix
       S_phi = [ c3, s3, 0;
                -s3, c3, 0;
                  0, 0, 1]*[c2, 0, -s2;
                             0, 1, 0;
                            s2, 0, c2]*[1, 0, 0;
                                        0, c1, s1;
                                        0, -s1, c1];
  */
  //switch_state = phi[2] >= phi2_critical or phi[2] <= -phi2_critical;
  /*when switch_state then
    S_fix = pre(S_rel);
    reinit(phi, zeros(3));
  end when;*/
  S_fix = identity(3);
  
  S_phi = [c2*c3, c1*s3 + s1*s2*c3, s1*s3 - c1*s2*c3; -c2*s3, c1*c3 - s1*s2*s3, 
     s1*c3 + c1*s2*s3; s2, -s1*c2, c1*c2];
  S_rel = S_phi*S_fix;
  
  // No translational movement
  r_rela = zeros(3);
  v_rela = zeros(3);
  a_rela = zeros(3);
  
  // Kinematic differential equations for rotational motion
  w_fix = S_fix*w_rela;
  der(phi) = {w_fix[1] + (s1*w_fix[2] - c1*w_fix[3])*s2/c2,c1*w_fix[2] + s1*
    w_fix[3],(-s1*w_fix[2] + c1*w_fix[3])/c2};
  der(w_rela) = z_rela;
  
  // Kinematic relationships
  frame_b.S = frame_a.S*transpose(S_rel);
  frame_b.r0 = frame_a.r0;
  
  frame_b.v = S_rel*frame_a.v;
  frame_b.w = S_rel*(frame_a.w + w_rela);
  
  frame_b.a = S_rel*frame_a.a;
  frame_b.z = S_rel*(frame_a.z + cross(frame_a.w, w_rela) + z_rela);
  
  // cut-torques are zero
  frame_a.f = -transpose(S_rel)*frame_b.f;
  frame_a.t = zeros(3);
  frame_b.t = zeros(3);
end Spherical;

VehicleDynamics.Utilities.Joints.JointUPS VehicleDynamics.Utilities.Joints.JointUPS

Universal - prismatic - spherical joint aggregation (no constraints and no states)

VehicleDynamics.Utilities.Joints.JointUPS

Information


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. This joint introduces neither constraints nor state variables. The universal joint is defined in the following way:

Note, a singularity takes place when n_a = e_x, i.e., when n_a is along the line connecting the universal and the spherical joint. Therefore, if possible n_a should be selected in such a way that it is perpendicular to e_x in the initial configuration (i.e., the distance to the singularity is as large as possible).

An additional frame_pa is present. It is defined in the following way:

Finally, a frame_pb is present. It is defined in the following way:

 

Parameters

NameDefaultDescription
n_a[3]{0,0,1}Axis of revolute joint 1 of universal joint resolved in frame_a

Modelica definition

model JointUPS 
  "Universal - prismatic - spherical joint aggregation (no constraints and no states)"
   
  
  import SI = Modelica.SIunits;
  parameter Real n_a[3]={0,0,1} 
    "Axis of revolute joint 1 of universal joint resolved in frame_a";
  
  ModelicaAdditions.MultiBody.Interfaces.Frame_a frame_a;
  ModelicaAdditions.MultiBody.Interfaces.Frame_a frame_b;
  ModelicaAdditions.MultiBody.Interfaces.Frame_b frame_pa;
  ModelicaAdditions.MultiBody.Interfaces.Frame_b frame_pb;
  
  Real S_rel[3, 3] "Transformation matrix from frame_a to frame_b";
  SI.Position r_rela[3] 
    "Vector from origin of frame_a to origin of frame_b, resolved in frame_a";
  SI.Velocity v_rela[3] "= der(r_rela)";
  SI.Acceleration a_rela[3] "= der(v_rela)";
  SI.Power P "Total power stored in this element (should be zero)";
  
protected 
  SI.Velocity v_aux[3] "= cross(frame_a.w, r_rela)";
  SI.Length norm_r_rela "length of vector r_rel_a";
  SI.Length norm_n_y "length of vector n_y";
  Real e_x[3] 
    "Unit vector from origin of frame_a to origin of frame_b, resolved in frame_a";
  Real e_y[3] 
    "Unit vector perpendicular to n_a and to e_x, resolved in frame_a";
  Real e_z[3] 
    "Unit vector perpendicular to e_x and to e_z, resolved in frame_a";
  Real n_y[3] "Vector perpendicular to n_a and to e_x, resolved in frame_a";
  Real S_pa_a[3, 3] "Transformation matrix from frame_a to frame_pa";
  Real S_pb_b[3, 3] "Transformation matrix from frame_b to frame_pb";
  Real e_y2[3] "= e_y/norm_r_rela";
  SI.AngularVelocity w_rel_pa[3] 
    "Angular velocity of frame_pa with respect to frame_a, resolved in frame_pa";
  SI.AngularAcceleration z_rel_pa[3] 
    "Angular acceleration of frame_pa with respect to frame_a, resolved in frame_pa";
  Real W[3, 3](each unit="rad/m") "w_rel_c = W*v_rela";
  SI.Velocity v_rela_scaled[3] "= v_rela/norm_r_rela";
  Real der_e_x[3] "= der(e_x)";
  Real der_e_y[3] "= der(e_y)";
  Real der_e_z[3] "= der(e_z)";
  Real aux1[3] "= cross(n_a, e_x)";
  Real norm_aux1 "=sqrt(aux1*aux1)";
  Real aux2;
  Real aux3[3];
  SI.Torque t_cd_a[3] "= frame_pa.t + frame_pb.t, resolved in frame_a";
  SI.Force f_bd_a[3] "= frame_b.f + frame_pb.f, resolved in frame_a";
  Real WW[3, 3];
protected 
  parameter Real S_pa_c[3, 3](each fixed=false) 
    "Transformation matrix from frame_c to frame_pa";
initial equation 
  S_pa_c = [e_x, e_y, e_z];
equation 
  S_rel = transpose(frame_b.S)*frame_a.S;
  r_rela = transpose(frame_a.S)*(frame_b.r0 - frame_a.r0);
  v_aux = cross(frame_a.w, r_rela);
  v_rela = transpose(S_rel)*frame_b.v - frame_a.v - v_aux;
  a_rela = transpose(S_rel)*frame_b.a - frame_a.a - cross(frame_a.z, r_rela) - 
    cross(frame_a.w, v_aux + 2*v_rela);
  
  /* Compute relative transformation matrix S_pa_a (from frame_a to frame_pa) and
     absolute transformation matrices frame_pa.S, frame_pb.S
  */
  norm_r_rela = noEvent(sqrt(r_rela*r_rela));
  e_x = r_rela/norm_r_rela;
  n_y = cross(n_a, e_x);
  norm_n_y = noEvent(sqrt(n_y*n_y));
  e_y = n_y/norm_n_y;
  e_z = cross(e_x, e_y);
  S_pa_a = S_pa_c*transpose([e_x, e_y, e_z]);
  frame_pa.S = frame_a.S*transpose(S_pa_a);
  frame_pb.S = frame_pa.S;
  
  /* Compute relative angular velocity between frame_a and frame_pa
     (the derivation is long, but the result is simple)
  */
  e_y2 = e_y/norm_r_rela;
  aux1 = cross(n_a, e_x);
  norm_aux1 = noEvent(sqrt(aux1*aux1));
  aux2 = n_a*e_x;
  W = [aux2/norm_aux1*transpose([e_y2]); -transpose([e_z/norm_r_rela]); 
    transpose([e_y2])];
  WW = S_pa_c*W;
  w_rel_pa = WW*v_rela;
  frame_pa.w = S_pa_a*frame_a.w + w_rel_pa;
  frame_pb.w = frame_pa.w;
  
  /* Compute relative angular acceleration between frame_a and frame_pa
     (the derivation is long)
  */
  v_rela_scaled = v_rela/norm_r_rela;
  der_e_x = v_rela_scaled - (e_x*v_rela_scaled)*e_x;
  aux3 = cross(n_a, der_e_x)/norm_n_y;
  der_e_y = aux3 - (e_y*aux3)*e_y;
  der_e_z = cross(der_e_x, e_y) + cross(e_x, der_e_y);
  z_rel_pa = S_pa_c*(W*(a_rela - v_rela*(e_x*v_rela_scaled)) + [((n_a*der_e_x)*
    transpose([e_y]) + aux2*transpose([der_e_y]))/norm_aux1; -transpose([
    der_e_z]); transpose([der_e_y])]*v_rela_scaled);
  frame_pa.z = S_pa_a*frame_a.z + z_rel_pa + cross(frame_pa.w, w_rel_pa);
  frame_pb.z = frame_pa.z;
  
  /* Set remaining kinematic quantities of frame_pa and frame_pb */
  frame_pa.r0 = frame_a.r0;
  frame_pa.v = S_pa_a*frame_a.v;
  frame_pa.a = S_pa_a*frame_a.a;
  
  S_pb_b = transpose(frame_pb.S)*frame_b.S;
  frame_pb.r0 = frame_b.r0;
  frame_pb.v = S_pb_b*frame_b.v;
  frame_pb.a = S_pb_b*frame_b.a;
  
  /* Force and torque balance */
  t_cd_a = transpose(S_pa_a)*(frame_pa.t + frame_pb.t);
  f_bd_a = -transpose(WW)*t_cd_a;
  frame_b.f = -transpose(S_pb_b)*frame_pb.f + S_rel*f_bd_a;
  frame_b.t = zeros(3);
  frame_a.f = -transpose(S_pa_a)*frame_pa.f - f_bd_a;
  frame_a.t = -t_cd_a - cross(r_rela, f_bd_a);
  
  /* Total power (should be zero) */
  P = frame_a.f*frame_a.v + frame_a.t*frame_a.w + frame_b.f*frame_b.v + frame_b
    .t*frame_b.w + frame_pa.f*frame_pa.v + frame_pa.t*frame_pa.w + frame_pb.f*
    frame_pb.v + frame_pb.t*frame_pb.w;
  
end JointUPS;

VehicleDynamics.Utilities.Joints.JointRSU VehicleDynamics.Utilities.Joints.JointRSU

Revolute - spherical - universal - joint aggregation (no constraints, no states)

VehicleDynamics.Utilities.Joints.JointRSU

Information


This joint aggregation assumes that it is used in a kinematic loop which becomes a tree-structure when the JointRSU element is removed. In this case JointRSU analytically computes the unknown revolute joint angle by analytically solving a non-linear equation. As a result the kinematic loop is analytically solved.

At the initial time, the coordinate systems frame_ta and frame_tb are parallel to frame_a, frame_c is parallel to frame_b and the angle of the revolute joint (= rev.q) is zero.


Parameters

NameDefaultDescription
n_a[3]{0,0,1}Axis of rotation of revolute joint resolved in frame_a
n_b[3]{0,0,1}First rotation axis of universal joint fixed and resolved in frame_b
r_a[3] Position vector from origin of frame_a to spherical joint at initial time, resolved in frame_a [m]

Modelica definition

model JointRSU 
  "Revolute - spherical - universal - joint aggregation (no constraints, no states)"
   
  
  import SI = Modelica.SIunits;
  
  extends ModelicaAdditions.MultiBody.Interfaces.Interact2;
  parameter Real n_a[3]={0,0,1} 
    "Axis of rotation of revolute joint resolved in frame_a";
  parameter Real n_b[3]={0,0,1} 
    "First rotation axis of universal joint fixed and resolved in frame_b";
  parameter SI.Position r_a[3] 
    "Position vector from origin of frame_a to spherical joint at initial time, resolved in frame_a";
  final parameter SI.Position rL_init0[3](each fixed=false) 
    "Position vector from spherical to universal joint at initial time, resolved in inertial system";
  final parameter SI.Position L(fixed=false) 
    "Distance between universal and spherical joint";
  SI.Power P "Total power flowing into this element (has to be zero)";
  Real c;
  
  
  ModelicaAdditions.MultiBody.Parts.FrameTranslation frameTranslation(r=r_a);
  DependentRevolute rev(
    n=n_a, 
    L=L, 
    q_guess=0) "dependent revolute joint";
  ModelicaAdditions.MultiBody.Interfaces.Frame_b frame_c;
  JointUS jointUS(
    L=L, 
    n_a=n_b, 
    kinematicConstraint=false);
  ModelicaAdditions.MultiBody.Interfaces.Frame_b frame_ta;
  ModelicaAdditions.MultiBody.Interfaces.Frame_b frame_tb;
initial equation 
  rL_init0 = frame_b.r0 - (frame_a.r0 + frame_a.S*r_a);
  L = sqrt(rL_init0*rL_init0);
equation 
  rev.relativePosition_a.signal = vector([r_rela; v_rela; a_rela]);
  rev.relativePosition_b.signal = vector([r_a; zeros(3); zeros(3)]);
  P = frame_a.f*frame_a.v + frame_a.t*frame_a.w + frame_b.f*frame_b.v + frame_b
    .t*frame_b.w + frame_c.f*frame_c.v + frame_c.t*frame_c.w + frame_ta.f*
    frame_ta.v + frame_ta.t*frame_ta.w + frame_tb.f*frame_tb.v + frame_tb.t*
    frame_tb.w;
  
  /* Compute force in the rod of jointUS */
  c = (cross(n_a, r_a)*(jointUS.S_rel*jointUS.nx));
  jointUS.fRod = -n_a*(frame_ta.t + frame_tb.t + cross(r_a, frame_tb.f - 
    jointUS.fb_b1))/noEvent(if abs(c) < 1.e-10 then 1.e-10 else c);
  connect(frameTranslation.frame_b, jointUS.frame_b);
  connect(jointUS.frame_a, frame_b);
  connect(jointUS.frame_c, frame_c);
  connect(frame_ta, frameTranslation.frame_a);
  connect(frameTranslation.frame_b, frame_tb);
  connect(rev.frame_a, frame_a);
  connect(rev.frame_b, frameTranslation.frame_a);
end JointRSU;

VehicleDynamics.Utilities.Joints.JointUS VehicleDynamics.Utilities.Joints.JointUS

Universal - Spherical joint aggregation (one constraint)

VehicleDynamics.Utilities.Joints.JointUS

Information


This component consists of a universal joint at frame_a and a spherical joint at frame_b that are connected together with a rigid rod. This joint aggregation has no mass and no inertia and introduces the constraint that the distance between the origin of frame_a and the origin of frame_b is constant (= parameter L). The universal joint is defined in the following way:

Note, a singularity takes place when n_a = e_x, i.e., when n_a is along the line connecting the universal and the spherical joint. Therefore, if possible n_a should be selected in such a way that it is perpendicular to e_x in the initial configuration (i.e., the distance to the singularity is as large as possible).

An additional frame_c is present. It is defined in the following way:

Note, this joint aggregation can be used in many cases also, if in reality a rod with spherical joints at each end are present. Such a system has an additional degree of freedom to rotate the rod along its axis. In practice this rotation is usually of no interested and is mathematically removed by replacing one of the spherical joints by a universal joint.


Parameters

NameDefaultDescription
L1length of the rod (distance origin of frame_a to origin of frame_b) [m]
n_a[3]{0,0,1}First rotation axis of universal joint resolved in frame_a
kinematicConstrainttrue = false, if used together with DependentRevolute to solve loop analytically

Modelica definition

model JointUS 
  "Universal - Spherical joint aggregation (one constraint)" 
  import SI = Modelica.SIunits;
  extends ModelicaAdditions.MultiBody.Interfaces.CutJoint;
  parameter SI.Length L=1 
    "length of the rod (distance origin of frame_a to origin of frame_b)";
  parameter Real n_a[3]={0,0,1} 
    "First rotation axis of universal joint resolved in frame_a";
  parameter Boolean kinematicConstraint=true 
    "|Advanced|| = false, if used together with DependentRevolute to solve loop analytically";
  SI.Force fRod "Constraint force in direction of the rod";
  SI.Power P "Total power flowing into this element (has to be zero)";
  
protected 
  Real Sc[3, 3];
  SI.Position r0c[3];
  Real vc[3];
  Real wc[3];
  Real ac[3];
  Real zc[3];
  Real b1[3];
  Real bd[3];
  Real bdd[3];
  Real normb;
  Real bb;
  Real nx[3];
  Real ny[3];
  Real nz[3];
  Real S_relc[3, 3];
  Real nxd[3];
  Real nyd[3];
  Real nzd[3];
  SI.AngularVelocity w_relc[3];
  Real nxdd[3];
  Real nydd[3];
  Real nzdd[3];
  Real z_relc[3];
  Real constraintResidue;
  Real constraintResidue_d;
  Real constraintResidue_dd;
  SI.Torque tc_a[3] "Torque frame_c.t resolved in frame_a";
  SI.Force fb_a[3] "Force frame_b.f resolved in frame_a";
  SI.Force fb_a1[3];
  SI.Force fb_b1[3];
  
public 
  ModelicaAdditions.MultiBody.Interfaces.Frame_b frame_c(
    S=Sc, 
    r0=r0c, 
    v=vc, 
    w=wc, 
    a=ac, 
    z=zc);
protected 
  parameter Real S_init[3, 3](each fixed=false) 
    "Transformation matrix from [e_x, e_y, e_z] to frame_a at initializiation";
initial equation 
  S_init = [nx, ny, nz];
  // Constraint equations on position, velocity and acceleration level
equation 
  P = frame_a.f*frame_a.v + frame_a.t*frame_a.w + frame_b.f*frame_b.v + frame_b
    .t*frame_b.w + frame_c.f*frame_c.v + frame_c.t*frame_c.w;
  if kinematicConstraint then
    constraintResidue = (r_rela*r_rela - L*L)/2;
    constraintResidue_d = r_rela*v_rela;
    constraintResidue_dd = r_rela*a_rela + v_rela*v_rela;
    constrain(constraintResidue, constraintResidue_d, constraintResidue_dd);
    // constraintResidue = 0;
  else
    constraintResidue = 0;
    constraintResidue_d = 0;
    constraintResidue_dd = 0;
  end if;
  
  /*Compute relative transformation matrix S_relc (cut a  cut c) and
 absolute transformation matrix Sc (cut c  inertial system)*/
  nx = r_rela/L;
  b1 = cross(n_a, nx);
  normb = sqrt(b1*b1);
  ny = b1/normb;
  nz = cross(nx, ny);
  S_relc = S_init*transpose([nx, ny, nz]);
  Sc = Sa*transpose(S_relc);
  
  /*Compute relative angular velocity w_relc and absolute angular velocity wc
 (both w_relc and wc are resolved in cut-frame C):
 w_relc = vec(S_relc*der(S_relc')), where h=vec([  0, -h3,  h2
                                               h3,   0, -h1
                                              -h2,  h1,   0 ])
 nxd = der(nx), nxdd = der(nxd)
 nyd = der(ny), nydd = der(nyd)
 nzd = der(nz), nzdd = der(nzd)
 nyd    = der( b/normb )
        = der(b)/normb + b*der(1/normb)
        = der(b)/normb - b*( ny'*der(b)/normb ) / normb
        = der(b)/normb - ( ny'*der(b)/normb )*ny
        = bd - (ny'*bd)*ny
*/
  nxd = v_rela/L;
  bd = cross(n_a, nxd)/normb;
  bb = ny*bd;
  nyd = bd - bb*ny;
  nzd = cross(nxd, ny) + cross(nx, nyd);
  w_relc = S_init*{nz*nyd,nx*nzd,ny*nxd};
  wc = S_relc*wa + w_relc;
  
  /*Compute relative angular acceleration z_relc and absolute angular
 acceleration zc, with z_relc = der(w_relc).
 der(bd)  = cross(n_a,nxdd)/normb - cross(n_a,nxd)*der(1/normb)
          = cross(n_a,nxdd)/normb + cross(n_a,nxd)*( ny'*der(b)/normb ) / normb
          = cross(n_a,nxdd)/normb + bd*(ny'*bd)
 der(nyd) = der(bd) - der(bb)*ny - bb*der(ny)
          = der(bd) - (nyd'*bd + ny'*der(bd))*ny - bb*nyd
*/
  nxdd = a_rela/L;
  bdd = cross(n_a, nxdd)/normb - bb*bd;
  nydd = bdd - (nyd*bd + ny*bdd)*ny - bb*nyd;
  
  nzdd = cross(nxdd, ny) + cross(nx, nydd) + 2*cross(nxd, nyd);
  z_relc = S_init*{nz*nydd + nzd*nyd,nx*nzdd + nxd*nzd,ny*nxdd + nyd*nxd};
  zc = S_relc*za + cross(wc, w_relc) + z_relc;
  
  /*Compute translational quantities of cut c*/
  r0c = r0a;
  vc = S_relc*va;
  ac = S_relc*aa;
  
  /*Compute cut-forces/torques at frame_a/b */
  tc_a = transpose(S_relc)*frame_c.t;
  fb_a1 = -((tc_a*n_a)/(L*normb))*ny + ((tc_a*ny)/L)*nz;
  fb_b1 = S_rel*fb_a1;
  fb_a = fb_a1 - fRod*nx;
  frame_b.f = S_rel*fb_a;
  frame_a.f = -fb_a - transpose(S_relc)*frame_c.f;
  frame_b.t = zeros(3);
  frame_a.t = -tc_a - cross(r_rela, fb_a);
end JointUS;

VehicleDynamics.Utilities.Joints.DependentRevolute VehicleDynamics.Utilities.Joints.DependentRevolute

Revolute joint where the rotation angle is computed from a length constraint

VehicleDynamics.Utilities.Joints.DependentRevolute

Information


Joint where frame_b rotates around axis n which is fixed in frame_a. The joint axis has an additional flange where it can be driven with elements of the Modelica.Mechanics.Rotational library. The relative angle q [rad] and the relative angular velocity qd [rad/s] are used as state variables.

The following parameters are used to define the joint:

  n : Axis of rotation resolved in frame_a (= same as in frame_b).
      n  must not necessarily be a unit vector. E.g.,
         n = {0, 0, 1} or n = {1, 0, 1}
  q0: Rotation angle offset in [deg].
      If q=q0, frame_a and frame_b are identical.
  startValueFixed: true, if start values of q, qd are fixed.

Parameters

NameDefaultDescription
n[3]{0,0,1}Axis of rotation resolved in frame_a (= same as in frame_b)
q00Rotation angle offset (see info) [deg]
L1length of connecting rod [m]
q_guess0Guess for q at initial time in [deg]

Modelica definition

model DependentRevolute 
  "Revolute joint where the rotation angle is computed from a length constraint"
   
  
  import Modelica.Math.*;
  extends ModelicaAdditions.MultiBody.Interfaces.TreeJoint;
  parameter Real n[3]={0,0,1} 
    "Axis of rotation resolved in frame_a (= same as in frame_b)";
  parameter Real q0=0 "Rotation angle offset (see info) [deg]";
  parameter SI.Position L=1 "length of connecting rod";
  parameter Real q_guess=0 "Guess for q at initial time in [deg]";
  
  final parameter Boolean positiveBranch(fixed=false) 
    "Based on q_guess, selection of one of the two solutions";
  SI.Position r1_a[3] 
    "Vector from frame_a to ConnectingRod frame_a, resolved in frame_a";
  SI.Position r1_b[3];
  SI.Position r2_b[3] 
    "Vector from frame_b to ConnectingRod frame_b, resolved in frame_b";
  SI.Position nr1;
  SI.Position nr2;
  
  // Constraint equation: A*cos(phi) + B*sin(phi) + C = 0
  SI.Length A;
  SI.Length B;
  SI.Length C;
  Real k1;
  Real k2;
  
  SI.Angle q(stateSelect=StateSelect.never);
  SI.AngularVelocity qd(stateSelect=StateSelect.never);
  SI.AngularAcceleration qdd;
  Real q_deg;
  Real nn[3];
  Real sinq;
  Real cosq;
  
  //Real LL;
  //Real delta;
  // Real qd2;
  Real v1_a[3];
  Real v1_b[3];
  Real v2_b[3];
  SI.Position r_diff_b[3];
  SI.Velocity v_diff_b[3];
  Real a1_a[3];
  Real a2_b[3];
  Real der_r1_b[3];
  Real der_r_diff_b[3];
  Real der_v_diff_b[3];
  Real k3[3];
  Real k4;
  Real k4a;
  Real qd2;
  Real qdd2;
  
  Real constraintResidue;
  Real constraintResidue_d;
  Real constraintResidue_dd;
  
  Modelica.Mechanics.Rotational.Interfaces.Flange_a axis;
  Modelica.Mechanics.Rotational.Interfaces.Flange_b bearing;
  Modelica.Blocks.Interfaces.InPort relativePosition_a(n=9);
  Modelica.Blocks.Interfaces.InPort relativePosition_b(n=9);
  function selectBranch 
    "determine branch which is closest to initial angle q_guess" 
    
    import Modelica.Math.*;
    input SI.Position L "length of connetion rod";
    input Real q_guess "guess of initial angle in [deg]";
    input Real n[3] 
      "unit vector along axis of rotation, resolved in frame_a/frame_b";
    input SI.Position r1[3] 
      "position vector from frame_a to connectingRod, resolved in frame_a";
    input SI.Position r2[3] 
      "position vector from frame_b to connectingRod, resolved in frame_b";
    output Boolean positiveBranch "branch of the initial solution";
  protected 
    Real nr1;
    Real nr2;
    Real A;
    Real B;
    Real C;
    Real k1;
    Real k2;
    Real cosq1;
    Real sinq1;
    Real cosq2;
    Real sinq2;
    Real q1;
    Real q2;
  algorithm 
    nr1 := n*r1;
    nr2 := n*r2;
    A := -2*(r2*r1 - nr1*nr2);
    B := 2*r2*cross(n, r1);
    C := r1*r1 + r2*r2 - L*L - 2*nr1*nr2;
    
    k1 := A*A + B*B;
    k2 := sqrt(k1 - C*C);
    
    cosq1 := (-A*C + B*k2)/k1;
    sinq1 := (-B*C - A*k2)/k1;
    q1 := atan2(sinq1, cosq1);
    
    cosq2 := (-A*C - B*k2)/k1;
    sinq2 := (-B*C + A*k2)/k1;
    q2 := atan2(sinq2, cosq2);
    
    if abs(q1 - q_guess) <= abs(q2 - q_guess) then
      positiveBranch := true;
    else
      positiveBranch := false;
    end if;
  end selectBranch;
  
initial equation 
  positiveBranch = selectBranch(L, q_guess, nn, r1_a, r2_b);
equation 
  axis.phi = q;
  bearing.phi = 0;
  
  /*rotation matrix*/
  nn = n/sqrt(n*n);
  S_rel = [nn]*transpose([nn]) + (identity(3) - [nn]*transpose([nn]))*cosq - 
    skew(nn)*sinq;
  
  /*other kinematic quantities*/
  r_rela = zeros(3);
  v_rela = zeros(3);
  a_rela = zeros(3);
  w_rela = nn*qd2;
  z_rela = nn*qdd2;
  
  /* Transform the kinematic quantities from frame_a to frame_b and the
     force and torque acting at frame_b to frame_a
     (= general equations of a "TreeJoint" specialized to this class).
  */
  Sb = Sa*transpose(S_rel);
  r0b = r0a;
  
  vb = S_rel*va;
  wb = S_rel*(wa + w_rela);
  
  ab = S_rel*aa;
  zb = S_rel*(za + z_rela + cross(wa, w_rela));
  
  fa = transpose(S_rel)*fb;
  ta = transpose(S_rel)*tb;
  
  // d'Alemberts principle
  // axis.tau = nn*tb;
  
  // Loop close conditions
  r1_a = relativePosition_a.signal[1:3];
  r2_b = relativePosition_b.signal[1:3];
  
  // L = sqrt((S_rel*r1_a - r2_b)*(S_rel*r1_a - r2_b))
  nr1 = nn*r1_a;
  nr2 = nn*r2_b;
  A = -2*(r2_b*r1_a - nr1*nr2);
  B = 2*r2_b*cross(nn, r1_a);
  C = r1_a*r1_a + r2_b*r2_b - L*L - 2*nr1*nr2;
  
  k1 = A*A + B*B;
  k2 = noEvent(sqrt(k1 - C*C));
  
  cosq = (-A*C + (if positiveBranch then B else -B)*k2)/k1;
  sinq = (-B*C + (if positiveBranch then -A else A)*k2)/k1;
  
  //delta = A*cosq + B*sinq + C;
  
  //q = atan2(sinqq, cosqq);
  //q = atan(sinq/cosq);
  
  v1_a = relativePosition_a.signal[4:6];
  v2_b = relativePosition_b.signal[4:6];
  r1_b = S_rel*r1_a;
  v1_b = S_rel*v1_a;
  r_diff_b = r1_b - r2_b;
  v_diff_b = v1_b - v2_b;
  k4a = r_diff_b*cross(nn, r1_b);
  k4 = noEvent(if abs(k4a) > 1.e-10 then k4a else 1.e-10);
  qd2 = r_diff_b*v_diff_b/k4;
  
  a1_a = relativePosition_a.signal[7:9];
  a2_b = relativePosition_b.signal[7:9];
  k3 = qd*cross(nn, r1_b);
  
  der_r1_b = v1_b - k3;
  der_r_diff_b = der_r1_b - v2_b;
  der_v_diff_b = S_rel*a1_a - qd*cross(nn, v1_b) - a2_b;
  
  qdd2 = (der_r_diff_b*(v_diff_b - qd*cross(nn, r1_b)) + r_diff_b*(der_v_diff_b
     - qd*cross(nn, der_r1_b)))/k4;
  
  qd = der(q);
  qdd = der(qd);
  q_deg = q*180/Modelica.Constants.pi;
  constraintResidue = q - atan2(sinq, cosq);
  constraintResidue_d = qd - qd2;
  constraintResidue_dd = qdd - qdd2;
  constrain(constraintResidue, constraintResidue_d, constraintResidue_dd);
end DependentRevolute;

VehicleDynamics.Utilities.Joints.RelDistance2 VehicleDynamics.Utilities.Joints.RelDistance2

Distance between flange_a and flange_b is converted to translational cuts.

VehicleDynamics.Utilities.Joints.RelDistance2

Information

 

Parameters

NameDefaultDescription
s00Relative distance offset of translational joint (distance frame_a -> frame_b = s0 + axis.s) [m]
sEps1.E-6prevent zero-division if rel. distance s=0 [m]

Modelica definition

model RelDistance2 
  "Distance between flange_a and flange_b is converted to translational cuts."
   
  
  import SI = Modelica.SIunits;
  parameter SI.Position s0=0 
    "Relative distance offset of translational joint (distance frame_a -> frame_b = s0 + axis.s)";
  extends ModelicaAdditions.MultiBody.Interfaces.LineForce;
  
  SI.Position res1;
  SI.Position res2;
  
  Modelica.Mechanics.Translational.Interfaces.Flange_b bearing;
  Modelica.Mechanics.Translational.Interfaces.Flange_a axis;
equation 
  res1 = axis.s + s0 - s;
  res2 = der(axis.s) - sd;
  constrain(res1, res2);
  bearing.s = 0;
  axis.f = -f;
end RelDistance2;

VehicleDynamics.Utilities.Joints.FreeMotion4 VehicleDynamics.Utilities.Joints.FreeMotion4

Free motion joint (6 degrees-of-freedom, used in spanning tree)

VehicleDynamics.Utilities.Joints.FreeMotion4

Information

 

Joint which does not constrain the motion between frame_a and frame_b. Such a joint is just used to define the desired states to be used. The joint is realized in such a way, that a singularity cannot occur. This is achieved because the Cardan angles are defined between a frame_fix fixed in frame_a and frame_b. Whenever the Cardan angles are near a singularity, the integration is stopped and frame_fix is changed, such that the Cardan angles are far away from the singularity. The following state variables are used:

  r_rela[3]: Distance vector from the origin of frame_a to the origin
             of frame_b, resolved in frame_a in [m].
  phi[3]   : Cardan angles, also called Tait-Bryan angles, i.e.,
             rotate around 1-, 2-, 3-axis in [rad] from intermediate
             frame_fix, which is fixed in frame_a, to frame_b. Initially, frame_fix
             is identical to frame_a. If phi[2] is near its singularity (= pi/2 or -pi/2),
             the frame_fix and phi are changed, such that phi[2] is far away from 
             its singularity.
  v_rela[3]: = der(r_rela); relative velocity of frame_b with respect to frame_a
             resolved in frame_a in [m/s].
  w_rela[3]: Relative angular velocity of frame_b with respect to frame_a
             resolved in frame_a in [rad/s].

Modelica definition

model FreeMotion4 
  "Free motion joint (6 degrees-of-freedom, used in spanning tree)" 
  
  import SI = Modelica.SIunits;
  SI.Position r_rela[3](each stateSelect=StateSelect.always) = {t_x.q,t_y.q,
    t_z.q} "Position vector from frame_a to frame_b, resolved in frame_a";
  SI.Velocity v_rela[3](each stateSelect=StateSelect.always) = {t_x.qd,t_y.
    qd,t_z.qd} "= der(r_rela)";
  SI.Angle phi[3](each stateSelect=StateSelect.always) = {r_x.q,r_y.q,r_z.q}
     "Cardan angles from frame_a to frame_b";
  SI.AngularVelocity w_rela[3](each stateSelect=StateSelect.always) = {r_x.
    qd,r_y.qd,r_z.qd} "= der(phi)";
  
  
  ModelicaAdditions.MultiBody.Joints.Revolute r_y(n={0,1,0});
  ModelicaAdditions.MultiBody.Joints.Revolute r_z(n={0,0,1});
  ModelicaAdditions.MultiBody.Joints.Revolute r_x(n={1,0,0});
  ModelicaAdditions.MultiBody.Joints.Prismatic t_y(n={0,1,0});
  ModelicaAdditions.MultiBody.Joints.Prismatic t_x;
  ModelicaAdditions.MultiBody.Joints.Prismatic t_z(n={0,0,1});
  ModelicaAdditions.MultiBody.Interfaces.Frame_a frame_a;
  ModelicaAdditions.MultiBody.Interfaces.Frame_b frame_b;
equation 
  
  connect(t_x.frame_a, frame_a);
  connect(t_x.frame_b, t_y.frame_a);
  connect(t_y.frame_b, t_z.frame_a);
  connect(t_z.frame_b, r_x.frame_a);
  connect(r_x.frame_b, r_y.frame_a);
  connect(r_y.frame_b, r_z.frame_a);
  connect(r_z.frame_b, frame_b);
end FreeMotion4;

VehicleDynamics.Utilities.Joints.FreeMotion3 VehicleDynamics.Utilities.Joints.FreeMotion3

Free motion joint (6 degrees-of-freedom, used in spanning tree)

VehicleDynamics.Utilities.Joints.FreeMotion3

Information

 

Joint which does not constrain the motion between frame_a and frame_b. Such a joint is just used to define the desired states to be used. The joint is realized in such a way, that a singularity cannot occur. This is achieved because the Cardan angles are defined between a frame_fix fixed in frame_a and frame_b. Whenever the Cardan angles are near a singularity, the integration is stopped and frame_fix is changed, such that the Cardan angles are far away from the singularity. The following state variables are used:

  r_rela[3]: Distance vector from the origin of frame_a to the origin
             of frame_b, resolved in frame_a in [m].
  phi[3]   : Cardan angles, also called Tait-Bryan angles, i.e.,
             rotate around 1-, 2-, 3-axis in [rad] from intermediate
             frame_fix, which is fixed in frame_a, to frame_b. Initially, frame_fix
             is identical to frame_a. If phi[2] is near its singularity (= pi/2 or -pi/2),
             the frame_fix and phi are changed, such that phi[2] is far away from 
             its singularity.
  v_rela[3]: = der(r_rela); relative velocity of frame_b with respect to frame_a
             resolved in frame_a in [m/s].
  w_rela[3]: Relative angular velocity of frame_b with respect to frame_a
             resolved in frame_a in [rad/s].

Modelica definition

model FreeMotion3 
  "Free motion joint (6 degrees-of-freedom, used in spanning tree)" 
  
  // S_rel needs a correct start value, because pre(S_rel) is referenced below
  extends ModelicaAdditions.MultiBody.Interfaces.TreeJoint(S_rel(start=identity(3)));
  SI.Angle phi[3] "Cardan angles from a frame fixed in frame_a to frame_b";
protected 
  constant SI.Angle phi2_critical_deg=80 
    "angle in [deg] too close to singularity. Redefine S_fix and phi";
  constant SI.Angle phi2_critical=phi2_critical_deg*Modelica.Constants.pi/180.0;
  constant Real c2_small=1.e-5 
    "if cos(phi[2]) < c2_small, c2_small is used as guard against zero division";
  SI.Velocity vaux[3];
  Real s1;
  Real s2;
  Real s3;
  Real c1;
  Real c2;
  Real c2a;
  Real c3;
  Boolean switch_state;
  Real S_phi[3, 3] "S_rel = S_phi(phi)*S_fix";
  Real S_fix[3, 3](start=identity(3)) "S_rel = S_phi(phi)*S_fix";
  SI.AngularVelocity w_fix[3] 
    "Relative angular velocity resolved in intermediate frame S_fix";
  
equation 
  
  /* Determine sines and cosines of the Cardan angles */
  s1 = Modelica.Math.sin(phi[1]);
  s2 = Modelica.Math.sin(phi[2]);
  s3 = Modelica.Math.sin(phi[3]);
  c1 = Modelica.Math.cos(phi[1]);
  c2a = Modelica.Math.cos(phi[2]);
  c3 = Modelica.Math.cos(phi[3]);
  
  /* Below, some expressions are divided by c2. By construction, it is not possible
     that c2=0, during continuous simulation. However, at initial time and when
     large numerical errors occur, c2=0 is possible, which would result in a division
     by zero. The following statement is a guard against this unlikely situation.
  */
  c2 = if noEvent(c2a > c2_small or c2a < -c2_small) then c2a else if noEvent(
    c2a >= 0) then c2_small else -c2_small;
  
  /* Relative transformation matrix
       S_phi = [ c3, s3, 0; 
                -s3, c3, 0; 
                  0, 0, 1]*[c2, 0, -s2; 
                             0, 1, 0; 
                            s2, 0, c2]*[1, 0, 0; 
                                        0, c1, s1; 
                                        0, -s1, c1];
  */
  switch_state = phi[2] >= phi2_critical or phi[2] <= -phi2_critical;
  S_fix = identity(3);
  /*when switch_state then
    S_fix = pre(S_rel);
    reinit(phi, zeros(3));
  end when;
  */
  S_phi = [c2*c3, c1*s3 + s1*s2*c3, s1*s3 - c1*s2*c3; -c2*s3, c1*c3 - s1*s2*s3, 
     s1*c3 + c1*s2*s3; s2, -s1*c2, c1*c2];
  S_rel = S_phi*S_fix;
  
  // Kinematic differential equations for translational motion
  der(r_rela) = v_rela;
  //der(v_rela) = a_rela;
  a_rela = zeros(3);
  // Kinematic differential equations for rotational motion    
  w_fix = S_fix*w_rela;
  der(phi) = {w_fix[1] + (s1*w_fix[2] - c1*w_fix[3])*s2/c2,c1*w_fix[2] + s1*
    w_fix[3],(-s1*w_fix[2] + c1*w_fix[3])/c2};
  //der(w_rela) = z_rela;
  z_rela = zeros(3);
  
  // Kinematic relationships
  frame_b.S = frame_a.S*transpose(S_rel);
  frame_b.r0 = frame_a.r0 + frame_a.S*r_rela;
  
  vaux = cross(frame_a.w, r_rela);
  frame_b.v = S_rel*(frame_a.v + v_rela + vaux);
  frame_b.w = S_rel*(frame_a.w + w_rela);
  
  frame_b.a = S_rel*(frame_a.a + cross(frame_a.z, r_rela) + cross(frame_a.w, 
    vaux + 2*v_rela) + a_rela);
  frame_b.z = S_rel*(frame_a.z + cross(frame_a.w, w_rela) + z_rela);
  
  // cut-forces and cut-torques are zero
  frame_a.f = zeros(3);
  frame_a.t = zeros(3);
  frame_b.f = zeros(3);
  frame_b.t = zeros(3);
end FreeMotion3;

VehicleDynamics.Utilities.Joints.FreeMotion2 VehicleDynamics.Utilities.Joints.FreeMotion2

Free motion joint (6 degrees-of-freedom, used in spanning tree)

VehicleDynamics.Utilities.Joints.FreeMotion2

Information

 

Joint which does not constrain the motion between frame_a and frame_b. Such a joint is just used to define the desired states to be used. The joint is realized in such a way, that a singularity cannot occur. This is achieved because the Cardan angles are defined between a frame_fix fixed in frame_a and frame_b. Whenever the Cardan angles are near a singularity, the integration is stopped and frame_fix is changed, such that the Cardan angles are far away from the singularity. The following state variables are used:

  r_rela[3]: Distance vector from the origin of frame_a to the origin
             of frame_b, resolved in frame_a in [m].
  phi[3]   : Cardan angles, also called Tait-Bryan angles, i.e.,
             rotate around 1-, 2-, 3-axis in [rad] from intermediate
             frame_fix, which is fixed in frame_a, to frame_b. Initially, frame_fix
             is identical to frame_a. If phi[2] is near its singularity (= pi/2 or -pi/2),
             the frame_fix and phi are changed, such that phi[2] is far away from 
             its singularity.
  v_rela[3]: = der(r_rela); relative velocity of frame_b with respect to frame_a
             resolved in frame_a in [m/s].
  w_rela[3]: Relative angular velocity of frame_b with respect to frame_a
             resolved in frame_a in [rad/s].

Modelica definition

model FreeMotion2 
  "Free motion joint (6 degrees-of-freedom, used in spanning tree)" 
  
  // S_rel needs a correct start value, because pre(S_rel) is referenced below
  extends ModelicaAdditions.MultiBody.Interfaces.TreeJoint(S_rel(start=identity(3)));
  SI.Angle phi[3] "Cardan angles from a frame fixed in frame_a to frame_b";
protected 
  constant SI.Angle phi2_critical_deg=80 
    "angle in [deg] too close to singularity. Redefine S_fix and phi";
  constant SI.Angle phi2_critical=phi2_critical_deg*Modelica.Constants.pi/180.0;
  constant Real c2_small=1.e-5 
    "if cos(phi[2]) < c2_small, c2_small is used as guard against zero division";
  SI.Velocity vaux[3];
  Real s1;
  Real s2;
  Real s3;
  Real c1;
  Real c2;
  Real c2a;
  Real c3;
  //Boolean switch_state;
  Real S_phi[3, 3] "S_rel = S_phi(phi)*S_fix";
  Real S_fix[3, 3](start=identity(3)) "S_rel = S_phi(phi)*S_fix";
  SI.AngularVelocity w_fix[3] 
    "Relative angular velocity resolved in intermediate frame S_fix";
  
equation 
  
  /* Determine sines and cosines of the Cardan angles */
  s1 = Modelica.Math.sin(phi[1]);
  s2 = Modelica.Math.sin(phi[2]);
  s3 = Modelica.Math.sin(phi[3]);
  c1 = Modelica.Math.cos(phi[1]);
  c2a = Modelica.Math.cos(phi[2]);
  c3 = Modelica.Math.cos(phi[3]);
  
  /* Below, some expressions are divided by c2. By construction, it is not possible
     that c2=0, during continuous simulation. However, at initial time and when
     large numerical errors occur, c2=0 is possible, which would result in a division
     by zero. The following statement is a guard against this unlikely situation.
  */
  
  
    //c2 = if noEvent(c2a > c2_small or c2a < -c2_small) then c2a else if noEvent(
  //  c2a >= 0) then c2_small else -c2_small;
  
  c2 = c2a;
  
  /* Relative transformation matrix
       S_phi = [ c3, s3, 0; 
                -s3, c3, 0; 
                  0, 0, 1]*[c2, 0, -s2; 
                             0, 1, 0; 
                            s2, 0, c2]*[1, 0, 0; 
                                        0, c1, s1; 
                                        0, -s1, c1];
  */
  //switch_state = phi[2] >= phi2_critical or phi[2] <= -phi2_critical;
  S_fix = identity(3);
  /*when switch_state then
    S_fix = pre(S_rel);
    reinit(phi, zeros(3));
  end when;
  */
  S_phi = [c2*c3, c1*s3 + s1*s2*c3, s1*s3 - c1*s2*c3; -c2*s3, c1*c3 - s1*s2*s3, 
     s1*c3 + c1*s2*s3; s2, -s1*c2, c1*c2];
  S_rel = S_phi*S_fix;
  
  // Kinematic differential equations for translational motion
  der(r_rela) = v_rela;
  der(v_rela) = a_rela;
  
  // Kinematic differential equations for rotational motion    
  w_fix = S_fix*w_rela;
  der(phi) = {w_fix[1] + (s1*w_fix[2] - c1*w_fix[3])*s2/c2,c1*w_fix[2] + s1*
    w_fix[3],(-s1*w_fix[2] + c1*w_fix[3])/c2};
  der(w_rela) = z_rela;
  
  // Kinematic relationships
  frame_b.S = frame_a.S*transpose(S_rel);
  frame_b.r0 = frame_a.r0 + frame_a.S*r_rela;
  
  vaux = cross(frame_a.w, r_rela);
  frame_b.v = S_rel*(frame_a.v + v_rela + vaux);
  frame_b.w = S_rel*(frame_a.w + w_rela);
  
  frame_b.a = S_rel*(frame_a.a + cross(frame_a.z, r_rela) + cross(frame_a.w, 
    vaux + 2*v_rela) + a_rela);
  frame_b.z = S_rel*(frame_a.z + cross(frame_a.w, w_rela) + z_rela);
  
  // cut-forces and cut-torques are zero
  frame_a.f = zeros(3);
  frame_a.t = zeros(3);
  frame_b.f = zeros(3);
  frame_b.t = zeros(3);
end FreeMotion2;

VehicleDynamics.Utilities.Joints.RelDistance VehicleDynamics.Utilities.Joints.RelDistance

Distance between flange_a and flange_b is converted to translational cuts.

VehicleDynamics.Utilities.Joints.RelDistance

Information

 

Parameters

NameDefaultDescription
sEps1.E-6prevent zero-division if rel. distance s=0 [m]

Modelica definition

model RelDistance 
  "Distance between flange_a and flange_b is converted to translational cuts."
   
  
  extends ModelicaAdditions.MultiBody.Interfaces.LineForce;
  Real aux[3];
  SI.Position res1;
  SI.Position res2;
  
  Modelica.Mechanics.Translational.Interfaces.Flange_b bearing;
  Modelica.Mechanics.Translational.Interfaces.Flange_a axis;
equation 
  
  res1 = axis.s + s;
  res2 = der(axis.s) + sd;
  constrain(res1, res2);
  
  bearing.s = 0;
  
  axis.f = f;
  
  aux = na*s;
end RelDistance;

HTML-documentation generated by Dymola Fri Jul 11 13:27:36 2003 .