LIBRARY

Body

Rigid body with mass, inertia tensor, and one frame connector.

Extends PartialOrientation to get orientation state variables (Euler angles or quaternions) via the quat structural parameter.

This component extends from PartialOrientation This component extends from Renderable

Usage

MultibodyComponents.Body(k=0.1, render=true, color=world_default_body_color(), specular_coefficient=1.5, m=1, r_cm=[0.0, 0, 0], I_11=0.001, I_22=0.001, I_33=0.001, I_21=0, I_31=0, I_32=0, radius=world_default_body_diameter() / 2, cylinder_radius=world_default_body_diameter() / 6, length_fraction=1, I=[[I_11, I_21, I_31], [I_21, I_22, I_32], [I_31, I_32, I_33]], r_cm_length=norm_(r_cm), r_cm_dir=r_cm / max(r_cm_length, 1e-10))

Parameters:

Name	Description	Units	Default value
orientation_state		–	OrientationState.None()
sequence		–	[1, 2, 3]
statePriority		–	10
linearStatePriority		–	1
point_gravity	Use a point-gravity field pointing towards the world origin instead of the uniform field g*n. Structural: only the selected field model is emitted into the equations, keeping force balances linear under the default uniform field.	–	false
k		–	0.1
render		–	true
color		–	world_defau...ody_color()
specular_coefficient		–	1.5
m	mass	kg	1
r_cm	Vector from frame_a to center of mass, resolved in frame_a	m	[0.0, 0, 0]
I_11	Element (1,1) of inertia tensor	kg.m2	0.001
I_22	Element (2,2) of inertia tensor	kg.m2	0.001
I_33	Element (3,3) of inertia tensor	kg.m2	0.001
I_21	Element (2,1) of inertia tensor	kg.m2	0
I_31	Element (3,1) of inertia tensor	kg.m2	0
I_32	Element (3,2) of inertia tensor	kg.m2	0
radius	Radius of the body in animations	–	world_defau...meter() / 2
cylinder_radius	Radius of the cylinder from frame to COM in animations	–	world_defau...meter() / 6
length_fraction	Fraction of the length of the body that is the cylinder from frame to COM in animations	–	1

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)

Variables

Name	Description	Units
w_a	Absolute angular velocity of frame_a resolved in frame_a	rad/s
z_a	Absolute angular acceleration of frame_a resolved in frame_a	rad/s2
Q	Unit quaternion with [w,i,j,k]	–
Q_hat	Non-unit quaternion with [w,i,j,k]	–
Q_hat_d		–
n_q		–
c_q		–
phi	Euler angles	rad
phid		rad/s
phidd		rad/s2
r_0	Position vector from origin of world frame to origin of frame_a	m
v_0	Absolute velocity of frame_a, resolved in world frame (= D(r_0))	m/s
a_0	Absolute acceleration of frame_a resolved in world frame (= D(v_0))	m/s2
g_0	gravity acceleration	m/s2
v_cm_0	Absolute velocity of the center of mass, resolved in world frame	m/s
KE	Kinetic energy of the body, translational plus rotational	–
PE	Potential energy of the body in a uniform gravity field, zero when the center of mass is at the world origin. With a point-gravity field, this expression is only a local approximation.	–

Behavior

Source

