#include "mbs_data.h"
#include "mbs_aux.h"
#include "mbs_sensor_struct.h"

Functions
int	dirdynared (MbsAux mbs_aux, MbsData s)

int	invdynared (MbsAux mbs_aux, MbsData s)
	Compute the required forces ont the actuated joints in a configuration. More...

int	mbs_Mred (MbsAux mbs_aux, MbsData s)
	compute Mr, the reduced mass matrix [nqu x nqu] obtained after the 2 consecutive reductions (Robotran Basics). More...

int	mbs_Rred (MbsAux mbs_aux, MbsData s)
	compute Rred, the reduced equations of motions on a residual form [nqu x 1] obtained after the 2 consecutive reductions (Robotran Basics). More...

int	mbs_Rred_core (MbsAux mbs_aux, MbsData s)
	Compute the residual force on independant joints and required force in driven joints. More...

int	mbs_comp_LUdcmp_mJv (MbsData s, MbsAux mbs_aux)
	compute and update (!) the mJv matrix, decomposing it to the LU format mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]]; return ludcmp(mbs_aux->mJv, s->nqv, mbs_aux->ind_mJv, &d); More...

int	mbs_close_geo (MbsData s, MbsAux mbs_aux)
	compute a position of the multibody system which solves the constraints More...

int	mbs_step_close_geo (MbsData s, MbsAux mbs_aux)
	compute an iteration of the position of the multibody system to solve the constraints More...

int	mbs_anim_close_geo (MbsData mbs_data, MbsAux mbs_aux)
	Generate the animation for failed Newton-Raphson procedure. More...

void	mbs_close_kin (MbsData s, MbsAux mbs_aux)
	compute the dependent velocities that solves the constraints. More...

void	mbs_calc_hJ (MbsData s, MbsAux mbs_aux)
	compute the current value of the constraints ( ) and the constraint Jacobian matrix ( $\textbf{J}=\frac{\partial h(q)}{\partial q^t}$ ). More...

void	mbs_calc_jdqd (MbsData s, MbsAux mbs_aux)
	compute the quadratic term of the constraints at acceleration level: $\dot{\textbf{J}}\dot{q}(q,\dot{q})$ More...

int	mbs_calc_force (MbsData *s)
	compute the force and torques applied on the multibody system. More...

Function Documentation

◆ dirdynared()

int dirdynared	(	MbsAux *	mbs_aux,
		MbsData *	s
	)

◆ invdynared()

int invdynared	(	MbsAux *	mbs_aux,
		MbsData *	s
	)

Compute the required forces ont the actuated joints in a configuration.

Parameters

[in,out]	mbs_aux	the local computation structure, where the result is stored in the variable Mr.
[in,out]	s	the MbsData of the system, where q(iqv),qd(iqv) and qdd(iqv) are updated.

Returns: Error status, <0 in case of failure.

case 1 : UNconstrained system

case 2 : UNconstrained system, fully driven

case 3 : constrained system with u : no actuated driven, overactuation possible

case 4 : constrained fully driven : actionnement sur c ou v

◆ mbs_anim_close_geo()

int mbs_anim_close_geo	(	MbsData *	mbs_data,
		MbsAux *	mbs_aux
	)

Generate the animation for failed Newton-Raphson procedure.

Parameters

[in,out]	mbs_data	the MbsData structure.
[in]	mbs_aux	the mbs_aux structure of the failed process. The initial coordinates vector values are stored in MbsAux::q_save.

◆ mbs_calc_force()

int mbs_calc_force ( MbsData * s )

compute the force and torques applied on the multibody system.

Set the matrices MbsData::frc and MbsData::trq at zero, then compute the contribution of:

Links forces
Links 3D forces
External forces
Joints forces

Parameters

[in,out] s the MbsData structure.

Returns: 0 if everything went well, <0 if an error has occured

◆ mbs_calc_hJ()

void mbs_calc_hJ	(	MbsData *	s,
		MbsAux *	mbs_aux
	)

compute the current value of the constraints ( $h(q)$ ) and the constraint Jacobian matrix ( $\textbf{J}=\frac{\partial h(q)}{\partial q^t}$ ).

Parameters

[in,out]	s	the MbsData structure.
[in,out]	mbs_aux	the MbsAux structure related to the MbsData structure.

◆ mbs_calc_jdqd()

void mbs_calc_jdqd	(	MbsData *	s,
		MbsAux *	mbs_aux
	)

compute the quadratic term of the constraints at acceleration level: $\dot{\textbf{J}}\dot{q}(q,\dot{q})$

Parameters

[in,out]	s	the MbsData structure.
[in,out]	mbs_aux	the MbsAux structure related to the MbsData structure.

