LIBRARY

`UniversalSpherical`

Joint assembly: universal joint at frame_a + spherical joint at frame_b, connected by a fixed-length rigid rod. The rod's body-fixed frame is exposed via frame_ia so a rod mass / shape can be attached (connect(joint_us.frame_ia, body.frame_a)). Recommended for closed kinematic loops since the rotational kinematics of the assembly are solved symbolically.

Singular when n1_a is parallel to the rod direction rRod_ia. Choose n1_a orthogonal to the initial rod direction whenever possible.

n1_a: axis 1 of the universal joint, resolved in frame_a. Axis 2 is perpendicular to n1_a and to the rod and is computed automatically.
rRod_ia: vector from frame_a origin to frame_b origin, resolved in frame_ia. Its norm sets the rod length.
kinematic_constraint: when true, the rod-position equation uses the rotation-matrix-aware form preferred by the symbolic loop solver.

This component extends from PartialTwoFrames This component extends from Renderable

Usage

MultibodyComponents.UniversalSpherical(render=true, color=[0, 0.1, 1, 0.9], specular_coefficient=1.5, n1_a=[0, 0, 1], rRod_ia=[1, 0, 0], rod_radius=0.05, sphere_diameter=0.1, sphere_color=[1, 0.2, 1, 0.9], rod_length=norm_(rRod_ia), eRod_ia=rRod_ia / rod_length, e2_ia=cross(n1_a, eRod_ia), e3_ia=cross(eRod_ia, e2_ia))

Parameters:

Name	Description	Units	Default value
`kinematic_constraint`		–	true
`constraint_residue_external`		–	false
`render`		–	true
`color`		–	[0, 0.1, 1, 0.9]
`specular_coefficient`		–	1.5
`n1_a`	Axis 1 of the universal joint, resolved in frame_a	–	[0, 0, 1]
`rRod_ia`	Vector from frame_a origin to frame_b origin, resolved in frame_ia	–	[1, 0, 0]
`rod_radius`	Rendering radius of the rod cylinder	–	0.05
`sphere_diameter`	Diameter of the sphere drawn at the spherical-joint end	–	0.1
`sphere_color`	RGBA color of the sphere drawn at the spherical-joint end	–	[1, 0.2, 1, 0.9]

Connectors

frame_a - Frame3D is the fundamental 3D connector used for 6DOF motion. Most components have one or several Frame

connectors that can be connected together (Frame3D)

frame_b - Frame3D is the fundamental 3D connector used for 6DOF motion. Most components have one or several Frame

connectors that can be connected together (Frame3D)

frame_ia - Frame3D is the fundamental 3D connector used for 6DOF motion. Most components have one or several Frame

connectors that can be connected together (Frame3D)

Variables

Name	Description	Units
`f_rod`	Constraint force along the rod (positive on frame_a, directed a → b)	–
`rRod_0`	Position vector frame_a → frame_b, resolved in world	m
`rRod_a`	Position vector frame_a → frame_b, resolved in frame_a	m
`eRod_a`	Unit vector along the rod, resolved in frame_a	–
`n2_a`	n1_a × eRod_a (axis 2 of the universal joint), resolved in frame_a	–
`length2_n2_a`	Squared length of n2_a	–
`length_n2_a`	Length of n2_a	–
`e2_a`	Unit vector along axis 2 of the universal joint, resolved in frame_a	–
`e3_a`	Unit vector perpendicular to eRod_a and e2_a, resolved in frame_a	–
`der_rRod_a_L`	der(rRod_a) / rod_length	–
`w_rel_ia1`	Angular velocity of intermediate frame ia1 wrt frame_a, in ia1 basis	–
`f_b_a1`	frame_b.f resolved in frame_a, without the f_rod component	–
`f_b_a`	frame_b.f resolved in frame_a	–
`f_ia_a`	frame_ia.f resolved in frame_a	–
`t_ia_a`	frame_ia.tau resolved in frame_a	–
`constraint_residue`	Constraint residue: length constraint by default, or rod force when external	–

Behavior

Source

dyad

