c4dynamics.states.lib.quatbody.quatbody.inteqm

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.

Integrates equations of six degrees motion using the Runge-Kutta method.

This method numerically integrates the equations of motion for a dynamic system using the fourth-order Runge-Kutta method as given by eqm.int6q.

The derivatives of the equations are of six dimensional motion with quaternion attitude kinematics as given by eqm.eqm6q. The quaternion is normalized at the end of each step.

Parameters:

• forces (numpy.array or list) – An external forces vector acting on the body, forces = [Fx, Fy, Fz]

• moments (numpy.array or list) – An external moments vector acting on the body, moments = [Mx, My, Mz]

• dt (float or int) – Interval time step for integration.

Returns:

out (numpy.float64) – An acceleration array at the final time step, \([dv_x, dv_y, dv_z, dp, dq, dr]\).

Warning

This method is not recommanded when the vectors of forces or moments depend on the state variables. Since the vectors of forces and moments are provided once at the entrance to the integration, they remain constant for the entire steps. Therefore, when the forces or moments depend on the state variables the results of this method are not accurate and may lead to instability.

Examples

A drone applies a roll torque to perform a half roll and then stops the rotation by an opposite torque, ending up inverted (\(q_x \approx 1\), i.e. \(\varphi \approx 180°\)).

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()
>>> drone.I = [0.0082, 0.0082, 0.0149]

Main loop:

>>> for ti in np.arange(0, 1, dt):
...   torque = [0.1, 0, 0] if ti < 0.5 else [-0.1, 0, 0]
...   drone.inteqm(np.zeros(3), torque, dt)  
...   drone.store(ti)

Plot results:

>>> _, ax = plt.subplots(2, 1, gridspec_kw = {'hspace': 0.5})
>>> drone.plot('p', ax = ax[0])
>>> ax[0].set(xlabel = '')  
>>> drone.plot('qx', ax = ax[1])