#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_buffer.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, int status)
	Finalize the equilibrium process. 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,
		int	status
	)

Finalize the equilibrium process.

It writes the buffers, restore fields that were modified for the module (ie. velocities put to 0) and call the user equilibrium finish operations.

Parameters

[in,out]	mbs_equil	The equilibrium module structure.
[in,out]	mbs_data	The multibody system. More than restoring some value, MbsData::DoneEquil is also set to success or failure.
[in]	status	The status of the equilibrium analysis, 0 for success any other value is considered as an error code.

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