#include <float.h>
#include <string.h>
#include <errno.h>
#include "mbs_data.h"
#include "mbs_define.h"
#include "mbs_path.h"
#include "mbs_sensor_utilities.h"
#include "mbs_trigo.h"
#include "mbs_matrix.h"
#include "mbs_message.h"
#include "mbs_1D_array.h"
#include "useful_functions.h"

Macros
#define	MSG_ERR ">> MBSDATA>> "

#define	MSG_PRE ">> MBSDATA>> "

Functions
static void	mbs_print_diff_dvec_1 (const char prefix, double v1, double *v2, int size)

static void	mbs_print_diff_dmat_1_xyz (const char prefix, double m1, double *m2, int nc)

static void	mbs_print_diff_dmat_1_inertia (const char prefix, double m1, double *m2, int nc)

void	mbs_print_data (MbsData *s)

int	mbs_print_qx (MbsDataUtils data_utils, int nqx, int qx, const char prefix, FILE fs)
	Non-exposed function (outside header) to print joints names. More...

int	mbs_print_qu (MbsData s, FILE fs)
	Print the names of the independent joints. More...

int	mbs_print_qv (MbsData s, FILE fs)
	Print the names of the dependent joints. More...

int	mbs_print_qc (MbsData s, FILE fs)
	Print the names of the driven and locked joints. More...

int	mbs_print_qdriven (MbsData s, FILE fs)
	Print the names of the driven (not locked) joints. More...

int	mbs_print_qlocked (MbsData s, FILE fs)
	Print the names of the locked (not driven) joints. More...

void	mbs_copy_data (MbsData s, MbsData s_copy)
	Copy all fields between two MBSdata structures. More...

void	mbs_reset_joints_kine (MbsData *s)
	Reset joints kinematics (q, qd, qdd) of a MbsData structure to initial values. More...

void	mbs_reset_data (MbsData *s)
	Reset all fields of a MbsData structure to initial values. More...

int	mbs_write_data (MbsData s, char filename, char *function_name)
	Write a source with a function allocating and setting a MbsData copy of the provided. More...

int	mbs_compare_data (MbsData s1, MbsData s2, int accelred)
	Compare two MbsData. More...

double *	mbs_data_get_IO (MbsData mbs, const char io_name, int in_out)
	Return the adress of the specified User IO. More...

double *	dpt_to_dvec_3 (MbsData *mbs_data, int point, double coord[3])
	Retrieve the anchor point coordinate and set them in an array. More...

double *	dpt_to_dvec_4 (MbsData *mbs_data, int point, double coord[4])
	Retrieve the anchor point coordinate and set them in an array. More...

double *	dpt_absolute_to_dvec3 (MbsData mbs_data, MbsSensor sens, int body_id, int point_id, double coord_I[3])
	Compute the absolute coordinate of an anchor point from the base origin. More...

int	dvec_3_to_dpt (MbsData *mbs, int point_id, double coord[3])
	Set the coordinate in the mbs anchor point. More...

int	dvec_4_to_dpt (MbsData *mbs, int point_id, double coord[4])
	Set the coordinate in the mbs anchor point. More...

Macro Definition Documentation

◆ MSG_ERR

#define MSG_ERR ">> MBSDATA>> "

◆ MSG_PRE

#define MSG_PRE ">> MBSDATA>> "

Function Documentation

◆ dpt_absolute_to_dvec3()

double* dpt_absolute_to_dvec3	(	MbsData *	mbs_data,
		MbsSensor *	sens,
		int	body_id,
		int	point_id,
		double	coord_I[3]
	)

Compute the absolute coordinate of an anchor point from the base origin.

Parameters

[in]	mbs_data	The multibody system.
[in,out]	sens	The sensor memory to be used for computation (fields will be updated). NULL can be provided.
[in]	body_id	The body index which is also the parent joint index in MbsData::njoint. The base index is 0. In case of invalid value (lower than 0, higher than MbsData:npt), NULL is returned.
[in]	point_id	The point index in MbsData::dpt. The origin of any body can be reffered with 0. In case of invalid value (lower than 0, higher than MbsData:npt), NULL is returned.

It is not checked that the anchor point is located in the specified body.

Parameters

[in,out] coord_I Pointer to store the coordinates. If NULL is provided, the memory is allocated by the function and must be freed by the caller.

Returns: The pointer to the array containing the coordinates. This is coord except if NULL was provided. If NULL is returned, an error has occured.

◆ dpt_to_dvec_3()

double* dpt_to_dvec_3	(	MbsData *	mbs_data,
		int	point,
		double	coord[3]
	)

Retrieve the anchor point coordinate and set them in an array.

The size of the array is 3.

The anchor point coordinates are expressed in its body frame with respect to the body origin point.

Parameters

[in]	mbs_data	The multibody system.
[in]	point	The point index in MbsData::dpt. The base origin index is 0. In case of invalid value (lower than 0, higher than MbsData:npt), NULL is returned.
[out]	coord	Pointer to store the coordinates. If NULL is provided, the memory is allocated by the function and must be freed by the caller.