"""
Joint assembly: universal joint at `frame_a` + spherical joint at `frame_b`,
connected by a fixed-length rigid rod. The rod's body-fixed frame is exposed
via `frame_ia` so a rod mass / shape can be attached
(`connect(joint_us.frame_ia, body.frame_a)`). Recommended for closed
kinematic loops since the rotational kinematics of the assembly are solved
symbolically.

Singular when `n1_a` is parallel to the rod direction `rRod_ia`. Choose
`n1_a` orthogonal to the initial rod direction whenever possible.

- `n1_a`: axis 1 of the universal joint, resolved in `frame_a`. Axis 2 is
  perpendicular to `n1_a` and to the rod and is computed automatically.
- `rRod_ia`: vector from `frame_a` origin to `frame_b` origin, resolved in
  `frame_ia`. Its norm sets the rod length.
- `kinematic_constraint`: when true, the rod-position equation uses the
  rotation-matrix-aware form preferred by the symbolic loop solver.
"""
component UniversalSpherical
  extends PartialTwoFrames()
  extends Renderable(color = [0, 0.1, 1, 0.9])
  frame_ia = Frame3D() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 400, "y1": 450, "x2": 500, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  rod_shape = CylinderShape(render = render, color = color, r = frame_a.r_0, R = transpose(frame_a.R), length_direction = eRod_a, length = rod_length, width = 2 * rod_radius, height = 2 * rod_radius) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 570, "y1": 690, "x2": 670, "y2": 790, "rot": 0}
      },
      "tags": []
    }
  }
  sphere_shape = SphereShape(render = render, color = sphere_color, r = frame_b.r_0, R = transpose(frame_b.R), length = sphere_diameter, width = sphere_diameter, height = sphere_diameter) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 440, "y1": 640, "x2": 540, "y2": 740, "rot": 0}
      },
      "tags": []
    }
  }
  structural parameter kinematic_constraint::Boolean = true
  structural parameter constraint_residue_external::Boolean = false
  "Axis 1 of the universal joint, resolved in frame_a"
  parameter n1_a::Real[3] = [0, 0, 1]
  "Vector from frame_a origin to frame_b origin, resolved in frame_ia"
  parameter rRod_ia::Real[3] = [1, 0, 0]
  "Rendering radius of the rod cylinder"
  parameter rod_radius::Real = 0.05
  "Diameter of the sphere drawn at the spherical-joint end"
  parameter sphere_diameter::Real = 0.1
  "RGBA color of the sphere drawn at the spherical-joint end"
  parameter sphere_color::Real[4] = [1, 0.2, 1, 0.9]
  final parameter rod_length::Real = norm_(rRod_ia)
  final parameter eRod_ia::Real[3] = rRod_ia / rod_length
  final parameter e2_ia::Real[3] = cross(n1_a, eRod_ia)
  final parameter e3_ia::Real[3] = cross(eRod_ia, e2_ia)
  "Constraint force along the rod (positive on frame_a, directed a → b)"
  variable f_rod::Real
  "Position vector frame_a → frame_b, resolved in world"
  variable rRod_0::Position[3]
  "Position vector frame_a → frame_b, resolved in frame_a"
  variable rRod_a::Position[3]
  "Unit vector along the rod, resolved in frame_a"
  variable eRod_a::Real[3]
  "n1_a × eRod_a (axis 2 of the universal joint), resolved in frame_a"
  variable n2_a::Real[3]
  "Squared length of n2_a"
  variable length2_n2_a::Real
  "Length of n2_a"
  variable length_n2_a::Real
  "Unit vector along axis 2 of the universal joint, resolved in frame_a"
  variable e2_a::Real[3]
  "Unit vector perpendicular to eRod_a and e2_a, resolved in frame_a"
  variable e3_a::Real[3]
  "der(rRod_a) / rod_length"
  variable der_rRod_a_L::Real[3]
  "Angular velocity of intermediate frame ia1 wrt frame_a, in ia1 basis"
  variable w_rel_ia1::Real[3]
  "frame_b.f resolved in frame_a, without the f_rod component"
  variable f_b_a1::Real[3]
  "frame_b.f resolved in frame_a"
  variable f_b_a::Real[3]
  "frame_ia.f resolved in frame_a"
  variable f_ia_a::Real[3]
  "frame_ia.tau resolved in frame_a"
  variable t_ia_a::Real[3]
  "Constraint residue: length constraint by default, or rod force when external"
  variable constraint_residue::Real
