#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "math.h"
#include "mbs_path.h"
#include "mbs_lmgc.h"
#include "mbs_load_xml.h"
#include "mbs_project_interface.h"
#include "nrfct.h"
#include "mbs_sensor.h"

Macros
#define	SUFFIX_STR_LENGTH 15

Functions
void	nullifyArray (double *a, int size)

void	printArray (char filepath, double time, double a, int size)

void	mbs_lmgc_set_nb_mbs (int nb)
	Set the number of mbs. More...

int	mbs_lmgc_checkMbsIndex (int iMbs)

void	mbs_lmgc_set_filename (int iMbs, char *filenamePtr, char filename)

void	mbs_lmgc_set_mbsdata (int iMbs, MbsData *mbs_data)
	Set mbs_data (already loaded in memory) for the specified mbs. More...

void	mbs_lmgc_set_mbs_file (int iMbs, char *filename)
	Set the path to the mbs file for the specified mbs. More...

void	mbs_lmgc_set_res_file (int iMbs, char *filename)
	Set the path to the result files for the specified mbs. More...

void	mbs_lmgc_set_anim_file (int iMbs, char *filename)
	Set the path to the anim file for the specified mbs. More...

void	mbs_lmgc_free_node_list (int nbNodes, MbsLmgcNode *nodeList)

void	mbs_lmgc_set_node_sensor_ids (int iMbs, int nbNodes, int *sensorIds)
	Set the number of nodes for the specified mbs and set the index of sensors on which the nodes are attached. More...

int	mbs_lmgc_get_node_nb (int iMbs)
	Get the number of nodes attached to a mbs. More...

MbsData *	mbs_lmgc_initialize_data (char *mbsFilename)

void	mbs_lmgc_initialize ()
	Initialize the system. More...

void	mbs_lmgc_finalize ()
	Save final configuration, free the memory and finalize the process. More...

void	mbs_lmgc_increment (double tsim)
	Increment time step. More...

void	mbs_lmgc_compute_free_vlocy (double h, double theta)
	Compute the free velocity of all mbs. More...

void	mbs_lmgc_update_nodes_3D (int i, double coorTT, double localFrameTT, int storage)
	Update the kinematics of 3D nodes belonging to the specified mbs. More...

void	mbs_lmgc_update_nodes_2D (int i, double *coorTT, int storage)
	Update the kinematics of 2D nodes belonging to the specified mbs. More...

void	mbs_lmgc_nullify_reac (int i_mbs)
	Reset the global reaction to zero for the specified mbs. More...

void	mbs_lmgc_nullify_rAux (int i_mbs)
	Reset the auxiliary reaction to zero for the specified mbs. More...

void	mbs_lmgc_add_reaction (int i_mbs, int i_node, double reacNode, double reacGlob)
	Add the given node reaction to the reaction of the global mbs. More...

void	mbs_lmgc_add_reac (int i_mbs, int i_node, double *reacNode)
	Add the given node reaction contribution to the reaction of the global mbs. More...

void	mbs_lmgc_add_rAux (int i_mbs, int i_node, double *reacNode)
	Add the given node reaction to the reaction of the global mbs. More...

void	mbs_lmgc_nullify_vFree (int i_mbs)
	Reset the generalized velocity to zero for the specified mbs. More...

void	mbs_lmgc_nullify_vAux (int i_mbs)
	Reset the generalized velocity to zero for the specified mbs. More...

void	mbs_lmgc_comp_vAux_Iaux (int i_mbs)
	Compute the generalized velocity (vAux),. More...

void	mbs_lmgc_comp_vAux_Ireac (int i_mbs)
	Compute the generalized velocity (vAux) from value stored in Ireac,. More...

void	mbs_lmgc_get_velocity (int i_mbs, int i_node, double vlocyNode, double vlocyGen, double **JRed)
	Compute the velocity of a node (linear and angular velocity) from a given value of the generalized velocities. More...

void	mbs_lmgc_get_vBeg (int i_mbs, int i_node, double *vlocyNode)
	Compute and set the velocity of a node at the beginning of the current time step. More...

void	mbs_lmgc_get_vFree (int i_mbs, int i_node, double *vlocyNode)
	Compute and set the free velocity of a node for the current time step. More...

void	mbs_lmgc_get_vAux (int i_mbs, int i_node, double *vlocyNode)
	Compute and set the auxiliary velocity of a node for the current time step. More...

void	mbs_lmgc_compute_dof (double h, double theta)
	Compute the new value of the generalized velocity and position for all mbs. More...