Returns: The pointer to the array containing the coordinates. This is coord except if NULL was provided. If NULL is returned, an error has occured.

◆ dpt_to_dvec_4()

double* dpt_to_dvec_4	(	MbsData *	mbs_data,
		int	point,
		double	coord[4]
	)

Retrieve the anchor point coordinate and set them in an array.

The size of the array is 4, but index 0 will not be used.

The anchor point coordinates are expressed in its body frame with respect to the body origin point.

Parameters

[in]	mbs_data	The multibody system.
[in]	point	The point index in MbsData::dpt. The base origin index is 0. In case of invalid value (lower than 0, higher than MbsData:npt), NULL is returned.
[out]	coord	Pointer to store the coordinates. If NULL is provided, the memory is allocated by the function and must be freed by the caller.

Returns: The pointer to the array containing the coordinates. This is coord except if NULL was provided. If NULL is returned, an error has occured.

◆ dvec_3_to_dpt()

int dvec_3_to_dpt	(	MbsData *	mbs,
		int	point_id,
		double	coord[3]
	)

Set the coordinate in the mbs anchor point.

The size of the array is 3.

The anchor point coordinates must be expressed in its body frame with respect to the body origin point.

Parameters

[out]	mbs	mbs_data The multibody system, MbsData::dpt will be affected.
[in]	point_id	The point index in MbsData::dpt. In case of invalid value (lower than 1, higher than MbsData:npt), NULL is returned.
[in]	coord	The coordinate to be set as [x, y, z].

Returns: MBS_INFO_SUCCESS or MBS_INFO_FAILURE (macro in mbs_define.h).

◆ dvec_4_to_dpt()

int dvec_4_to_dpt	(	MbsData *	mbs,
		int	point_id,
		double	coord[4]
	)

Set the coordinate in the mbs anchor point.

The size of the array is 4, but index 0 will not be used.

The anchor point coordinates must be expressed in its body frame with respect to the body origin point.

Parameters