relations
  # Guesses keep initialization away from the degenerate zero-length rod
  # configuration (where eRod_a / e2_a become 0/0); mirror Multibody.jl.
  guess rRod_0 = rRod_ia
  guess rRod_a = rRod_ia
  guess length2_n2_a = 1
  guess constraint_residue = 0
  if kinematic_constraint
    rRod_0 = transpose(frame_b.R) * (frame_b.R * frame_b.r_0) - transpose(frame_a.R) * (frame_a.R * frame_a.r_0)
  else
    rRod_0 = frame_b.r_0 - frame_a.r_0
  end
  rRod_a = resolve2(frame_a.R, rRod_0)
  eRod_a = rRod_a / rod_length
  n2_a = cross(n1_a, eRod_a)
  length2_n2_a = dot(n2_a, n2_a)
  assert(length2_n2_a > 1e-10, "A UniversalSpherical joint is in the singular configuration of the universal joint.")
  length_n2_a = sqrt(length2_n2_a)
  e2_a = n2_a / length_n2_a
  e3_a = cross(eRod_a, e2_a)
  # constraint_residue is pinned to 0. When constraint_residue_external is false
  # the length constraint below provides the second equation. When true, the
  # parent assembly (e.g. JointUSR) supplies `constraint_residue = f_rod - <rod force>`
  # instead, so the kinematic loop is solved analytically.
  constraint_residue = 0
  if !constraint_residue_external
    constraint_residue = dot(rRod_0, rRod_0) - rod_length ^ 2
  end
  der_rRod_a_L = (resolve2(frame_a.R, der(rRod_0)) - cross(angular_velocity2(ori(frame_a)), rRod_a)) / rod_length
  w_rel_ia1 = [dot(e3_a, cross(n1_a, der_rRod_a_L)) / length_n2_a, -dot(e3_a, der_rRod_a_L), dot(e2_a, der_rRod_a_L)]
  frame_ia.r_0 = frame_a.r_0
  RotationMatrix(frame_ia.R) = absolute_rotation(frame_a, R_rel_ia_from(eRod_a, e2_a, e3_a, eRod_ia, e2_ia, e3_ia, w_rel_ia1))
  f_ia_a = resolve1(R_rel_ia_from(eRod_a, e2_a, e3_a, eRod_ia, e2_ia, e3_ia, w_rel_ia1), frame_ia.f)
  t_ia_a = resolve1(R_rel_ia_from(eRod_a, e2_a, e3_a, eRod_ia, e2_ia, e3_ia, w_rel_ia1), frame_ia.tau)
  f_b_a1 = -e2_a * (dot(n1_a, t_ia_a) / (rod_length * dot(n1_a, e3_a))) + e3_a * (dot(e2_a, t_ia_a) / rod_length)
  f_b_a = -f_rod * eRod_a + f_b_a1
  frame_b.f = resolve_relative(f_b_a, frame_a.R, frame_b.R)
  frame_b.tau = [0, 0, 0]
  [0, 0, 0] = frame_a.f + f_b_a + f_ia_a
  [0, 0, 0] = frame_a.tau + t_ia_a + cross(rRod_a, f_b_a)
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MultibodyComponents/UniversalSpherical.svg"},
    "labels": [
      {
        "label": "$(instance)",
        "x": 500,
        "y": 200,
        "rot": 0,
        "attrs": {"font-size": "160"}
      }
    ]
  }
}
end

Flattened Source

dyad

