# File kernel/src/modelingTools/NewtonEulerDS.hpp¶

Go to the source code of this file

Typedefs

typedef void (*FExt_NE)(double t, double *f, unsigned int size_z, double *z)
typedef void (*FInt_NE)(double t, double *q, double *v, double *f, unsigned int size_z, double *z)

Pointer to function for plug-in.

Functions

void computeExtForceAtPos(SP::SiconosVector q, bool isMextExpressedInInertialFrame, SP::SiconosVector force, bool forceAbsRef, SP::SiconosVector pos, bool posAbsRef, SP::SiconosVector fExt, SP::SiconosVector mExt, bool accumulate)

Compute the force and moment vectors applied to a body with state q from a force vector at a given position.

void computeT(SP::SiconosVector q, SP::SimpleMatrix T)
class NewtonEulerDS : public SecondOrderDS
#include <NewtonEulerDS.hpp>

NewtonEuler non linear dynamical systems.

The equations of motion in the Newton-Euler formalism can be stated as

\verbatim \left\{\begin{array}{rcl} M \dot v + F_{int}(q,v, \Omega, t)&=& F_{ext}(t), \ \ I \dot \Omega + \Omega \wedge I\Omega + M_{int}(q,v, \Omega, t) &=& M_{ext}(t), \ \ \dot q &=& T(q) [ v, \Omega] \ \ \dot R &=& R \tilde \Omega,\quad R^{-1}=R^T,\quad \det(R)=1 . \end{array}\right.

with

• $$x_G,v_G$$ position and velocity of the center of mass expressed in a inertial frame of reference (world frame)

• $$\Omega$$ angular velocity vector expressed in the body-fixed frame (frame attached to the object)

• $$R$$ rotation matrix form the inertial frame to the body-fixed frame $$R^{-1}=R^T, \det(R)=1$$, i.e $$R\in SO^+(3)$$

• $$M=m\,I_{3\times 3}$$ diagonal mass matrix with $$m \in \mathbb{R}$$ the scalar mass

• $$I$$ constant inertia matrix

• $$F_{ext}$$ and $$M_{ext}$$ are the external applied forces and moment

In the current implementation, $$R$$ is parametrized by a unit quaternion.

Right-hand side computation

void resetToInitialState()

reset the state to the initial state

void initRhs(double time)

allocate (if needed) and compute rhs and its jacobian.

Parameters
• time: of initialization

void initializeNonSmoothInput(unsigned int level)

set nonsmooth input to zero

Parameters
• level: input-level to be initialized.

virtual void computeRhs(double time)

update right-hand side for the current state

Parameters
• time: of interest

virtual void computeJacobianRhsx(double time)

update $$\nabla_x rhs$$ for the current state

Parameters
• time: of interest

void resetAllNonSmoothParts()

reset non-smooth part of the rhs (i.e.

p), for all ‘levels’

void resetNonSmoothPart(unsigned int level)

set nonsmooth part of the rhs (i.e.

p) to zero for a given level

Parameters
• level:

SP::SiconosVector forces() const

get forces

Return

pointer on a SiconosVector

SP::SiconosMatrix jacobianqForces() const

get JacobianqForces

Return

pointer on a SiconosMatrix

SP::SiconosMatrix jacobianvForces() const

get JacobianvForces

Return

pointer on a SiconosMatrix

Attributes access

virtual unsigned int getqDim() const

Returns dimension of vector q.

SP::SiconosVector q() const

Return

pointer on a SiconosVector

virtual void setQ(const SiconosVector &newValue)

set value of generalized coordinates vector (copy)

Parameters
• newValue:

virtual void setQPtr(SP::SiconosVector newPtr)

set value of generalized coordinates vector (pointer link)

Parameters
• newPtr:

void setQ0(const SiconosVector &newValue)

set initial state (copy)

Parameters
• newValue:

void setQ0Ptr(SP::SiconosVector newPtr)

Parameters
• newPtr:

SP::SiconosVector twist() const

get twist

Return

pointer on a SiconosVector

SP::SiconosVector velocity() const

get twist

Return

pointer on a SiconosVector this accessor is left to get a uniform access to velocity. This should be removed with MechanicalDS class

SP::SiconosVector twist0() const
SP::SiconosVector velocity0() const

get initial velocity (pointer)

Return

pointer on a SiconosVector

void setVelocity(const SiconosVector &newValue)

set velocity (copy)

Parameters
• newValue:

void setVelocityPtr(SP::SiconosVector newPtr)

Parameters
• newPtr:

void setVelocity0(const SiconosVector &newValue)

set initial velocity (copy)

Parameters
• newValue:

void setVelocity0Ptr(SP::SiconosVector newPtr)

Parameters
• newPtr:

SP::SiconosVector acceleration() const

Return

pointer on a SiconosVector

virtual void computeMass()

default function to compute the mass

virtual void computeMass(SP::SiconosVector position)

function to compute the mass

Parameters
• position: value used to evaluate the mass matrix

SP::SiconosVector linearVelocity(bool absoluteRef) const

Get the linear velocity in the absolute (inertial) or relative (body) frame of reference.

Return

A SiconosVector of size 3 containing the linear velocity of this dynamical system.

Parameters
• absoluteRef: If true, velocity is returned in the inertial frame, otherwise velocity is returned in the body frame.

void linearVelocity(bool absoluteRef, SiconosVector &v) const

Fill a SiconosVector with the linear velocity in the absolute (inertial) or relative (body) frame of reference.

Parameters
• absoluteRef: If true, velocity is returned in the inertial frame, otherwise velocity is returned in the body frame.

• v: A SiconosVector of size 3 to receive the linear velocity.

SP::SiconosVector angularVelocity(bool absoluteRef) const

Get the angular velocity in the absolute (inertial) or relative (body) frame of reference.

Return

A SiconosVector of size 3 containing the angular velocity of this dynamical system.

Parameters
• absoluteRef: If true, velocity is returned in the inertial frame, otherwise velocity is returned in the body frame.

void angularVelocity(bool absoluteRef, SiconosVector &w) const

Fill a SiconosVector with the angular velocity in the absolute (inertial) or relative (body) frame of reference.

Parameters
• absoluteRef: If true, velocity is returned in the inertial frame, otherwise velocity is returned in the body frame.

• w: A SiconosVector of size 3 to receive the angular velocity.

double scalarMass() const

get mass value

Return

a double

void setScalarMass(double mass)

Modify the scalar mass.

SP::SiconosMatrix inertia()
void setInertia(SP::SiconosMatrix newInertia)
void setInertia(double ix, double iy, double iz)
void updateMassMatrix()

to be called after scalar mass or inertia matrix have changed

SP::SiconosVector fExt() const

get fExt

Return

pointer on a plugged vector

void setFExtPtr(SP::SiconosVector newPtr)

set fExt to pointer newPtr

Parameters
• newPtr: a SP to a Simple vector

void setMExtPtr(SP::SiconosVector newPtr)

set mExt to pointer newPtr

Parameters
• newPtr: a SP to a Simple vector

SP::SiconosVector mGyr() const

get mGyr

Return

pointer on a plugged vector

SP::SimpleMatrix T()
SP::SimpleMatrix Tdot()
SP::SiconosVector dotq()

Memory vectors management

const SiconosMemory &qMemory()

get all the values of the state vector q stored in memory

Return

a memory

const SiconosMemory &twistMemory()

get all the values of the state vector twist stored in memory

Return

a memory

const SiconosMemory &velocityMemory()

get all the values of the state vector twist stored in memory

Return

a memory

void initMemory(unsigned int steps)

initialize the SiconosMemory objects with a positive size.

Parameters

void swapInMemory()

push the current values of x, q and r in the stored previous values xMemory, qMemory, rMemory,

const SiconosMemory &forcesMemory()

get forces in memory buff

Return

pointer on a SiconosMemory

const SiconosMemory &dotqMemory()

Miscellaneous public methods

double computeKineticEnergy()

To compute the kinetic energy.

void display(bool brief = true) const

print the data to the screen

void setIsMextExpressedInInertialFrame(bool value)
void setNullifyMGyr(bool value)
virtual void normalizeq()
void init_inverse_mass()

Allocate memory for the lu factorization of the mass of the system.

Useful for some integrators with system inversion involving the mass

void update_inverse_mass()

Update the content of the lu factorization of the mass of the system, if required.

Plugins management

void setComputeJacobianFIntqByFD(bool value)
void setComputeJacobianFIntvByFD(bool value)
void setComputeJacobianMIntqByFD(bool value)
void setComputeJacobianMIntvByFD(bool value)
void setComputeFExtFunction(const std::string &pluginPath, const std::string &functionName)

allow to set a specified function to compute _fExt

Parameters
• pluginPath: the complete path to the plugin

• functionName: the name of the function to use in this plugin

void setComputeMExtFunction(const std::string &pluginPath, const std::string &functionName)

allow to set a specified function to compute _mExt

Parameters
• pluginPath: the complete path to the plugin

• functionName: the name of the function to use in this plugin

void setComputeFExtFunction(FExt_NE fct)

set a specified function to compute _fExt

Parameters
• fct: a pointer on the plugin function

void setComputeMExtFunction(FExt_NE fct)

set a specified function to compute _mExt

Parameters
• fct: a pointer on the plugin function

void setComputeFIntFunction(const std::string &pluginPath, const std::string &functionName)

allow to set a specified function to compute _fInt

Parameters
• pluginPath: the complete path to the plugin

• functionName: the name of the function to use in this plugin

void setComputeMIntFunction(const std::string &pluginPath, const std::string &functionName)

allow to set a specified function to compute _mInt

Parameters
• pluginPath: the complete path to the plugin

• functionName: the name of the function to use in this plugin

void setComputeFIntFunction(FInt_NE fct)

set a specified function to compute _fInt

Parameters
• fct: a pointer on the plugin function

void setComputeMIntFunction(FInt_NE fct)

set a specified function to compute _mInt

Parameters
• fct: a pointer on the plugin function

void setComputeJacobianFIntqFunction(const std::string &pluginPath, const std::string &functionName)

allow to set a specified function to compute the jacobian w.r.t q of the internal forces

Parameters
• pluginPath: std::string : the complete path to the plugin

• functionName: std::string : the name of the function to use in this plugin

void setComputeJacobianFIntvFunction(const std::string &pluginPath, const std::string &functionName)

allow to set a specified function to compute the jacobian following v of the internal forces w.r.t.

Parameters
• pluginPath: std::string : the complete path to the plugin

• functionName: std::string : the name of the function to use in this plugin

void setComputeJacobianFIntqFunction(FInt_NE fct)

set a specified function to compute jacobian following q of the FInt

Parameters
• fct: a pointer on the plugin function

void setComputeJacobianFIntvFunction(FInt_NE fct)

set a specified function to compute jacobian following v of the FInt

Parameters
• fct: a pointer on the plugin function

void setComputeJacobianMIntqFunction(const std::string &pluginPath, const std::string &functionName)

allow to set a specified function to compute the jacobian w.r.t q of the internal forces

Parameters
• pluginPath: std::string : the complete path to the plugin

• functionName: std::string : the name of the function to use in this plugin

void setComputeJacobianMIntvFunction(const std::string &pluginPath, const std::string &functionName)

allow to set a specified function to compute the jacobian following v of the internal forces w.r.t.

Parameters
• pluginPath: std::string : the complete path to the plugin

• functionName: std::string : the name of the function to use in this plugin

void setComputeJacobianMIntqFunction(FInt_NE fct)

set a specified function to compute jacobian following q of the FInt

Parameters
• fct: a pointer on the plugin function

void setComputeJacobianMIntvFunction(FInt_NE fct)

set a specified function to compute jacobian following v of the FInt

Parameters
• fct: a pointer on the plugin function

virtual void computeFExt(double time)

function to compute the external forces

Parameters
• time: the current time

virtual void computeFExt(double time, SP::SiconosVector fExt)

default function to compute the external forces

Parameters
• time: the current time

• fExt: the computed external force (in-out param)

virtual void computeMExt(double time, SP::SiconosVector mExt)

function to compute the external moments The external moments are expressed by default in the body frame, since the Euler equation for Omega is written in the bodyfixed frame.

Nevertheless, if _isMextExpressedInInertialFrame) is set to true, we assume that the external moment is given in the inertial frame and we perform the rotation afterwards

