Quatbody

Quatbody#

class c4dynamics.states.lib.quatbody.quatbody(x=0, y=0, z=0, vx=0, vy=0, vz=0, phi=0, theta=0, psi=0, p=0, q=0, r=0, quat=None)[source]#

A rigid-body object with a quaternion attitude

The quatbody is the quaternion replicate of the rigidbody. It extends the datapoint class to form an elementary rigidbody object in space, i.e. an object with length and attitude, where the attitude is represented by a unit quaternion rather than by Euler angles.

The dynamics model is the same model of the rigidbody. However, the Euler angles kinematics are singular at \(\theta = \pm 90°\) (gimbal lock), while the quaternion kinematics are not. This makes the quatbody suitable for agile vehicles, such as drones, that may reach any attitude (e.g. flips).

Its state vector consists of the following variables:

\[X = [x, y, z, v_x, v_y, v_z, q_w, q_x, q_y, q_z, p, q, r]^T\]

Position coordinates, velocity coordinates.
Attitude quaternion (scalar first), angular rates.

The quaternion \([q_w, q_x, q_y, q_z]\) represents the same attitude as the 3-2-1 Euler angles \([\varphi, \theta, \psi]\) of the rigidbody (see euler2quat). The Euler angles phi, theta, psi are available as properties that are derived from the quaternion.

Arguments

xfloat or int, optional: The x-position of the datapoint. Default value \(x = 0\).
yfloat or int, optional: The y-position of the datapoint. Default value \(y = 0\).
zfloat or int, optional: The z-position of the datapoint. Default value \(z = 0\).
vxfloat or int, optional: Component of velocity along the x-axis. Default value \(v_x = 0\).
vyfloat or int, optional: Component of velocity along the y-axis. Default value \(v_y = 0\).
vzfloat or int, optional: Component of velocity along the z-axis. Default value \(v_z = 0\).
phifloat or int, optional: Initial Euler angle representing rotation around the x-axis (rad). Default value \(\varphi = 0\).
thetafloat or int, optional: Initial Euler angle representing rotation around the y-axis (rad). Default value \(\theta = 0\).
psifloat or int, optional: Initial Euler angle representing rotation around the z-axis (rad). Default value \(\psi = 0\).
pfloat or int, optional: Angular rate around the x-axis (roll). (rad/sec). Default value \(p = 0\).
qfloat or int, optional: Angular rate around the y-axis (pitch). (rad/sec). Default value \(q = 0\).
rfloat or int, optional: Angular rate around the z-axis (yaw). (rad/sec). Default value \(r = 0\).
quatarray_like, optional: Initial attitude quaternion \([q_w, q_x, q_y, q_z]\). If provided, it overrides phi, theta, and psi. The quaternion is normalized. Default value: None.

The initial attitude is provided either by Euler angles, as in the rigidbody, or directly by a quaternion. The vector of initial conditions can be retrieved by calling quatbody.X0:

>>> from c4dynamics import quatbody

>>> qb = quatbody(z = 10, psi = 90 * c4d.d2r, r = 0.5)
>>> qb.X0 
[0  0  10  0  0  0  0.707  0  0  0.707  0  0  0.5]

Note

The quaternion components are named \(q_w, q_x, q_y, q_z\) (qw, qx, qy, qz) rather than \(q_0, ..., q_3\), since the 0 suffix of a state variable is reserved for its initial value (e.g. q0 is the initial pitch rate, see state.X0).

Parameters:

mass (float) – The mass of the datapoint
I ([float, float, float]) – An array of moments of inertia

Example

A quadcopter performs a back flip: it starts at a level attitude with a pitch rate of one revolution per second, and rotates freely (no external forces nor moments) for one second.

The Euler angles kinematics of a rigidbody cross the singularity at \(\theta = 90°\). The quaternion kinematics of the quatbody remain regular along the entire maneuver.

import required packages:

>>> import c4dynamics as c4d
>>> from matplotlib import pyplot as plt
>>> import numpy as np

Settings and initial conditions:

>>> dt = 0.001
>>> drone = c4d.quatbody(z = 10, q = 2 * np.pi)
>>> drone.I = [0.0082, 0.0082, 0.0149]
>>> drone.mass = 1.2

Main loop:

>>> for t in np.arange(0, 1, dt):
...   drone.store(t)
...   drone.inteqm(np.zeros(3), np.zeros(3), dt)  

After a full revolution the quaternion returns to the level attitude (up to the sign):

>>> drone.store(1)
>>> np.abs(drone.quat)  
[1  0  0  0]

The pitch component of the quaternion, \(q_y = sin(q \cdot t / 2)\), goes smoothly through the entire revolution:

>>> _, ax = plt.subplots(2, 1)
>>> drone.plot('qw', ax = ax[0])
>>> ax[0].set(xlabel = '')  
>>> drone.plot('qy', ax = ax[1])

The animate method allows the user to play the attitude histories given a 3D model (requires installation of open3D), exactly as with the rigidbody.animate.

Properties

`quatbody.mass`	Gets and sets the object's mass.
`quatbody.I`	Gets and sets the array of moments of inertia.
`quatbody.quat`	Gets and sets the attitude quaternion.
`quatbody.angles`	Returns an array of Euler angles.
`quatbody.phi`	Gets and sets the Euler roll angle, \(\varphi\) (rad).
`quatbody.theta`	Gets and sets the Euler pitch angle, \(\theta\) (rad).
`quatbody.psi`	Gets and sets the Euler yaw angle, \(\psi\) (rad).
`quatbody.ang_rates`	Returns an array of angular rates.
`quatbody.BR`	Returns a Body-from-Reference Direction Cosine Matrix (DCM).
`quatbody.RB`	Returns a Reference-from-Body Direction Cosine Matrix (DCM).

Methods

`quatbody.inteqm`(forces, moments, dt)	Advances the state vector, `quatbody.X`, with respect to the input forces and moments on a single step of time, dt.
`quatbody.data`([var, scale])	Returns arrays of stored time and data.
`quatbody.plot`(var[, scale, ax, filename, ...])	Draws plots of trajectories or variable evolution over time.
`quatbody.animate`(modelpath[, angle0, ...])	Animate a rigidbody.

Quatbody

Contents

Quatbody#