#include <math.h>
#include <stdio.h>
#include "mbs_define.h"
#include "mbs_errors_names.h"
#include "mbs_equil.h"
#include "mbs_path.h"
#include "mbs_project_interface.h"
#include "mbs_equil_struct.h"
#include "set_output.h"
#include "MBSfun.h"
#include "mbs_linalg.h"
#include "mbs_message.h"
#include "nrfct.h"
#include "useful_functions.h"
#include "mbs_check.h"
#include "mbs_trigo.h"
#include "mbs_linearipk.h"

Macros
#define	MSG_ERR ">> Equil>> "

#define	MSG_PRE " >> "

Functions
MbsEquil *	mbs_new_equil (MbsData *s)
	Allocate the memory for the Equilibrium Options (but not for all the Equilibrium structure) Initialize the options structure with default options. More...

MbsEquil *	mbs_new_equil_aux (MbsData s, MbsAux mbs_aux)

void	mbs_delete_equil (MbsEquil eq, MbsData s)
	Free the Equilibrium structure (pointers table, Equibrium options, ...) More...

int	mbs_run_equil (MbsEquil eq, MbsData s)
	Run the Equilibrium process. More...

int	mbs_equil_save (MbsEquil eq, MbsData s, int iter)
	Q?? is that necessary. More...

int	mbs_equil_init (MbsEquil eq, MbsData s)
	Initialize the Equilibrium structure variables. More...

int	mbs_equil_loop (MbsEquil eq, MbsData s)
	Process the equibrium : find the equilibrium variables, x that satisfy f(x)= [Fruc ; fxe] = 0 Based on the solvepk algorithm (see Numerics) More...

int	mbs_equil_finish (MbsEquil eq, MbsData s)
	Set equilibrum flag to done in MbsData structure Put the MBSysPAD initial velocities and accelerations back in the MbsData structure offer the possibility for user to finish operation through the use of user_equil_finish. More...

void	mbs_equil_exchange (MbsEquilOptions *options)
	Allocate the memory for the changed equilibrium variables through a table of pointers. More...

void	mbs_equil_set_variable (MbsEquilOptions options, double address, int id_exchanged, int replaced_variable_id)
	set the given pointer to the exchange variable table. More...

void	mbs_equil_add_variable (MbsEquilOptions options, double address, int id_added)
	set the given pointer to the additionnal variable table. More...

void	mbs_equil_addition (MbsEquilOptions *options)
	Allocate the memory for the addition equilibrium variables through a table of pointers. More...

void	mbs_equil_ignorance (MbsEquilOptions *options)
	Allocate the memory for the int table containing the index of the qu variables to ignore ! More...