Parameters
• time: the current time

• mExt: the computed external moment (in-out param)

virtual void computeMExt(double time)
void addExtForceAtPos(SP::SiconosVector force, bool forceAbsRef, SP::SiconosVector pos = SP::SiconosVector(), bool posAbsRef = false)

Adds a force/torque impulse to a body’s FExt and MExt vectors in either absolute (inertial) or relative (body) frame.

Modifies contents of _fExt and _mExt! Therefore these must have been set as constant vectors using setFExtPtr and setMExtPtr prior to calling this function. Adjustments to _mExt will take into account the value of _isMextExpressedInInertialFrame.

Parameters
• force: A force vector to be added.

• forceAbsRef: If true, force is in inertial frame, otherwise it is in body frame.

• pos: A position at which force should be applied. If nullptr, the center of mass is assumed.

• posAbsRef: If true, pos is in inertial frame, otherwise it is in body frame.

void computeJacobianMExtqExpressedInInertialFrameByFD(double time, SP::SiconosVector q)
void computeJacobianMExtqExpressedInInertialFrame(double time, SP::SiconosVector q)
void computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v)

default function to compute the internal forces

Parameters
• time: the current time function to compute the internal forces

• time: the current time

• q:

• v:

virtual void computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector fInt)

default function to compute the internal forces

