mbs_close_loops.c File Reference

#include "mbs_errors_names.h"
#include "MBSfun.h"
#include "nrfct.h"
#include "mbs_project_interface.h"
#include "mbs_message.h"
#include "mbs_solvekin.h"
#include "mbs_trigo.h"

Functions
double	norm_vector (double *v, int n)
double	norminf_vector (double *v, int n)
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 ( ). More...
void	mbs_calc_jdqd (MbsData *s, MbsAux *mbs_aux)
	compute the quadratic term of the constraints at acceleration level: More...

Function Documentation

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

void mbs_calc_hJ	(	MbsData *	s,
		MbsAux *	mbs_aux
	)

compute the current value of the constraints ( ) and the constraint Jacobian matrix ( ).

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:

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: .
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: .
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_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: .
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)

norm_vector()

double norm_vector	(	double *	v,
		int	n
	)

norminf_vector()

double norminf_vector	(	double *	v,
		int	n
	)