int	mbs_equil_fct (double x[], int nx, double fx[], MbsEquil eq, MbsAux aux, MbsData *s)
	Update the equilibrium variables (qu, xsub and xe) through the pointers eq->x_ptr Compute the equilibrium vector function f(x) (i.e. More...

int	mbs_equil_fsolvepk (int(fun_ptr)(double , int, double , MbsEquil , MbsAux , MbsData ), MbsEquil eq, MbsAux aux, MbsData *s)
	Solve equation f(x)=0 based on given initial values. More...

int	mbs_equil_grad_lpk (MbsEquil eq, MbsData s)
	Compute the gradient of f(x) for a given configuration based on the lpk parabolic fitting method. More...

int	mbs_equil_grad_dev (MbsEquil eq, MbsData s)
	Compute the gradient of f(x) for a given configuration based on a deviation computation. More...

void	mbs_print_equil (MbsEquil *eq)
	Print the equilibrium structure and its options. More...

Detailed Description

This file implements the functions of the equil module in C.

Creation date: 26/11/2015

Author: Quentin Docquier (based on the work of Aubain Verle and Paul Fisette)

(c) Universite catholique de Louvain

Macro Definition Documentation

◆ MSG_ERR

#define MSG_ERR ">> Equil>> "

◆ MSG_PRE

#define MSG_PRE " >> "

Function Documentation

◆ mbs_delete_equil()

void mbs_delete_equil	(	MbsEquil *	eq,
		MbsData *	mbs_data
	)

Free the Equilibrium structure (pointers table, Equibrium options, ...)

The options (MbsEquilOptions) and MbsAux structures are also freed

Parameters

eq	the MbsEquil to be freed
mbs_data	the MbsData structure of the model for which the equilibrium is deleted

◆ mbs_equil_add_variable()

void mbs_equil_add_variable	(	MbsEquilOptions *	options,
		double *	address,
		int	id_added
	)

set the given pointer to the additionnal variable table.

Parameters

[in,out]	options	Equilibrium options structure (where the pointers table is stored).
[in]	address	The memory adress of the variable to be used as equilibrium variable.
[in]	id_added	number of the current added variable (start at 1)

◆ mbs_equil_addition()

void mbs_equil_addition ( MbsEquilOptions * options )

Allocate the memory for the addition equilibrium variables through a table of pointers.

options Equilibrium options structure (where the pointers table is stored)

◆ mbs_equil_exchange()

void mbs_equil_exchange ( MbsEquilOptions * options )

Allocate the memory for the changed equilibrium variables through a table of pointers.

options Equilibrium options structure (where the pointers table is stored)

◆ mbs_equil_fct()

int mbs_equil_fct	(	double	x[],
		int	nx,
		double	fx[],
		MbsEquil *	eq,
		MbsAux *	aux,
		MbsData *	s
	)

Update the equilibrium variables (qu, xsub and xe) through the pointers eq->x_ptr Compute the equilibrium vector function f(x) (i.e.

the function to set to 0) -1- Extract Fru from Fruc -2- Compute Fxe based on a the user-defined function

x vector containing the equilibrium variables (qu, xsub, xe) nx number of equilibrium variables fx vector containing the equilibrium functions evaluation, f(x) eq ?Q to change for a more general use of function of fsolvepk aux ?Q to change for a more general use of function of fsolvepk s ?Q to change for a more general use of function of fsolvepk

Returns: MBS_INFO_SUCCESS in case of success, any other value in case of error.

◆ mbs_equil_finish()

int mbs_equil_finish	(	MbsEquil *	mbs_equil,
		MbsData *	mbs_data
	)

Set equilibrum flag to done in MbsData structure Put the MBSysPAD initial velocities and accelerations back in the MbsData structure offer the possibility for user to finish operation through the use of user_equil_finish.

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

◆ mbs_equil_fsolvepk()

int mbs_equil_fsolvepk	(	int()(double , int, double , MbsEquil , MbsAux , MbsData )	fun_ptr,
		MbsEquil *	eq,
		MbsAux *	aux,
		MbsData *	s
	)

Solve equation f(x)=0 based on given initial values.

Algorithm developped by Paul Fisette and ...

Parameters

[in]	fun_ptr	pointer to the error function f(x) to be solved.
[in,out]	eq	the MbsEquil to be run.
[in,out]	aux	the MbsAux structure related to the model for which the equilibrium is computed.
[in,out]	s	the MbsData structure of the model for which the equilibrium is computed.

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

< This is the transpose of the gradient matrix as it is provided (via mbs_rank_0()) to LAPACK libraries that uses row major memory arrangement.

◆ mbs_equil_grad_dev()

int mbs_equil_grad_dev	(	MbsEquil *	eq,
		MbsData *	s
	)

Compute the gradient of f(x) for a given configuration based on a deviation computation.

Parameters

[in,out]	eq	the MbsEquil to be run.
[in,out]	s	the MbsData structure of the model for which the equilibrium is computed.

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

◆ mbs_equil_grad_lpk()

int mbs_equil_grad_lpk	(	MbsEquil *	eq,
		MbsData *	s
	)

Compute the gradient of f(x) for a given configuration based on the lpk parabolic fitting method.

Parameters

[in,out]	eq	the MbsEquil to be run.
[in,out]	s	the MbsData structure of the model for which the equilibrium is computed.

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

◆ mbs_equil_ignorance()

void mbs_equil_ignorance ( MbsEquilOptions * options )

Allocate the memory for the int table containing the index of the qu variables to ignore !

options Equilibrium options structure (where the pointers table is stored)

◆ mbs_equil_init()

int mbs_equil_init	(	MbsEquil *	mbs_equil,
		MbsData *	mbs_data
	)

Initialize the Equilibrium structure variables.

The function links the pointers table, x_ptr with:

the independent variables : qu
the substitution variables : xch (if defined by the user)
the extra equilibrium variables : xe (if defined by the user)

It also links the extra equilibrium function pointer, fxe_ptr with the user_equil_fxe (if defined by the user).

Parameters

[in,out]	mbs_equil	the MbsEquil to be run
[in,out]	mbs_data	the MbsData structure of the model for which the equilibrium is computed

Allocate the memory for x_ptr and x based on Equilibrium Options.

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

◆ mbs_equil_loop()

int mbs_equil_loop	(	MbsEquil *	mbs_equil,
		MbsData *	mbs_data
	)

Process the equibrium : find the equilibrium variables, x that satisfy f(x)= [Fruc ; fxe] = 0 Based on the solvepk algorithm (see Numerics)

Parameters

[in,out]	mbs_equil	the MbsEquil to be run.
[in,out]	mbs_data	the MbsData structure of the model for which the equilibrium is computed.

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

◆ mbs_equil_save()

int mbs_equil_save	(	MbsEquil *	eq,
		MbsData *	s,
		int	iter
	)

Q?? is that necessary.

◆ mbs_equil_set_variable()

void mbs_equil_set_variable	(	MbsEquilOptions *	options,
		double *	address,
		int	id_exchanged,
		int	replaced_variable_id
	)

set the given pointer to the exchange variable table.

Parameters

[in,out]	options	Equilibrium options structure (where the pointers table is stored).
[in]	address	The memory adress of the variable to be used as equilibrium variable.
[in]	id_exchanged	number of the exchanged variable (start at 1)
[in]	replaced_variable_id	Index of the equilibrium variable (by default the id of the joint) to be removed and replaced by the variable at the given adress.

◆ mbs_new_equil()

MbsEquil* mbs_new_equil ( MbsData * mbs_data )

Allocate the memory for the Equilibrium Options (but not for all the Equilibrium structure) Initialize the options structure with default options.

Parameters

mbs_data the MbsData structure of the model for which the equilibrium will be computed

Returns: An allocated MbsEquil structure.

◆ mbs_new_equil_aux()

MbsEquil* mbs_new_equil_aux	(	MbsData *	s,
		MbsAux *	mbs_aux
	)

◆ mbs_print_equil()

void mbs_print_equil ( MbsEquil * eq )

Print the equilibrium structure and its options.

eq the Equilibrium structure

◆ mbs_run_equil()

int mbs_run_equil	(	MbsEquil *	mbs_equil,
		MbsData *	mbs_data
	)

Run the Equilibrium process.

The following function are called :

mbs_equil_init
mbs_equil_loop
mbs_equil_finish

The MbsData structure associated is modified (equilibrium configuration, q, qd, Qq and possibly other parameters m,I,dpt, ... )

Parameters

[in,out]	mbs_equil	the MbsEquil to be run
[in,out]	mbs_data	the MbsData structure of the model for which the equilibrium is computed

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

Macros

Functions

Detailed Description

Macro Definition Documentation

◆ MSG_ERR

◆ MSG_PRE

Function Documentation

◆ mbs_delete_equil()

◆ mbs_equil_add_variable()

◆ mbs_equil_addition()

◆ mbs_equil_exchange()

◆ mbs_equil_fct()

◆ mbs_equil_finish()

◆ mbs_equil_fsolvepk()

◆ mbs_equil_grad_dev()

◆ mbs_equil_grad_lpk()

◆ mbs_equil_ignorance()

◆ mbs_equil_init()

◆ mbs_equil_loop()

◆ mbs_equil_save()

◆ mbs_equil_set_variable()

◆ mbs_new_equil()

◆ mbs_new_equil_aux()

◆ mbs_print_equil()

◆ mbs_run_equil()