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

Functions
int	dirdynared (MbsAux mbs_aux, MbsData s)
	Compute the joint accelerations of the system in the current configuration. More...

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 the reduced equations of motions on a residual form [nqu x 1]. 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...

int	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...

int	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...

double *	get_ball_force (MbsData mbs_data, MbsSensor sens, int ancestor, int ball_id, double forces_I[3])
	Express the force component of a ball constraint in the inertial frame. More...

double *	get_rod_force (MbsData mbs_data, MbsSensor sens, int rod_id, int body_1_id, int pt_1_id, int body_2_id, int pt_2_id, double forces_I[3])
	Return the forces components of a rod constraints in inertial frame. More...

double *	get_link_force (MbsData mbs_data, MbsSensor sens, int link_id, int body_1_id, int pt_1_id, int body_2_id, int pt_2_id, double *trsh, double forces_I[3])
	Compute the component of the forces in a link force in the inertial frame. More...

Function Documentation

◆ dirdynared()

int dirdynared	(	MbsAux *	mbs_aux,
		MbsData *	s
	)

Compute the joint accelerations of the system in the current configuration.

The current configuration must be considered after the following updates:

Driven joints has been updated at the time in MbsData::time
Dependent joints coordinates and velocities have been updated to solve constraints.
Computation of all user forces (joint, links, external...)

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: MBS_INFO_SUCCESS in case of success, any other value in case of error.

◆ get_ball_force()

double* get_ball_force	(	MbsData *	mbs_data,
		MbsSensor *	sens,
		int	ancestor,
		int	ball_id,
		double	forces_I[3]
	)

Express the force component of a ball constraint in the inertial frame.

The function needs that the Lagrangre's multipliers in MbsData::lambda are already computed. They are computed during the computation of the system dynamic.

Parameters

[in]	mbs_data	Pointer to the multibody model structure.
[out]	sens	Pointer to the sensor available for computation. A new sensor is allocated (and freed) if the provided pointer is NULL.
[in]	ancestor	The index of the first common ancestor to the two ball extremities. The base index is 0.
[in]	ball_id	Index of the ball cut.
[out]	forces_I	The array where to fill the forces components in inertial frame. If NULL a new array is allocated and must be freed by the caller.

Returns: The array with the force expressed in inertial frame. This is forces_I except if NULL was provided. NULL is returned in case of error.

◆ get_link_force()

double* get_link_force	(	MbsData *	mbs_data,
		MbsSensor *	sens,
		int	link_id,
		int	body_1_id,
		int	pt_1_id,
		int	body_2_id,
		int	pt_2_id,
		double *	trsh,
		double	forces_I[3]
	)

Compute the component of the forces in a link force in the inertial frame.

The function needs that the link force amplitude in MbsData::Fl is already computed. It is computed during the computation of the system dynamic. The sign convention of the link forces are currently:

MbsData::Fl[i] > 0: link in traction, trying to get the bodies closer.
MbsData::Fl[i] < 0: link in compression, trying to separate the bodies.

The computed force is applied at the point pt_2_id located on the body body_2_id.

The components of the force applied on the other point are simply the opposite of the returned force. It can also be obtained by inversing the bodies and points indices.

Parameters

[in]	mbs_data	Pointer to the multibody model structure.
[out]	sens	Pointer to the sensor available for computation. A new sensor is allocated (and freed) if the provided pointer is NULL.
[in]	link_id	The index of the link.
[in]	body_1_id	The index of the body with the first extremity of the link. The base is referenced by index 0.
[in]	pt_1_id	The index of the anchor point which is the first extremity of the link force. If the extremity is located on a body origin, the index 0 must be provided.
[in]	body_2_id	The index of the body with the second extremity of the link. The base is referenced by index 0. The returned force component are applied on this body.
[in]	pt_2_id	The index of the anchor point which is the second extremity of the link force. If the extremity is located on a body origin, the index 0 must be provided. The returned force component are applied on this point.
[in]	trsh	Pointer to the minimal link length allowed when computed. A default value of 1e-14 is used if NULL is provided. If the lenght of the rod is lower than this value, NULL is returned.