Parameters
• time: the current time

• q:

• v:

• fInt: the computed internal force vector

void computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v)

default function to compute the internal moments

Parameters
• time: the current time

• q:

• v:

virtual void computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v, SP::SiconosVector mInt)

default function to compute the internal moments

Parameters
• time: the current time

• q:

• v:

• mInt: the computed internal moment vector

virtual void updatePlugins(double time)

default function to update the plugins functions using a new time:

Parameters
• time: the current time

void init_forces()

Allocate memory for forces and its jacobian.

virtual void computeForces(double time)

Default function to compute forces.

Parameters
• time: double, the current time

virtual void computeForces(double time, SP::SiconosVector q, SP::SiconosVector twist)

function to compute forces with some specific values for q and twist (ie not those of the current state).

Parameters
• time: double : the current time

• q: SP::SiconosVector: pointers on q

• twist: SP::SiconosVector: pointers on twist

virtual void computeJacobianqForces(double time)

Default function to compute the jacobian w.r.t.

q of forces

Parameters
• time: double, the current time

virtual void computeJacobianvForces(double time)

Default function to compute the jacobian w.r.t.

v of forces

Parameters
• time: double, the current time

virtual void computeMGyr(SP::SiconosVector twist)

function to compute gyroscopic forces with some specific values for q and twist (ie not those of the current state).