void	mbs_lmgc_update_dof ()
	Finalize time step. More...

Variables
MbsLmgcSystem *	globalSystem = NULL
	Global pointer to the Robotran-LMGC90 system. More...

Macro Definition Documentation

◆ SUFFIX_STR_LENGTH

#define SUFFIX_STR_LENGTH 15

Function Documentation

◆ mbs_lmgc_add_rAux()

void mbs_lmgc_add_rAux	(	int	i_mbs,
		int	i_node,
		double *	reacNode
	)

Add the given node reaction to the reaction of the global mbs.

The given reaction is thus projected on the generalized coordinates space:

$raux += J_{(sens,red)}^{T} * reacNode$
rAux is stored in MbsLmgcData.
current value of $J_{(sens,red)}$ is used (i.e. value stored MbsLmgcData -> MbsLmgcNode)

The array on which it is added corresponds to LMGC90 flag iRaux_ .

Parameters

i_mbs	the index of the mbs for which the reaction must be applied
i_node	the index of the node on which the reaction is applied
reacNode	a 6 element (fx, fy, fz, tx, ty, tz) array containing the components of the force and torque to apply on the node. Components are given in the global frame.

◆ mbs_lmgc_add_reac()

void mbs_lmgc_add_reac	(	int	i_mbs,
		int	i_node,
		double *	reacNode
	)

Add the given node reaction contribution to the reaction of the global mbs.

The given reaction is thus projected on the generalized coordinates space:

$reac += J_{(sens,red)}^{T} * reacNode$
reac is stored in MbsLmgcData.
current value of $J_{(sens,red)}$ is used (i.e. value stored MbsLmgcData -> MbsLmgcNode)

The array on which it is added corresponds to LMGC90 flag iReac_ .

Parameters

i_mbs	the index of the mbs for which the reaction must be applied
i_node	the index of the node on which the reaction is applied
reacNode	a 6 element (fx, fy, fz, tx, ty, tz) array containing the components of the force and torque to apply on the node. Components are given in the global frame.

◆ mbs_lmgc_add_reaction()

void mbs_lmgc_add_reaction	(	int	i_mbs,
		int	i_node,
		double *	reacNode,
		double *	reacGlob
	)

Add the given node reaction to the reaction of the global mbs.

The given reac is thus projected on the generalized coordinates space: reac_generalized = Jsens^T * reac_node

The array on which it is added is specified by the argument reacGlob

Parameters

i_mbs	the index of the mbs for which the reaction must be applied
i_node	the index of the node on which the reaction is applied
reacNode	a 6 element (fx, fy, fz, tx, ty, tz) array containing the components of the force and torque to apply on the node reference point. Components are given in the global frame.
reacGlob	the pointer to the array on which the reac is added

◆ mbs_lmgc_checkMbsIndex()

int mbs_lmgc_checkMbsIndex ( int iMbs )

◆ mbs_lmgc_comp_vAux_Iaux()

void mbs_lmgc_comp_vAux_Iaux ( int i_mbs )

Compute the generalized velocity (vAux),.

It solve $M_{red} * vAux = rAux$
The array for vAux and rAux corresponds to the LMGC90 flag iVaux_e_invM_t_Raux_

WARNING: this function assumes that the reduced mass matrix $M_{red}$ stored in the MbsAux structure (lds field of the MbsLmgcData of the specified mbs) has already been computed.
vAux will be computed simply by calling cholsl.

Parameters

i_mbs the index of the mbs for which the velocity must be reset

◆ mbs_lmgc_comp_vAux_Ireac()

void mbs_lmgc_comp_vAux_Ireac ( int i_mbs )

Compute the generalized velocity (vAux) from value stored in Ireac,.

It solve $M_{red} * vAux = reac$
The array for vAux and reac corresponds to the LMGC90 flag iVaux_e_invM_t_Ireac

WARNING: this function assumes that the reduced mass matrix $M_{red}$ stored in the MbsAux structure (lds field of the MbsLmgcData of the specified mbs) has already been computed.
vAux will be computed simply by calling cholsl.

Parameters

i_mbs the index of the mbs for which the velocity must be reset

◆ mbs_lmgc_compute_dof()

void mbs_lmgc_compute_dof	(	double	h,
		double	theta
	)

Compute the new value of the generalized velocity and position for all mbs.

The following operation are done:

Compute vAux by solving : $M_{red} * vAux = rAux$
Update the independent velocity with : $qd_{u} = vFree + vAux$
Update the independent velocity with : $q_{u} = qm_{u} + h * \theta * qd_{u}$
Update driven joint position, velocity and acceleration at the value at the end of the time step
Solve constraints (dependent joints) in position and velocity.
Update tsim at the end of step (coherence with coordinate value in memory)