"""
Rigid body with mass, inertia tensor, and one frame connector.

Extends PartialOrientation to get orientation state variables
(Euler angles or quaternions) via the `quat` structural parameter.
"""
component Body
  extends PartialOrientation
  extends Renderable(color = world_default_body_color())
  # Visualization shapes
  sphere_shape = SphereShape(render = render, color = color, r = frame_a.r_0, R = transpose(frame_a.R), r_shape = r_cm, length = 2 * radius, width = 2 * radius, height = 2 * radius)
  cylinder_shape = CylinderShape(render = render, color = color, r = frame_a.r_0, R = transpose(frame_a.R), r_shape = r_cm, length_direction = -r_cm_dir, length = r_cm_length * length_fraction, width = 2 * cylinder_radius, height = 2 * cylinder_radius)
  structural parameter linearStatePriority::Integer = 1
  "Use a point-gravity field pointing towards the world origin instead of the uniform field `g*n`. Structural: only the selected field model is emitted into the equations, keeping force balances linear under the default uniform field."
  structural parameter point_gravity::Boolean = false
  "mass"
  parameter m::Mass = 1
  "Vector from frame_a to center of mass, resolved in frame_a"
  parameter r_cm::Length[3] = [0.0, 0, 0]
  "Element (1,1) of inertia tensor"
  parameter I_11::Inertia = 0.001
  "Element (2,2) of inertia tensor"
  parameter I_22::Inertia = 0.001
  "Element (3,3) of inertia tensor"
  parameter I_33::Inertia = 0.001
  "Element (2,1) of inertia tensor"
  parameter I_21::Inertia = 0
  "Element (3,1) of inertia tensor"
  parameter I_31::Inertia = 0
  "Element (3,2) of inertia tensor"
  parameter I_32::Inertia = 0
  "Radius of the body in animations"
  parameter radius::Real = world_default_body_diameter() / 2
  "Radius of the cylinder from frame to COM in animations"
  parameter cylinder_radius::Real = world_default_body_diameter() / 6
  "Fraction of the length of the body that is the cylinder from frame to COM in animations"
  parameter length_fraction::Real = 1
  "Position vector from origin of world frame to origin of frame_a"
  variable r_0::Position(statePriority = linearStatePriority, initial = case(orientation_state, OrientationState.None) ? missing : [0.0, 0, 0])[3]
  "Absolute velocity of frame_a, resolved in world frame (= D(r_0))"
  variable v_0::Velocity(statePriority = linearStatePriority, initial = case(orientation_state, OrientationState.None) ? missing : [0.0, 0, 0])[3]
  "Absolute acceleration of frame_a resolved in world frame (= D(v_0))"
  variable a_0::Acceleration[3]
  "gravity acceleration"
  variable g_0::Acceleration[3]
  "Absolute velocity of the center of mass, resolved in world frame"
  variable v_cm_0::Velocity[3]
  "Kinetic energy of the body, translational plus rotational"
  variable KE::Real
  "Potential energy of the body in a uniform gravity field, zero when the center of mass is at the world origin. With a point-gravity field, this expression is only a local approximation."
  variable PE::Real
  final parameter I::Inertia[3, 3] = [[I_11, I_21, I_31], [I_21, I_22, I_32], [I_31, I_32, I_33]]
  final parameter r_cm_length::Real = norm_(r_cm)
  final parameter r_cm_dir::Real[3] = r_cm / max(r_cm_length, 1e-10)
relations
  # Position
  r_0 = frame_a.r_0
  v_0 = der(r_0)
  a_0 = der(v_0)
  # Gravity (structural choice of field model, resolved at compile time)
  g_0 = gravity_acceleration(frame_a.r_0 + resolve1(frame_a.R, r_cm), point_gravity)
  # Energy
  v_cm_0 = v_0 + resolve1(frame_a.R, cross(w_a, r_cm))
  KE = 0.5 * m * dot(v_cm_0, v_cm_0) + 0.5 * dot(w_a, I * w_a)
  PE = -m * dot(g_0, frame_a.r_0 + resolve1(frame_a.R, r_cm))
  # Newton's second law (force balance in body frame)
  frame_a.f = m * (resolve2(frame_a.R, a_0 - g_0) + cross(z_a, r_cm) + cross(w_a, cross(w_a, r_cm)))
  # Euler's equation (torque balance in body frame)
  frame_a.tau = I * z_a + cross(w_a, I * w_a) + cross(r_cm, frame_a.f)
  # Bind frame_a.R / w_a to the orientation state inherited from PartialOrientation
  switch orientation_state
    case Euler
      RotationMatrix(frame_a.R) = Rrel
    case Quaternion
      frame_a.R = from_Q(Q)
    case None
      w_a = angular_velocity2(ori(frame_a))
  end
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MultibodyComponents/Body.svg"},
    "labels": [
      {
        "label": "$(instance)",
        "x": 500,
        "y": 150,
        "rot": 0,
        "attrs": {"font-size": "160"}
      }
    ]
  }
}
end

Flattened Source