Parameters
• twist: SP::SiconosVector: pointers on twist vector

virtual void computeMGyr(SP::SiconosVector twist, SP::SiconosVector mGyr)

function to compute gyroscopic forces with some specific values for q and twist (ie not those of the current state).

Parameters
• twist: pointer to twist vector

• mGyr: pointer to gyroscopic forces

virtual void computeJacobianMGyrtwist(double time)

Default function to compute the jacobian following q of mGyr.

Parameters
• time: the current time

virtual void computeJacobianMGyrtwistByFD(double time, SP::SiconosVector q, SP::SiconosVector twist)

Default function to compute the jacobian following q of mGyr by forward finite difference.

Parameters
• time: the current time

• q: current state

• twist: pointer to twist vector

void computeJacobianFIntq(double time)

Default function to compute the jacobian following v of mGyr.

Parameters
• time: the current timeTo compute the jacobian w.r.t q of the internal forces

• time: double : the current time

void computeJacobianFIntv(double time)

To compute the jacobian w.r.t v of the internal forces.

Parameters
• time: double : the current time

virtual void computeJacobianFIntq(double time, SP::SiconosVector position, SP::SiconosVector twist)

To compute the jacobian w.r.t q of the internal forces.

Parameters
• time: double

• position: SP::SiconosVector

• twist: SP::SiconosVector

void computeJacobianFIntqByFD(double time, SP::SiconosVector position, SP::SiconosVector twist)

To compute the jacobian w.r.t q of the internal forces by forward finite difference.

Parameters
• time: double

• position: SP::SiconosVector

• twist: SP::SiconosVector

virtual void computeJacobianFIntv(double time, SP::SiconosVector position, SP::SiconosVector twist)

To compute the jacobian w.r.t.

v of the internal forces

Parameters
• time: double: the current time

• position: SP::SiconosVector

• twist: SP::SiconosVector

void computeJacobianFIntvByFD(double time, SP::SiconosVector position, SP::SiconosVector twist)

To compute the jacobian w.r.t v of the internal forces by forward finite difference.

Parameters
• time: double

• position: SP::SiconosVector

• twist: SP::SiconosVector

virtual void computeJacobianMIntq(double time)