Precondition:

reac contains the new value of reactions

WARNING:

This function modify the vAux array of each system

Parameters

h	the size of the time step
theta	the value of the theta parameter in the theta scheme

◆ mbs_lmgc_compute_free_vlocy()

void mbs_lmgc_compute_free_vlocy	(	double	h,
		double	theta
	)

Compute the free velocity of all mbs.

After this function:

qm is set to the new contact configuration
acceleration (MbsData->qdd) are computed with tsim (begin of time step), qm (contact configuration) and qd (value of begin of time step)
vFree is computed for the current step

Parameters

[in]	h	the size of the time step
[in]	theta	the value of the theta parameter in the theta scheme

◆ mbs_lmgc_finalize()

void mbs_lmgc_finalize ( )

Save final configuration, free the memory and finalize the process.

If the MbsData structure was provided by function mbs_lmgc_set_mbsdata() , the structure is not freed.

◆ mbs_lmgc_free_node_list()

void mbs_lmgc_free_node_list	(	int	nbNodes,
		MbsLmgcNode *	nodeList
	)

◆ mbs_lmgc_get_node_nb()

int mbs_lmgc_get_node_nb ( int iMbs )

Get the number of nodes attached to a mbs.

Parameters

[in] iMbs the ID of the Multibody system associated to the mbs_data.
WARNING: ID's start from 0 !

Returns: the number of nodes attached on the specified mbs.

◆ mbs_lmgc_get_vAux()

void mbs_lmgc_get_vAux	(	int	i_mbs,
		int	i_node,
		double *	vlocyNode
	)

Compute and set the auxiliary velocity of a node for the current time step.

Compute the velocity of a node (linear and angular velocity) from a given value of the generalized velocities.
This is computed by $vlocyNode = J_{(sens,red)} * vAux_{uc}$

The array from which the velocity corresponds to the LMGC90 flag iVaux_.
The corresponding array is vAux stored in MbsLmgcData.

Parameters

i_mbs	the index of the mbs from which the velocity is taken
i_node	the index of the node for which the velocity is computed
vlocyNode	a 6 element (vx, vy, vz, omx, omy, omz) array in which the components of the velocity will be stored. Components are computed and expressed in the global frame.

◆ mbs_lmgc_get_vBeg()

void mbs_lmgc_get_vBeg	(	int	i_mbs,
		int	i_node,
		double *	vlocyNode
	)

Compute and set the velocity of a node at the beginning of the current time step.

Compute the velocity of a node (linear and angular velocity) from a given value of the generalized velocities.
This is computed by $vlocyNode = J_{(sens,red)} * vBeg_{uc}$

The array from which the velocity corresponds to the LMGC90 flag iVbeg_
The corresponding array is vBeg stored in MbsLmgcData.

Parameters

i_mbs	the index of the mbs from which the velocity is taken
i_node	the index of the node for which the velocity is computed
vlocyNode	a 6 element (vx, vy, vz, omx, omy, omz) array in which the components of the velocity will be stored. Components are computed and expressed in the global frame.

◆ mbs_lmgc_get_velocity()

void mbs_lmgc_get_velocity	(	int	i_mbs,
		int	i_node,
		double *	vlocyNode,
		double *	vlocyGen,
		double **	JRed
	)

Compute the velocity of a node (linear and angular velocity) from a given value of the generalized velocities.

This is computed as vlocy_node = Jsens * vlocy_generalized

The array from which the velocity is computed by the argument vlocyGen.

Parameters

i_mbs	the index of the mbs from which the velocity is taken
i_node	the index of the node for which the velocity is computed
vlocyNode	a 6 element (vx, vy, vz, omx, omy, omz) array in which the components of the velocity will be stored. Components are computed in the global frame.
JRed	the reduced jacobian to use
vlocyGen	the array from which the velocity is computed

◆ mbs_lmgc_get_vFree()

void mbs_lmgc_get_vFree	(	int	i_mbs,
		int	i_node,
		double *	vlocyNode
	)

Compute and set the free velocity of a node for the current time step.

Compute the velocity of a node (linear and angular velocity) from a given value of the generalized velocities.
This is computed by $vlocyNode = J_{(sens,red)} * vBeg_{uc}$

The array from which the velocity corresponds to the LMGC90 flag iVfree.
The corresponding array is vFree stored in MbsLmgcData.

Parameters