"""
Rigid body with mass, inertia tensor, and one frame connector.

Extends PartialOrientation to get orientation state variables
(Euler angles or quaternions) via the `quat` structural parameter.
"""
component Body
  frame_a = Frame3D() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 0, "y1": 450, "x2": 100, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  structural parameter orientation_state::OrientationState = OrientationState.None()
  structural parameter sequence::Integer[3] = [1, 2, 3]
  structural parameter statePriority::Integer = 10
  "Absolute angular velocity of frame_a resolved in frame_a"
  variable w_a::AngularVelocity(statePriority = case(orientation_state, OrientationState.Quaternion) ? 10 : 0, initial = case(orientation_state, OrientationState.Quaternion) ? [0.0, 0, 0] : missing)[3]
  "Absolute angular acceleration of frame_a resolved in frame_a"
  variable z_a::AngularAcceleration[3]
  "Unit quaternion with [w,i,j,k]"
  variable Q::Real[4] if case(orientation_state, OrientationState.Quaternion)
  "Non-unit quaternion with [w,i,j,k]"
  variable Q_hat::Real(statePriority = statePriority, initial = [1.0, 0, 0, 0])[4] if case(orientation_state, OrientationState.Quaternion)
  variable Q_hat_d::Real[4] if case(orientation_state, OrientationState.Quaternion)
  variable n_q::Real if case(orientation_state, OrientationState.Quaternion)
  variable c_q::Real if case(orientation_state, OrientationState.Quaternion)
  parameter k::Real = 0.1 if case(orientation_state, OrientationState.Quaternion)
  "Euler angles"
  variable phi::Angle(statePriority = statePriority, initial = [0, 0, 0])[3] if case(orientation_state, OrientationState.Euler)
  variable phid::AngularVelocity(statePriority = statePriority, initial = [0, 0, 0])[3] if case(orientation_state, OrientationState.Euler)
  variable phidd::AngularAcceleration[3] if case(orientation_state, OrientationState.Euler)
  structural variable Rrel::Native = case(orientation_state, OrientationState.Euler) ? axes_rotations(sequence, phi, phid) : (case(orientation_state, OrientationState.Quaternion) ? from_Q(Q) : nullrotation())
  parameter render::Boolean = true
  parameter color::Real[4] = [0.5, 0.5, 0.5, 1.0]
  parameter specular_coefficient::Real = 1.5
  # Visualization shapes
  sphere_shape = SphereShape(render = render, color = color, r = frame_a.r_0, R = transpose(frame_a.R), r_shape = r_cm, length = 2 * radius, width = 2 * radius, height = 2 * radius)
  cylinder_shape = CylinderShape(render = render, color = color, r = frame_a.r_0, R = transpose(frame_a.R), r_shape = r_cm, length_direction = -r_cm_dir, length = r_cm_length * length_fraction, width = 2 * cylinder_radius, height = 2 * cylinder_radius)
  structural parameter linearStatePriority::Integer = 1
  "Use a point-gravity field pointing towards the world origin instead of the uniform field `g*n`. Structural: only the selected field model is emitted into the equations, keeping force balances linear under the default uniform field."
  structural parameter point_gravity::Boolean = false
  "mass"
  parameter m::Mass = 1
  "Vector from frame_a to center of mass, resolved in frame_a"
  parameter r_cm::Length[3] = [0.0, 0, 0]
  "Element (1,1) of inertia tensor"
  parameter I_11::Inertia = 0.001
  "Element (2,2) of inertia tensor"
  parameter I_22::Inertia = 0.001
  "Element (3,3) of inertia tensor"
  parameter I_33::Inertia = 0.001
  "Element (2,1) of inertia tensor"
  parameter I_21::Inertia = 0
  "Element (3,1) of inertia tensor"
  parameter I_31::Inertia = 0
  "Element (3,2) of inertia tensor"
  parameter I_32::Inertia = 0
  "Radius of the body in animations"
  parameter radius::Real = world_default_body_diameter() / 2
  "Radius of the cylinder from frame to COM in animations"
  parameter cylinder_radius::Real = world_default_body_diameter() / 6
  "Fraction of the length of the body that is the cylinder from frame to COM in animations"
  parameter length_fraction::Real = 1
  "Position vector from origin of world frame to origin of frame_a"
  variable r_0::Position(statePriority = linearStatePriority, initial = case(orientation_state, OrientationState.None) ? missing : [0.0, 0, 0])[3]
  "Absolute velocity of frame_a, resolved in world frame (= D(r_0))"
  variable v_0::Velocity(statePriority = linearStatePriority, initial = case(orientation_state, OrientationState.None) ? missing : [0.0, 0, 0])[3]
  "Absolute acceleration of frame_a resolved in world frame (= D(v_0))"
  variable a_0::Acceleration[3]
  "gravity acceleration"
  variable g_0::Acceleration[3]
  "Absolute velocity of the center of mass, resolved in world frame"
  variable v_cm_0::Velocity[3]
  "Kinetic energy of the body, translational plus rotational"
  variable KE::Real
  "Potential energy of the body in a uniform gravity field, zero when the center of mass is at the world origin. With a point-gravity field, this expression is only a local approximation."
  variable PE::Real
  final parameter I::Inertia[3, 3] = [[I_11, I_21, I_31], [I_21, I_22, I_32], [I_31, I_32, I_33]]
  final parameter r_cm_length::Real = norm_(r_cm)
  final parameter r_cm_dir::Real[3] = r_cm / max(r_cm_length, 1e-10)