◆ mbs_close_geo()

int mbs_close_geo	(	MbsData *	s,
		MbsAux *	mbs_aux
	)

compute a position of the multibody system which solves the constraints

The algorithm calls a Newton/Raphson procedure which solves the equation: $v^{k+1} = v^{k}-\left(J_{v}\right)^{-1} h$ .
Robotran Basics Eq(17), chapter 3.2.1, pp. 12

Parameters

[in,out]	s	the MbsData structure.
[in,out]	mbs_aux	the MbsAux structure related to the MbsData structure.

Returns: the number of iterations needed to close the system or an error code. Error status are negative numbers. The value -50 means that the maximum iteration number has been reached.

◆ mbs_close_kin()

void mbs_close_kin	(	MbsData *	s,
		MbsAux *	mbs_aux
	)

compute the dependent velocities that solves the constraints.

The function solves the linear equation: $\dot{v} = \textbf{B}_{vu} \dot{u}$ .
Robotran Basics Eq(18), chapter 3.2.1, pp. 12

Parameters

[in,out]	s	the MbsData structure.
[in,out]	mbs_aux	the MbsAux structure related to the MbsData structure.

◆ mbs_comp_LUdcmp_mJv()

int mbs_comp_LUdcmp_mJv	(	MbsData *	s,
		MbsAux *	mbs_aux
	)

compute and update (!) the mJv matrix, decomposing it to the LU format mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]]; return ludcmp(mbs_aux->mJv, s->nqv, mbs_aux->ind_mJv, &d);

! modify the field mbs_aux->mJv

Parameters

[in,out]	s	the MbsData structure.
[in,out]	mbs_aux	the MbsAux structure related to the MbsData structure.

Returns: err code coming from LU decomposition, negative in case of error

◆ mbs_Mred()

int mbs_Mred	(	MbsAux *	mbs_aux,
		MbsData *	s
	)

compute Mr, the reduced mass matrix [nqu x nqu] obtained after the 2 consecutive reductions (Robotran Basics).

necessary for

the state space representation (computation of A) and then the modal analysis.

Parameters

[in,out]	mbs_aux	the local computation structure, where the result is stored in the variable Mr.
[in,out]	s	the MbsData of the system, where q(iqv),qd(iqv) and qdd(iqv) are updated.

◆ mbs_Rred()

int mbs_Rred	(	MbsAux *	mbs_aux,
		MbsData *	s
	)

compute Rred, the reduced equations of motions on a residual form [nqu x 1] obtained after the 2 consecutive reductions (Robotran Basics).

necessary for

equilibrium (even with non zero acceleration)
linearization of MBS: obtention of Gr Kr around a given configuration (*) [not necessarely an equilibrium]

Parameters

[in,out]	mbs_aux	the local computation structure, where the result Rred is stored.
[in,out]	s	the MbsData of the system, where q(iqv),qd(iqv) and qdd(iqv) are updated.

Returns: Error status, <0 in case of failure.

◆ mbs_Rred_core()

int mbs_Rred_core	(	MbsAux *	mbs_aux,
		MbsData *	s
	)

Compute the residual force on independant joints and required force in driven joints.

This function is the computationnal core of Rred and Invdynared computations. This function assumes that all independent joints and all driven joints are up-to-date (coordinates, velocities and accelerations).

Parameters

[in,out]	mbs_aux	the local computation structure, where the result Rred and Qc are stored.
[in,out]	s	the MbsData of the system, where q[qv], qd[qv], qdd[qv] and Qc are updated.

Returns: Error status, <0 in case of failure.

◆ mbs_step_close_geo()

int mbs_step_close_geo	(	MbsData *	s,
		MbsAux *	mbs_aux
	)

compute an iteration of the position of the multibody system to solve the constraints

The algorithm is one step of a Newton/Raphson procedure which solves the equation: $v^{k+1} = v^{k}-\left(J_{v}\right)^{-1} h$ .
Robotran Basics Eq(17), chapter 3.2.1, pp. 12

Parameters

[in,out]	s	the MbsData structure.
[in,out]	mbs_aux	the MbsAux structure related to the MbsData structure.

Returns: success (0) or error (<0)

Functions

Function Documentation

◆ dirdynared()

◆ invdynared()

◆ mbs_anim_close_geo()

◆ mbs_calc_force()

◆ mbs_calc_hJ()

◆ mbs_calc_jdqd()

◆ mbs_close_geo()

◆ mbs_close_kin()

◆ mbs_comp_LUdcmp_mJv()

◆ mbs_Mred()

◆ mbs_Rred()

◆ mbs_Rred_core()

◆ mbs_step_close_geo()