i_mbs	the index of the mbs from which the velocity is taken
i_node	the index of the node for which the velocity is computed
vlocyNode	a 6 element (vx, vy, vz, omx, omy, omz) array in which the components of the velocity will be stored. Components are computed and expressed in the global frame.

◆ mbs_lmgc_increment()

void mbs_lmgc_increment ( double tsim )

Increment time step.

Set the time at the beginning of the next iteration in the time simulation loop and save the system configuration.

Parameters

tsim	the new value of time at the begining of the time step

◆ mbs_lmgc_initialize()

void mbs_lmgc_initialize ( )

Initialize the system.

◆ mbs_lmgc_initialize_data()

MbsData* mbs_lmgc_initialize_data ( char * mbsFilename )

◆ mbs_lmgc_nullify_rAux()

void mbs_lmgc_nullify_rAux ( int i_mbs )

Reset the auxiliary reaction to zero for the specified mbs.

The vector to be reset corresponds to flag iRaux_ of LMGC90.
The corresponding array is rAux stored in MbsLmgcData.

Parameters

i_mbs the index of the mbs for which the reaction must be reset

◆ mbs_lmgc_nullify_reac()

void mbs_lmgc_nullify_reac ( int i_mbs )

Reset the global reaction to zero for the specified mbs.

The vector to be reset corresponds to flag iReac_ of LMGC90.
The corresponding array is reac stored in MbsLmgcData.

Parameters

i_mbs the index of the mbs for which the reaction must be reset

◆ mbs_lmgc_nullify_vAux()

void mbs_lmgc_nullify_vAux ( int i_mbs )

Reset the generalized velocity to zero for the specified mbs.

The array to be reset corresponds to the LMGC90 flag iVaux_. The corresponding array is vAux stored in MbsLmgcData.

Parameters

i_mbs the index of the mbs for which the velocity must be reset

◆ mbs_lmgc_nullify_vFree()

void mbs_lmgc_nullify_vFree ( int i_mbs )

Reset the generalized velocity to zero for the specified mbs.

The array to be reset corresponds to the LMGC90 flag iVfree. The corresponding array is vFree stored in MbsLmgcData.

Parameters

i_mbs the index of the mbs for which the velocity must be reset

◆ mbs_lmgc_set_anim_file()

void mbs_lmgc_set_anim_file	(	int	iMbs,
		char *	filename
	)

Set the path to the anim file for the specified mbs.

Parameters

[in]	iMbs	the ID of the Multibody system. WARNING: ID's start from 0 !
[in]	filename	the path to the folder where results have to be saved including the prefix of the results files

◆ mbs_lmgc_set_filename()

void mbs_lmgc_set_filename	(	int	iMbs,
		char **	filenamePtr,
		char *	filename
	)

◆ mbs_lmgc_set_mbs_file()

void mbs_lmgc_set_mbs_file	(	int	iMbs,
		char *	filename
	)

Set the path to the mbs file for the specified mbs.

Parameters

[in]	iMbs	the ID of the Multibody system. WARNING: ID's start from 0 !
[in]	filename	the path to the mbs file including the mbs file name and extension (.mbs)

◆ mbs_lmgc_set_mbsdata()

void mbs_lmgc_set_mbsdata	(	int	iMbs,
		MbsData *	mbs_data
	)

Set mbs_data (already loaded in memory) for the specified mbs.

Parameters

[in]	iMbs	the ID of the Multibody system associated to the mbs_data. WARNING: ID's start from 0 !
[in]	mbs_data	the pointer to the multibody system data structure to use

◆ mbs_lmgc_set_nb_mbs()

void mbs_lmgc_set_nb_mbs ( int nb )

Set the number of mbs.

This operation can be run only once in the process

Parameters

[in] nb the number of multibody system in the simulation

◆ mbs_lmgc_set_node_sensor_ids()

void mbs_lmgc_set_node_sensor_ids	(	int	iMbs,
		int	nbNodes,
		int *	sensorIds
	)

Set the number of nodes for the specified mbs and set the index of sensors on which the nodes are attached.

the size of sensorIds must be equal to nbNodes.

Parameters

[in]	iMbs	the ID of the Multibody system. WARNING: ID's start from 0 !
[in]	nbNodes	the number of nodes attached to the multibody system.
[in]	sensorIds	pointer to the lists containing the sensors id's.

◆ mbs_lmgc_set_res_file()

void mbs_lmgc_set_res_file	(	int	iMbs,
		char *	filename
	)

Set the path to the result files for the specified mbs.

It specify the path + the name without extension. A suffix _q.res will be added for the position result file A suffix _qd.res will be added for the velocity result file

