c4dynamics.eqm.derivs.eqm6q

c4dynamics.eqm.derivs.eqm6q(qb: quatbody, F: ndarray | list, M: ndarray | list) → ndarray

Translational and angular motion derivatives, quaternion attitude.

The same six degrees of freedom model as eqm6, where the attitude kinematics are given by the quaternion \(q = [q_w, q_x, q_y, q_z]\) rather than by Euler angles:

\[\dot{q} = {1 \over 2} \cdot q \otimes [0, p, q, r]\]

i.e.:

\[ \begin{align}\begin{aligned}\dot{q}_w = -{1 \over 2} (q_x p + q_y q + q_z r)\\\dot{q}_x = {1 \over 2} (q_w p + q_y r - q_z q)\\\dot{q}_y = {1 \over 2} (q_w q - q_x r + q_z p)\\\dot{q}_z = {1 \over 2} (q_w r + q_x q - q_y p)\end{aligned}\end{align} \]

Unlike the Euler angles derivatives, the quaternion derivatives are free of singularities (e.g. at \(\theta = \pm 90°\)).

Parameters:

• qb (quatbody) – C4dynamics’ quatbody object for which the equations of motion are calculated on.

• F (array_like) – Force vector \([F_x, F_y, F_z]\)

• M (array_like) – Moments vector \([M_x, M_y, M_z]\)

Returns:

out (numpy.array) – \([dx, dy, dz, dv_x, dv_y, dv_z, dq_w, dq_x, dq_y, dq_z, dp, dq, dr]\)

13 total derivatives; 6 of translational motion, 7 of rotational motion.

Examples

Euler integration on the equations of motion of a stick fixed at one edge:

(mass: 0.5 kg, moment of inertia about y: 0.4 kg*m^2, Length: 1m, initial pitch angle: 80° (converted to radians))

Import required packages:

>>> import c4dynamics as c4d
>>> import numpy as np

Settings and initial conditions

>>> dt = 0.5e-3
>>> t = np.arange(0, 10, dt)
>>> length =  1  # metter
>>> qb = c4d.quatbody(theta = 80 * c4d.d2r)
>>> qb.mass = 0.5 # kg
>>> qb.I = [0, 0.4, 0]

Main loop:

>>> for ti in t:
...    qb.store(ti)
...    tau_g = -qb.mass * c4d.g_ms2 * length / 2 * c4d.cos(qb.theta)
...    dx = c4d.eqm.eqm6q(qb, np.zeros(3), [0, tau_g, 0])
...    qb.X += dx * dt
>>> qb.plot('theta')