"""
Joint assembly: universal joint at `frame_a` + spherical joint at `frame_b`,
connected by a fixed-length rigid rod. The rod's body-fixed frame is exposed
via `frame_ia` so a rod mass / shape can be attached
(`connect(joint_us.frame_ia, body.frame_a)`). Recommended for closed
kinematic loops since the rotational kinematics of the assembly are solved
symbolically.

Singular when `n1_a` is parallel to the rod direction `rRod_ia`. Choose
`n1_a` orthogonal to the initial rod direction whenever possible.

- `n1_a`: axis 1 of the universal joint, resolved in `frame_a`. Axis 2 is
  perpendicular to `n1_a` and to the rod and is computed automatically.
- `rRod_ia`: vector from `frame_a` origin to `frame_b` origin, resolved in
  `frame_ia`. Its norm sets the rod length.
- `kinematic_constraint`: when true, the rod-position equation uses the
  rotation-matrix-aware form preferred by the symbolic loop solver.
"""
component UniversalSpherical
  frame_a = Frame3D() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -50, "y1": 450, "x2": 50, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  frame_b = Frame3D() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 950, "y1": 450, "x2": 1050, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  parameter render::Boolean = true
  parameter color::Real[4] = [0.5, 0.5, 0.5, 1.0]
  parameter specular_coefficient::Real = 1.5
  frame_ia = Frame3D() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 400, "y1": 450, "x2": 500, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  rod_shape = CylinderShape(render = render, color = color, r = frame_a.r_0, R = transpose(frame_a.R), length_direction = eRod_a, length = rod_length, width = 2 * rod_radius, height = 2 * rod_radius) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 570, "y1": 690, "x2": 670, "y2": 790, "rot": 0}
      },
      "tags": []
    }
  }
  sphere_shape = SphereShape(render = render, color = sphere_color, r = frame_b.r_0, R = transpose(frame_b.R), length = sphere_diameter, width = sphere_diameter, height = sphere_diameter) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 440, "y1": 640, "x2": 540, "y2": 740, "rot": 0}
      },
      "tags": []
    }
  }
  structural parameter kinematic_constraint::Boolean = true
  structural parameter constraint_residue_external::Boolean = false
  "Axis 1 of the universal joint, resolved in frame_a"
  parameter n1_a::Real[3] = [0, 0, 1]
  "Vector from frame_a origin to frame_b origin, resolved in frame_ia"
  parameter rRod_ia::Real[3] = [1, 0, 0]
  "Rendering radius of the rod cylinder"
  parameter rod_radius::Real = 0.05
  "Diameter of the sphere drawn at the spherical-joint end"
  parameter sphere_diameter::Real = 0.1
  "RGBA color of the sphere drawn at the spherical-joint end"
  parameter sphere_color::Real[4] = [1, 0.2, 1, 0.9]
  final parameter rod_length::Real = norm_(rRod_ia)
  final parameter eRod_ia::Real[3] = rRod_ia / rod_length
  final parameter e2_ia::Real[3] = cross(n1_a, eRod_ia)
  final parameter e3_ia::Real[3] = cross(eRod_ia, e2_ia)
  "Constraint force along the rod (positive on frame_a, directed a → b)"
  variable f_rod::Real
  "Position vector frame_a → frame_b, resolved in world"
  variable rRod_0::Position[3]
  "Position vector frame_a → frame_b, resolved in frame_a"
  variable rRod_a::Position[3]
  "Unit vector along the rod, resolved in frame_a"
  variable eRod_a::Real[3]
  "n1_a × eRod_a (axis 2 of the universal joint), resolved in frame_a"
  variable n2_a::Real[3]
  "Squared length of n2_a"
  variable length2_n2_a::Real
  "Length of n2_a"
  variable length_n2_a::Real
  "Unit vector along axis 2 of the universal joint, resolved in frame_a"
  variable e2_a::Real[3]
  "Unit vector perpendicular to eRod_a and e2_a, resolved in frame_a"
  variable e3_a::Real[3]
  "der(rRod_a) / rod_length"
  variable der_rRod_a_L::Real[3]
  "Angular velocity of intermediate frame ia1 wrt frame_a, in ia1 basis"
  variable w_rel_ia1::Real[3]
  "frame_b.f resolved in frame_a, without the f_rod component"
  variable f_b_a1::Real[3]
  "frame_b.f resolved in frame_a"
  variable f_b_a::Real[3]
  "frame_ia.f resolved in frame_a"
  variable f_ia_a::Real[3]
  "frame_ia.tau resolved in frame_a"
  variable t_ia_a::Real[3]
  "Constraint residue: length constraint by default, or rod force when external"
  variable constraint_residue::Real