Parameters

[in]	iMbs	the ID of the Multibody system. WARNING: ID's start from 0 !
[in]	filename	the path to the folder where results have to be saved including the prefix of the results files

◆ mbs_lmgc_update_dof()

void mbs_lmgc_update_dof ( )

Finalize time step.

Update the vBeg value for next time step.

◆ mbs_lmgc_update_nodes_2D()

void mbs_lmgc_update_nodes_2D	(	int	i_mbs,
		double *	coorTT,
		int	storage
	)

Update the kinematics of 2D nodes belonging to the specified mbs.

For each sensor attached to a nodes of the mbs it:

updates the kinematics (q, qd, qdd, rotation matrix, jacobian matrix);
computes the reduced jacobian of the sensor:
$J_{(sens,red)} = J{uc} + J{v}*Bv_{uc}$

The only axis considered in translation are X and Y; the only rotation is around Z axis.
Current value of q, Jv and $Bv_{uc}$ are used (i.e. value stored in MbsData and MbsAux)

Parameters

i_mbs	the index of the mbs for which the nodes must be updated
coorTT	the array in which the position of nodes must be copied: being the coordinate of all nodes of the mbs
storage	Flag to store the reduced Jacobian: 0: JRedBeg, computed in step initial configuration; 1: JRedQm, computed in step mid-configuration.

◆ mbs_lmgc_update_nodes_3D()

void mbs_lmgc_update_nodes_3D	(	int	i_mbs,
		double *	coorTT,
		double *	localFrameTT,
		int	storage
	)

Update the kinematics of 3D nodes belonging to the specified mbs.

For each sensor attached to a nodes of the mbs it:

updates the kinematics (q, qd, qdd, rotation matrix, jacobian matrix);
computes the reduced jacobian of the sensor:
$J_{(sens,red)} = J{uc} + J{v}*Bv_{uc}$

Current value of q, Jv and $Bv_{uc}$ are used (i.e. value stored in MbsData and MbsAux)

Parameters

i_mbs	the index of the mbs for which the nodes must be updated
coorTT	the array in which the position of nodes must be copied: being the coordinate of all nodes of the mbs
localFrameTT	the array in which the rotation matrix of nodes must be copied
storage	Flag to store the reduced Jacobian: 0: JRedBeg, computed in step initial configuration; 1: JRedQm, computed in step mid-configuration.

◆ nullifyArray()

void nullifyArray	(	double *	a,
		int	size
	)

◆ printArray()

void printArray	(	char *	filepath,
		double	time,
		double *	a,
		int	size
	)

Variable Documentation

◆ globalSystem

MbsLmgcSystem* globalSystem = NULL

Global pointer to the Robotran-LMGC90 system.

Macros

Functions

Variables

Macro Definition Documentation

◆ SUFFIX_STR_LENGTH

Function Documentation

◆ mbs_lmgc_add_rAux()

◆ mbs_lmgc_add_reac()

◆ mbs_lmgc_add_reaction()

◆ mbs_lmgc_checkMbsIndex()

◆ mbs_lmgc_comp_vAux_Iaux()

◆ mbs_lmgc_comp_vAux_Ireac()

◆ mbs_lmgc_compute_dof()

◆ mbs_lmgc_compute_free_vlocy()

◆ mbs_lmgc_finalize()

◆ mbs_lmgc_free_node_list()

◆ mbs_lmgc_get_node_nb()

◆ mbs_lmgc_get_vAux()

◆ mbs_lmgc_get_vBeg()

◆ mbs_lmgc_get_velocity()

◆ mbs_lmgc_get_vFree()

◆ mbs_lmgc_increment()

◆ mbs_lmgc_initialize()

◆ mbs_lmgc_initialize_data()

◆ mbs_lmgc_nullify_rAux()

◆ mbs_lmgc_nullify_reac()

◆ mbs_lmgc_nullify_vAux()

◆ mbs_lmgc_nullify_vFree()

◆ mbs_lmgc_set_anim_file()

◆ mbs_lmgc_set_filename()

◆ mbs_lmgc_set_mbs_file()

◆ mbs_lmgc_set_mbsdata()

◆ mbs_lmgc_set_nb_mbs()

◆ mbs_lmgc_set_node_sensor_ids()

◆ mbs_lmgc_set_res_file()

◆ mbs_lmgc_update_dof()

◆ mbs_lmgc_update_nodes_2D()

◆ mbs_lmgc_update_nodes_3D()

◆ nullifyArray()

◆ printArray()

Variable Documentation

◆ globalSystem