relations
  z_a = der(w_a)
  switch orientation_state
    case Quaternion
      # guess Q_hat = Q # TODO: Ideally it should be possible to provide this guess, but due to dyad codegen to initial equations the guess propagation is not working correctly and ODEProblem gets stuck. Once the initial condition for Q codegens to __initial_conditions, we can start providing this guess again.
      n_q = dot(Q_hat, Q_hat)
      c_q = k * (1 - n_q)
      der(Q_hat) = Q_hat_d
      Q_hat_d = omega_matrix(w_a) * Q_hat / 2 + c_q * Q_hat
      Q = Q_hat / sqrt(n_q)
    case Euler
      phid = der(phi)
      phidd = der(phid)
      w_a = angular_velocity2(Rrel)
    case None
  end
  # Position
  r_0 = frame_a.r_0
  v_0 = der(r_0)
  a_0 = der(v_0)
  # Gravity (structural choice of field model, resolved at compile time)
  g_0 = gravity_acceleration(frame_a.r_0 + resolve1(frame_a.R, r_cm), point_gravity)
  # Energy
  v_cm_0 = v_0 + resolve1(frame_a.R, cross(w_a, r_cm))
  KE = 0.5 * m * dot(v_cm_0, v_cm_0) + 0.5 * dot(w_a, I * w_a)
  PE = -m * dot(g_0, frame_a.r_0 + resolve1(frame_a.R, r_cm))
  # Newton's second law (force balance in body frame)
  frame_a.f = m * (resolve2(frame_a.R, a_0 - g_0) + cross(z_a, r_cm) + cross(w_a, cross(w_a, r_cm)))
  # Euler's equation (torque balance in body frame)
  frame_a.tau = I * z_a + cross(w_a, I * w_a) + cross(r_cm, frame_a.f)
  # Bind frame_a.R / w_a to the orientation state inherited from PartialOrientation
  switch orientation_state
    case Euler
      RotationMatrix(frame_a.R) = Rrel
    case Quaternion
      frame_a.R = from_Q(Q)
    case None
      w_a = angular_velocity2(ori(frame_a))
  end
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MultibodyComponents/Body.svg"},
    "labels": [
      {
        "label": "$(instance)",
        "x": 500,
        "y": 150,
        "rot": 0,
        "attrs": {"font-size": "160"}
      }
    ]
  }
}
end

Test Cases

No test cases defined.

Related

• Examples

• Experiments

• Analyses

• Tests

  • CentrifugalTest

  • CutForceTest

  • CutTorqueTest

  • OneDOFRollingWheelJointTest

  • OneDOFSlippingWheelSegwayTest

  • OneDOFSlippingWheelSpinningTest

  • PlanarToMultiBodyEmbeddedTest

  • PlanarToMultiBodyKinematicTest

  • PlanarToMultiBodyPendulumTest

  • PrescribedAccelerationTest

  • PrescribedPoseTest

  • PrescribedPositionTest

  • PrescribedVelocityTest

  • RelativeForceTest

  • SensorsTest

  • TestCableEndJoints

  • TestCableNoEndJoints

  • TestPlanarCableEndJoints

  • TestPlanarCableNoEndJoints

  • WorldForceTest