To compute the jacobian w.r.t q of the internal forces.

Parameters
• time: double : the current time

virtual void computeJacobianMIntv(double time)

To compute the jacobian w.r.t v of the internal forces.

Parameters
• time: double : the current time

virtual void computeJacobianMIntq(double time, SP::SiconosVector position, SP::SiconosVector twist)

To compute the jacobian w.r.t q of the internal forces.

Parameters
• time: double : the current time,

• position: SP::SiconosVector

• twist: SP::SiconosVector

void computeJacobianMIntqByFD(double time, SP::SiconosVector position, SP::SiconosVector twist)

To compute the jacobian w.r.t q of the internal moments by forward finite difference.

Parameters
• time: double

• position: SP::SiconosVector

• twist: SP::SiconosVector

virtual void computeJacobianMIntv(double time, SP::SiconosVector position, SP::SiconosVector twist)

To compute the jacobian w.r.t.

v of the internal forces

Parameters
• time: double: the current time

• position: SP::SiconosVector

• twist: SP::SiconosVector

void computeJacobianMIntvByFD(double time, SP::SiconosVector position, SP::SiconosVector twist)

To compute the jacobian w.r.t v of the internal moments by forward finite difference.

Parameters
• time: double

• position: SP::SiconosVector

• twist: SP::SiconosVector

virtual void computeT()
virtual void computeTdot()

Public Functions

NewtonEulerDS(SP::SiconosVector position, SP::SiconosVector twist, double mass, SP::SiconosMatrix inertia)

constructor from a minimum set of data

Parameters
• position: initial coordinates of this DynamicalSystem

• twist: initial twist of this DynamicalSystem

• mass: the mass

• inertia: the inertia matrix

virtual ~NewtonEulerDS()

destructor

ACCEPT_STD_VISITORS()

Protected Types

enum NewtonEulerDSRhsMatrices

Values:

jacobianXBloc00
jacobianXBloc01
jacobianXBloc10
jacobianXBloc11
zeroMatrix
zeroMatrixqDim
numberOfRhsMatrices

Protected Functions

NewtonEulerDS()

Default constructor.

void _init()

Common code for constructors should be replaced in C++11 by delegating constructors.

void _zeroPlugin()

build all _plugin…

ACCEPT_SERIALIZATION(NewtonEulerDS)

Protected Attributes

SP::SiconosVector _acceleration

_acceleration contains the time derivative of the twist

bool _computeJacobianFIntqByFD

If true, we compute the missing Jacobian by forward finite difference.

bool _computeJacobianFInttwistByFD

If true, we compute the missing Jacobian by forward finite difference.

bool _computeJacobianMIntqByFD

If true, we compute the missing Jacobian by forward finite difference.

bool _computeJacobianMInttwistByFD

If true, we compute the missing Jacobian by forward finite difference.

SP::SiconosVector _dotq

The time derivative of $$q$$, $$\dot q$$.

SiconosMemory _dotqMemory
double _epsilonFD

value of the step in finite difference

SP::SiconosVector _fExt

external forces of the system

SP::SiconosVector _fInt

internal forces of the system

SiconosMemory _forcesMemory
bool _hasConstantFExt

boolean if _fext is constant (set thanks to setFExtPtr for instance) false by default

bool _hasConstantMExt

boolean if _mext is constant (set thanks to setMExtPtr for instance) false by default

SP::SiconosMatrix _I

Inertial matrix.

bool _isMextExpressedInInertialFrame

if true, we assume that mExt is given in inertial frame (default false)

SP::SimpleMatrix _jacobianFIntq

jacobian_q FInt w.r.t q

SP::SimpleMatrix _jacobianFInttwist

jacobian_twist FInt w.r.t the twist

SP::SimpleMatrix _jacobianMExtq

jacobian_q MExt w.r.t q

SP::SimpleMatrix _jacobianMGyrtwist

jacobian_twist of mGyr w.r.t the twist

SP::SimpleMatrix _jacobianMIntq

jacobian_q MInt w.r.t q

SP::SimpleMatrix _jacobianMInttwist

jacobian_twist MInt w.r.t the twist

SP::SimpleMatrix _jacobianWrenchq

jacobian_q forces

SP::SimpleMatrix _jacobianWrenchTwist

jacobian_{twist} forces

SP::SiconosVector _mExt

external moment expressed in the inertial frame

