BodyKinematics6Dof Class Reference

#include <BodyKinematics6Dof.h>

Public Member Functions
	BodyKinematics6Dof (ISimObjectCreator *creator, const std::string &angularRateName, double convergenceTime)
int	PosIndex () const
	Global index of the position state.
int	QuaterIndex () const
	Global index of the attitude quaternion state.
int	LocalVelIndex () const
	Global index of the body-frame velocity state.
int	AngularRateIndex () const
	Global index of the body-frame angular-rate state.
int	LocalIndex (int globalIndex) const
	Index within the object's state slice of the state with global index globalIndex.
void	Derivatives (const double *X, double posDot[3], double quaterDot[4]) const
void	AddJacobian (const double *localX, double *J, int nStates) const

Static Public Member Functions
static void	RotationDerivatives (const double quater[4], double dRdq[4][9])

Detailed Description

The kinematics of a rigid body in six degrees of freedom, with the attitude as a quaternion.

Registers the four states of the body, in this order:

• Pos (3), the position in the global (NED) frame, default zero;
• Quater (4), the attitude quaternion [q0, q1, q2, q3], scalar first, default the identity;
• LocalVel (3), the linear velocity in the body frame, default zero;
• the angular rate in the body frame (3), named by the body (AngVel, Omega), default zero.

and supplies their kinematic derivatives

\[ \dot{\mathbf{p}} = \mathbf{R}(\mathbf{q})\,\mathbf{v}, \qquad \dot{\mathbf{q}} = \tfrac12\,\mathbf{q}\otimes(0,\boldsymbol{\omega}) + \tfrac{1}{2\tau}\left(1-|\mathbf{q}|^2\right)\mathbf{q} \]

(sfh::sim::OmegaToQDot, \(\tau\) the convergence time that drives the norm of \(\mathbf{q}\) towards one), and their complete analytic Jacobian: the four blocks \(\partial\dot{\mathbf{p}}/\partial\mathbf{q}\), \(\partial\dot{\mathbf{p}}/\partial\mathbf{v}\), \(\partial\dot{\mathbf{q}}/\partial\mathbf{q}\) (the rate matrix and the normalisation term) and \(\partial\dot{\mathbf{q}}/\partial\boldsymbol{\omega}\).

The body keeps its own kinetics: the wrench, the mass matrix and the derivatives of the velocities.

Two kinds of index are used. The state indices (PosIndex() and so on) are the model-global offsets ISimObjectCreator::AddState returned; OdeFcn is handed the global state vector and indexes it with them. A Jacobian callback is handed only this object's slice of the state vector (see fhsim_base's JacobianStates.h), which LocalIndex() indexes. AddJacobian() assumes that Pos is the object's first state, so the body must create this object before it registers any other state.

Constructor & Destructor Documentation

BodyKinematics6Dof()

BodyKinematics6Dof::BodyKinematics6Dof	(	ISimObjectCreator *	creator,
		const std::string &	angularRateName,
		double	convergenceTime
	)

Registers the four states with creator.

Parameters

creator	The creator of the SimObject that owns the body.
angularRateName	The name of the angular-rate state, for example AngVel or Omega.
convergenceTime	The time constant [s] with which the quaternion norm is driven towards one.

Member Function Documentation

AddJacobian()

void BodyKinematics6Dof::AddJacobian	(	const double *	localX,
		double *	J,
		int	nStates
	)		const

Adds the four kinematic blocks to a Jacobian.

Parameters

[in]	localX	The object's slice of the state vector, as OdeJacobian is handed it.
[in,out]	J	The object's Jacobian, nStates by nStates, row major.
[in]	nStates	The number of states of the object.

Derivatives()

void BodyKinematics6Dof::Derivatives	(	const double *	X,
		double	posDot[3],
		double	quaterDot[4]
	)		const

The kinematic derivatives.

Parameters

[in]	X	The global state vector, as OdeFcn is handed it.
[out]	posDot	The derivative of Pos, the velocity in the global frame.
[out]	quaterDot	The derivative of Quater.

RotationDerivatives()

static void BodyKinematics6Dof::RotationDerivatives ( const double  quater[4], double  dRdq[4][9]  )

static

The derivatives of the rotation matrix sfh::sim::Rquat against the four quaternion components.

Parameters

[in]	quater	The quaternion [q0, q1, q2, q3].
[out]	dRdq	dRdq[k][3 * i + j] is the derivative of R[i][j] against q_k.

---

The documentation for this class was generated from the following file:

• webfhsim/reloadrepos/fhsim_base/src/body/BodyKinematics6Dof.h