A pointer to a negative value (ie `trsh[0] < 0) will allow any link length which can lead to division by zero.

Parameters

[out] forces_I The array where to fill the forces components in inertial frame. It is filled with INFINITY if an error occurs. If NULL a new array is allocated and must be freed by the caller.

Returns: The array with the force expressed in inertial frame. This is forces_I except if NULL was provided. NULL is returned in case of error.

◆ get_rod_force()

double* get_rod_force	(	MbsData *	mbs_data,
		MbsSensor *	sens,
		int	rod_id,
		int	body_1_id,
		int	pt_1_id,
		int	body_2_id,
		int	pt_2_id,
		double	forces_I[3]
	)

Return the forces components of a rod constraints in inertial frame.

The function needs that the Lagrangre's multipliers in MbsData::lambda are already computed. They are computed during the computation of the system dynamic. The sign convention of the Lagrange multiplier assiciate to rod are:

MbsData::lambda[i] > 0: rod is in compression;
MbsData::lambda[i] < 0: rod is in traction.

The computed force is applied at the point pt_2_id located on the body body_2_id.

The components of the force applied on the other point are simply the opposite of the returned force. It can also be obtained by inversing the bodies and points indices.

The length of the rod is not required in input as the computation multiply:

the rod direction (unit vector) being [dx, dy, dz] / lenght
the force in the rod being Lagrange * length then the length vanishes.

Parameters

[in]	mbs_data	Pointer to the multibody model structure.
[out]	sens	Pointer to the sensor available for computation. A new sensor is allocated (and freed) if the provided pointer is NULL.
[in]	rod_id	The index of the rod cut.
[in]	body_1_id	The index of the body with the first extremity of the link. The base is referenced by index 0.
[in]	pt_1_id	The index of the anchor point which is the first extremity of the link force. If the extremity is located on a body origin, the index 0 must be provided.
[in]	body_2_id	The index of the body with the second extremity of the link. The base is referenced by index 0. The returned force component are applied on this body.
[in]	pt_2_id	The index of the anchor point which is the second extremity of the link force. If the extremity is located on a body origin, the index 0 must be provided. The returned force component are applied on this point.
[out]	forces_I	The array where to fill the forces components in inertial frame. It is filled with INFINITY if an error occurs. If NULL a new array is allocated and must be freed by the caller.

Returns: The array with the force expressed in inertial frame. This is forces_I except if NULL was provided. NULL is returned in case of error.

◆ 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: MBS_INFO_SUCCESS in case of success, any other value in case of error.

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: MBS_INFO_SUCCESS if everything went well, <0 if an error has occured

◆ mbs_calc_hJ()

int 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.

Returns: MBS_INFO_SUCCESS if everything went well, any other value if an error has occured.

◆ mbs_calc_jdqd()

int 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.

Returns: MBS_INFO_SUCCESS if everything went well, any other value if an error has occured.

◆ 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.

Returns: MBS_INFO_SUCCESS in case of success.

◆ mbs_Rred()

int mbs_Rred	(	MbsAux *	mbs_aux,
		MbsData *	s
	)

Compute the reduced equations of motions on a residual form [nqu x 1].

This form is oobtained after the 2 consecutive reductions (see theory; for example Âµ in Robotran Basics). The reduced form of the equations is necessary for:

equilibrium (even with non zero acceleration);
linearization: obtention of Gr Kr around any given configuration;

This function solves the constraints then call the mbs_Rred_core() function.

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: MBS_INFO_SUCCESS in case of success.

◆ 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.

It requires the invdyna symbolic function.

It requires that all independent joints and all driven joints are up-to-date (coordinates, velocities and accelerations).

It will update the forces applied on the system (internals and externals).

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.

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 (MBS_INFO_SUCCESS) or error.

Functions

Function Documentation

◆ dirdynared()

◆ get_ball_force()

◆ get_link_force()

◆ get_rod_force()

◆ 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()