SP::SiconosVector _mGyr

gyroscpical moment

SP::SiconosVector _mInt

external moment expressed in the body-fixed frame

internal moment of the forces

bool _nullifyMGyr

if true, we set the gyroscopic forces equal to 0 (default false)

SP::PluggedObject _pluginFExt

Plugin to compute strength of external forces.

SP::PluggedObject _pluginFInt

Plugin to compute strength of internal forces.

SP::PluggedObject _pluginJacqFInt

The following code is commented because the jacobian of _mInt and _fInt are not yet used by the numerical scheme.

Will be needed by a fully implicit scheme for instance.jacobian_q jacobian_{qDot} NewtonEulerDS plug-in to compute $$\nabla_qF_{Int}(\dot q, q, t)$$, id = “jacobianFIntq”

Parameters
• time: : current time

• sizeOfq: : size of vector q

• q: : pointer to the first element of q

• twist: : pointer to the first element of twist

• [inout] jacob: : pointer to the first element of the jacobian

• size: of vector z

• [inout] z: : a vector of user-defined parameters

SP::PluggedObject _pluginJacqMInt

NewtonEulerDS plug-in to compute $$\nabla_qM_{Int}(\dot q, q, t)$$, id = “jacobianMInttwist”.

Parameters
• time: : current time

• sizeOfq: : size of vector q

• q: : pointer to the first element of q

• twist: : pointer to the first element of twist

• [inout] jacob: : pointer to the first element of the jacobian

• size: of vector z

• [inout] z: : a vector of user-defined parameters

SP::PluggedObject _pluginJactwistFInt

NewtonEulerDS plug-in to compute $$\nabla_{\dot q}F_{Int}(\dot q, q, t)$$, id = “jacobianFIntTwist”.

Parameters
• time: : current time

• sizeOfq: : size of vector q

• q: : pointer to the first element of q

• twist: : pointer to the first element of twist

• [inout] jacob: : pointer to the first element of the jacobian

• size: of vector z

• [inout] z: : a vector of user-defined parameters

SP::PluggedObject _pluginJactwistMInt

NewtonEulerDS plug-in to compute $$\nabla_{\dot q}M_{Int}(\dot q, q, t)$$, id = “jacobianMInttwist”.

Parameters
• time: : current time

• sizeOfq: : size of vector q

• q: : pointer to the first element of q

• twist: : pointer to the first element of twist

• [inout] jacob: : pointer to the first element of the jacobian

• size: of vector z

• [inout] z: : a vector of user-defined parameters

SP::PluggedObject _pluginMExt

Plugin to compute the external moment expressed in the inertial frame.

SP::PluggedObject _pluginMInt

Plugin to compute moments of internal forces.

SP::SiconosVector _q

_q contains the representation of the system In the current implementation, we have _q[0:2] : the coordinates of the center of mass expressed in the inertial frame of reference (world frame) _q[3:6] : an unit quaternion representing the orientation of the solid.

This unit quaternion encodes the rotation mapping from the inertial frame of reference to the body-fixed frame

unsigned int _qDim

Dimension of _q, is not necessary equal to _ndof.

In our case, _qDim = 7 and _ndof =6

SiconosMemory _qMemory
VectorOfSMatrices _rhsMatrices

A container of matrices to save matrices that are involed in first order from of NewtonEulerDS system values (jacobianXBloc10, jacobianXBloc11, zeroMatrix, idMatrix) No get-set functions at the time.

Only used as a protected member.

double _scalarMass

Scalar mass of the system.

SP::SimpleMatrix _T

Matrix depending on the parametrization of the orientation $$v = T(q) \dot q$$.

SP::SimpleMatrix _Tdot

Time derivative of T.

$$\dot v = \dot T(q) \dot q + T(q) \ddot q$$

SP::SiconosVector _twist

_twist contains the twist of the Newton Euler dynamical system.

_twist[0:2] : $$v_G \in \RR^3$$ velocity of the center of mass in the inertial frame of reference (world frame). _twist[3:5] : $$\Omega\in\RR^3$$ angular velocity expressed in the body-fixed frame

SP::SiconosVector _twist0

Initial twist.

SiconosMemory _twistMemory

Memory vectors that stores the values within the timestep.

SP::SiconosVector _wrench

wrench (q,twist,t)= [ fExt - fInt ; mExtBodyFrame - mGyr - mInt ]^T