relations
  # Guesses keep initialization away from the degenerate zero-length rod
  # configuration (where eRod_a / e2_a become 0/0); mirror Multibody.jl.
  guess rRod_0 = rRod_ia
  guess rRod_a = rRod_ia
  guess length2_n2_a = 1
  guess constraint_residue = 0
  if kinematic_constraint
    rRod_0 = transpose(frame_b.R) * (frame_b.R * frame_b.r_0) - transpose(frame_a.R) * (frame_a.R * frame_a.r_0)
  else
    rRod_0 = frame_b.r_0 - frame_a.r_0
  end
  rRod_a = resolve2(frame_a.R, rRod_0)
  eRod_a = rRod_a / rod_length
  n2_a = cross(n1_a, eRod_a)
  length2_n2_a = dot(n2_a, n2_a)
  assert(length2_n2_a > 1e-10, "A UniversalSpherical joint is in the singular configuration of the universal joint.")
  length_n2_a = sqrt(length2_n2_a)
  e2_a = n2_a / length_n2_a
  e3_a = cross(eRod_a, e2_a)
  # constraint_residue is pinned to 0. When constraint_residue_external is false
  # the length constraint below provides the second equation. When true, the
  # parent assembly (e.g. JointUSR) supplies `constraint_residue = f_rod - <rod force>`
  # instead, so the kinematic loop is solved analytically.
  constraint_residue = 0
  if !constraint_residue_external
    constraint_residue = dot(rRod_0, rRod_0) - rod_length ^ 2
  end
  der_rRod_a_L = (resolve2(frame_a.R, der(rRod_0)) - cross(angular_velocity2(ori(frame_a)), rRod_a)) / rod_length
  w_rel_ia1 = [dot(e3_a, cross(n1_a, der_rRod_a_L)) / length_n2_a, -dot(e3_a, der_rRod_a_L), dot(e2_a, der_rRod_a_L)]
  frame_ia.r_0 = frame_a.r_0
  RotationMatrix(frame_ia.R) = absolute_rotation(frame_a, R_rel_ia_from(eRod_a, e2_a, e3_a, eRod_ia, e2_ia, e3_ia, w_rel_ia1))
  f_ia_a = resolve1(R_rel_ia_from(eRod_a, e2_a, e3_a, eRod_ia, e2_ia, e3_ia, w_rel_ia1), frame_ia.f)
  t_ia_a = resolve1(R_rel_ia_from(eRod_a, e2_a, e3_a, eRod_ia, e2_ia, e3_ia, w_rel_ia1), frame_ia.tau)
  f_b_a1 = -e2_a * (dot(n1_a, t_ia_a) / (rod_length * dot(n1_a, e3_a))) + e3_a * (dot(e2_a, t_ia_a) / rod_length)
  f_b_a = -f_rod * eRod_a + f_b_a1
  frame_b.f = resolve_relative(f_b_a, frame_a.R, frame_b.R)
  frame_b.tau = [0, 0, 0]
  [0, 0, 0] = frame_a.f + f_b_a + f_ia_a
  [0, 0, 0] = frame_a.tau + t_ia_a + cross(rRod_a, f_b_a)
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MultibodyComponents/UniversalSpherical.svg"},
    "labels": [
      {
        "label": "$(instance)",
        "x": 500,
        "y": 200,
        "rot": 0,
        "attrs": {"font-size": "160"}
      }
    ]
  }
}
end

Test Cases

No test cases defined.

Examples
Experiments
Analyses

Scalar Types

Partial Types

Enumerated Types

Function Types

Partial Types

Enumerated Types

Partial Types

Enumerated Types

Partial Types

Scalar Types

Partial Types

Partial Types

Types

Partial Types

Enumerated Types

Struct Types

Connectors

Components

Analyses

Examples

Tests

Scalar Types

Partial Types

Enumerated Types

UniversalSpherical ​

Usage ​

Parameters: ​

Connectors ​

Variables ​

Behavior ​

Source ​

Test Cases ​

Related ​

`UniversalSpherical`

Usage

Parameters:

Connectors

Variables

Behavior

Source

Test Cases

Related