archimedes.spatial.RigidBody¶

class archimedes.spatial.RigidBody(rpy_attitude: bool = False, baumgarte: float = 1.0)¶

6-dof rigid body dynamics model

This class implements 6-dof rigid body dynamics based on reference equations from Lewis, Johnson, and Stevens, “Aircraft Control and Simulation” [1].

This implementation is general and does not make any assumptions about the forces, moments, or mass properties. These must be provided as inputs to the dynamics function.

The model assumes a non-inertial body-fixed reference frame B and a Newtonian inertial reference frame N. The body frame is assumed to be located at the vehicle’s center of mass.

With these conventions, the state vector is defined as

x = [p_N, q, v_B, w_B]

where

  • p_N = position of the center of mass in the Newtonian frame N

  • q = attitude (orientation) of the vehicle as a unit quaternion

  • v_B = velocity of the center of mass in body frame B

  • w_B = angular velocity in body frame (ω_B)

The equations of motion are given by

\[\begin{split}\dot{p}_N &= R_{BN}^T(\mathbf{q}) v_B \\ \dot{\mathbf{q}} &= \frac{1}{2} \mathbf{\omega}_B \otimes \mathbf{q} \\ \dot{v}_B &= \frac{1}{m}(\mathbf{F}_B - \dot{m} v_B) - \mathbf{\omega}_B \times v_B \\ \dot{\mathbf{\omega}}_B &= J_B^{-1}(\mathbf{M}_B - \dot{J}_B \mathbf{\omega}_B - \mathbf{\omega}_B \times (J_B \mathbf{\omega}_B))\end{split}\]

where

  • R_{BN}(q) = direction cosine matrix (DCM)

  • m = mass of the vehicle

  • J_B = inertia matrix of the vehicle in body axes

  • F_B = net forces acting on the vehicle in body frame B

  • M_B = net moments acting on the vehicle in body frame B

The inputs to the dynamics function are a RigidBody.Input struct containing the forces, moments, mass, and inertia properties. By default the time derivatives of the mass and inertia are zero unless specified in the input struct.

Parameters:

  • rpy_attitude (bool, optional) – If True, use roll-pitch-yaw angles for attitude representation instead of quaternions. Default is False. Note that using roll-pitch-yaw angles introduces a singularity (gimbal lock) and are not recommended for general use.

  • baumgarte (float, optional) – Baumgarte stabilization factor for quaternion kinematics. Default is 1.0. This adds a correction term to the quaternion kinematics to help maintain the unit norm constraint.

Examples

>>> import archimedes as arc
>>> from archimedes.spatial import RigidBody, Rotation
>>> import numpy as np
>>> rigid_body = RigidBody()
>>> t = 0
>>> v_B = np.array([1, 0, 0])  # Constant velocity in x-direction
>>> att = Rotation.from_quat([1, 0, 0, 0])  # No rotation
>>> x = rigid_body.State(
...     p_N=np.zeros(3),
...     att=att,
...     v_B=v_B,
...     w_B=np.zeros(3),
... )
>>> u = rigid_body.Input(
...     F_B=np.array([0, 0, -9.81]),  # Gravity
...     M_B=np.zeros(3),
...     m=2.0,
...     J_B=np.diag([1.0, 1.0, 1.0]),
... )
>>> rigid_body.dynamics(t, x, u)
State(p_N=array([1., 0., 0.]),
  att=Rotation(quat=array([0., 0., 0., 0.]), scalar_first=True),
  v_B=array([ 0.   ,  0.   , -4.905]),
  w_B=array([0., 0., 0.]))

References

__init__(rpy_attitude: bool = False, baumgarte: float = 1.0) → None¶

Methods

__init__([rpy_attitude, baumgarte])

calc_dynamics(x, u)

Calculate dynamics (velocity and angular velocity derivatives)

calc_kinematics(x)

Calculate kinematics (position and attitude derivatives)

dynamics(t, x, u)

Calculate 6-dof dynamics

replace(**updates)

Returns a new object replacing the specified fields with new values.

Attributes

baumgarte

rpy_attitude