[out]	mbs	mbs_data The multibody system, MbsData::dpt will be affected.
[in]	point_id	The point index in MbsData::dpt. In case of invalid value (lower than 1, higher than MbsData:npt), NULL is returned.
[in]	coord	The coordinate to be set as [#, x, y, z].

Returns: MBS_INFO_SUCCESS or MBS_INFO_FAILURE (macro in mbs_define.h).

◆ mbs_compare_data()

int mbs_compare_data	(	MbsData *	s1,
		MbsData *	s2,
		int	accelred
	)

Compare two MbsData.

The following fields are not compared:

Other fields may be ommited.

The user model parameters are not compared.

If using "accelred" activate the flag accelred (non-null value). Otherwhise deactivate it (null value). The field specifiec to accelered (MbsData::jac_user and MbsData::jdqd_user) are allocated with size MbsData::nqv at file loading. When not using "accelred" this value can be changed without reallocating the fields.

Parameters

[in]	s1	The first multibody system.
[in]	s2	The second multibody system
[in]	accelred	Flag to compare specified field for accelred usage (user jacobian and jdqd fields). Should be set to False by default.

Returns: 0 if same system, -1 otherwhise

◆ mbs_copy_data()

void mbs_copy_data	(	MbsData *	s,
		MbsData *	s_copy
	)

Copy all fields between two MBSdata structures.

WARNING: This function is not updated daily with new functionnalities of MbsData. Use with care.

The function does not modify the followings:

user models
user IOs
multibody filename
build path
project path
project name
pointers to project functions
pointer to realtime structure

Parameters

[in]	s	Pointer to the multibody source.
[out]	s_copy	Pointer to the multibody output.

Returns

◆ mbs_data_get_IO()

double* mbs_data_get_IO	(	MbsData *	mbs,
		const char *	io_name,
		int	in_out
	)

Return the adress of the specified User IO.

The function set errno in case of error. No matches is not considered as an error.

Parameters

[in]	mbs	Pointer to the multibody structure.
[in]	io_name	Name of the user input or output to find.
[in]	in_out	Flag to determine if the requested user IO is an input, or or output: +1: user Input only; -1: user Output only; 0: Not specified, seach in inputs then output.

Returns: The adress of the double containing the user IO. NULL in case of error or if not found.

◆ mbs_print_data()

void mbs_print_data ( MbsData * s )

◆ mbs_print_diff_dmat_1_inertia()

static void mbs_print_diff_dmat_1_inertia	(	const char *	prefix,
		double **	m1,
		double **	m2,
		int	nc
	)

static

◆ mbs_print_diff_dmat_1_xyz()

static void mbs_print_diff_dmat_1_xyz	(	const char *	prefix,
		double **	m1,
		double **	m2,
		int	nc
	)

static

◆ mbs_print_diff_dvec_1()

static void mbs_print_diff_dvec_1	(	const char *	prefix,
		double *	v1,
		double *	v2,
		int	size
	)

static

◆ mbs_print_qc()

int mbs_print_qc	(	MbsData *	s,
		FILE *	fs
	)

Print the names of the driven and locked joints.

Parameters

[in]	s	The multibody system.
[in]	fs	Output pointer, provinding NULL uses stdout.

Returns: Status:

MBS_INFO_FAILURE: Invalid argument or contents in MbsData
MBS_INFO_WARNING: No related joints
MBS_INFO_SUCCESS: printed.

◆ mbs_print_qdriven()

int mbs_print_qdriven	(	MbsData *	s,
		FILE *	fs
	)

Print the names of the driven (not locked) joints.

Parameters

[in]	s	The multibody system.
[in]	fs	Output pointer, provinding NULL uses stdout.

Returns: Status:

MBS_INFO_FAILURE: Invalid argument or contents in MbsData
MBS_INFO_WARNING: No related joints
MBS_INFO_SUCCESS: printed.

◆ mbs_print_qlocked()

int mbs_print_qlocked	(	MbsData *	s,
		FILE *	fs
	)

Print the names of the locked (not driven) joints.

Parameters

[in]	s	The multibody system.
[in]	fs	Output pointer, provinding NULL uses stdout.

Returns: Status:

MBS_INFO_FAILURE: Invalid argument or contents in MbsData
MBS_INFO_WARNING: No related joints
MBS_INFO_SUCCESS: printed.

◆ mbs_print_qu()

int mbs_print_qu	(	MbsData *	s,
		FILE *	fs
	)

Print the names of the independent joints.

Parameters

[in]	s	The multibody system.
[in]	fs	Output pointer, provinding NULL uses stdout.

Returns: Status:

MBS_INFO_FAILURE: Invalid argument or contents in MbsData
MBS_INFO_WARNING: No related joints
MBS_INFO_SUCCESS: printed.

◆ mbs_print_qv()

int mbs_print_qv	(	MbsData *	s,
		FILE *	fs
	)

Print the names of the dependent joints.

Parameters

[in]	s	The multibody system.
[in]	fs	Output pointer, provinding NULL uses stdout.

Returns: Status:

MBS_INFO_FAILURE: Invalid argument or contents in MbsData
MBS_INFO_WARNING: No related joints
MBS_INFO_SUCCESS: printed.

◆ mbs_print_qx()

int mbs_print_qx	(	MbsDataUtils *	data_utils,
		int	nqx,
		int *	qx,
		const char *	prefix,
		FILE *	fs
	)

Non-exposed function (outside header) to print joints names.

This function is used to print the index of joints and the corresponding names. It was writtent to avoid to copy paste the code in multiple function: mbs_print_qu(), mbs_print_qc() ...

It is not exposed to the user as it is a bit tricky to use.

Parameters

[in]	data_utils	The multibody system utilities structure. It contains the pointer to the array of the joint names: MbsDataUtils::joint_names
[in]	nqx	Number of element in `qx`. It print the element from 1 to `nqx` included.
[in]	qx	Pointer to the joint indices. The first index read is `qx[1]` and the last index is `nqx:` `qx[nqx]`.
[in]	fs	Output stream pointer.

Returns: Status:

MBS_INFO_FAILURE: Joint names not filled in data_utils.
MBS_INFO_SUCCESS: printed.

◆ mbs_reset_data()

void mbs_reset_data ( MbsData * s )

Reset all fields of a MbsData structure to initial values.

Parameters

[in,out] s the MbsData structure to reset

◆ mbs_reset_joints_kine()

void mbs_reset_joints_kine ( MbsData * s )

Reset joints kinematics (q, qd, qdd) of a MbsData structure to initial values.

Parameters

[in,out] s the MbsData structure to reset

◆ mbs_write_data()

int mbs_write_data	(	MbsData *	mbs,
		char *	filename,
		char *	function_name
	)

Write a source with a function allocating and setting a MbsData copy of the provided.

Parameters

[in]	mbs	The MbsData structure to be set in the function.
[in]	filename	The name of the source file to be created.
[in]	function_name	The name of the function creating the new MbsData. NULL can be provided, in such a case the name will be "get_mbs_data".

Returns: Negative number in case of error, 0 otherwhise.

Macros

Functions

Macro Definition Documentation

◆ MSG_ERR

◆ MSG_PRE

Function Documentation

◆ dpt_absolute_to_dvec3()

◆ dpt_to_dvec_3()

◆ dpt_to_dvec_4()

◆ dvec_3_to_dpt()

◆ dvec_4_to_dpt()

◆ mbs_compare_data()

◆ mbs_copy_data()

◆ mbs_data_get_IO()

◆ mbs_print_data()

◆ mbs_print_diff_dmat_1_inertia()

◆ mbs_print_diff_dmat_1_xyz()

◆ mbs_print_diff_dvec_1()

◆ mbs_print_qc()

◆ mbs_print_qdriven()

◆ mbs_print_qlocked()

◆ mbs_print_qu()

◆ mbs_print_qv()

◆ mbs_print_qx()

◆ mbs_reset_data()

◆ mbs_reset_joints_kine()

◆ mbs_write_data()