 |
Robotran C Documentation
|
Go to the documentation of this file.
14 #ifndef __INTEGRATOR_H_INCLUDED__ // guard against multiple/recursive includes
15 #define __INTEGRATOR_H_INCLUDED__
80 int rk4(
double y[],
double dydx[],
int n,
double x,
double h,
double yout[],
109 double *x,
double y[],
double eps,
double hmax,
double htry,
110 long nmax,
double dydx[],
double yscal[],
double *hnext,
MbsData *s,
117 int ThetaSC(
double y[],
double dydx[],
int n,
double x,
double h,
double yout[],
140 int mbs_estim_jac_acc(
double x,
double htry,
double y[],
double dydx[],
int compute_dfdx,
double dfdx[],
double **dfdy,
int n,
168 int mbs_freeze_jac(
int freeze_index,
int *next_freeze_index,
double x,
double h,
double y[],
double dydx[],
169 int compute_dfdx,
double dfdx[],
double **dfdy,
double dydx_freeze[],
double dfdx_freeze[],
double **dfdy_freeze,
190 int euler_implicit(
double y[],
double dydx[],
int n,
double *x,
double h,
double yout[],
211 int w_methods(
double y[],
double dydx[],
int n,
double x,
double h,
double yout[],
void print_warnings_explicit_integrator(MbsDirdyn *mbs_dd, char *integrator_name)
Print the warning message, called by print_warnings_integrator for explicit integrators.
Definition: integrator.c:363
void mbs_msg(const char *msg,...)
Send a message.
Definition: mbs_message.c:87
void print_warnings_integrator(MbsData *mbs_data, MbsDirdyn *mbs_dd, int type_of_integrator)
Check the options set by user to warn him when he modified an unused option for the integrator.
Definition: integrator.c:272
Implementation of eig function using lapacke without using the lapackE interface.
Definition: cholesky.c:14
int loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
Integrate the motion from t0 to tf.
Definition: mbs_bader.c:65
int mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], int compute_dfdx, double dfdx[], double **dfdy, int n, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *dd)
Evaluate the Jacobian of a function accelerations using finite difference.
Definition: integrator.c:35
@ Dopri5
Definition: integrator.h:29
mbs_invdynared_d_ptr mbs_invdynared_dqd
Definition: mbs_data.h:81
This header defines global functions for Euler Implicit integrator in C.
This header defines global functions for W methods integrator in C.
int mbs_eig_0(double **A, int n, double *eval_a, double *eval_b, double **evec_r, double **evec_phi)
Compute the eigen values and vector of a matrix with index starting a 0.
Definition: eig_lapacke.c:29
int nState
size of the state vector
Definition: mbs_dirdyn_struct.h:174
This header defines global functions for Bader integrator in C.
int loop_w_methods(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
Integrate the motion from t0 to tf.
Definition: mbs_w_methods.c:86
int nqu
Number of independent articulations.
Definition: mbs_data.h:262
void mbs_error_msg(int err, const char *msg,...)
Send an error message.
Definition: mbs_message.c:131
double * get_dvec_0(int l_v)
create (with memory allocation) a vector (length l_v) of doubles, starting at index 0
Definition: mbs_1D_array.c:955
double dt_max
maximal time step [s], default = 1.0e-3
Definition: mbs_dirdyn_struct.h:136
@ RK4
Definition: integrator.h:29
#define DIRDYN_INTEGRATOR_OPTION_DEFAULT_dt_max
Definition: mbs_dirdyn_struct.h:31
void print_warnings_constant_step_integrator(MbsDirdyn *mbs_dd, char *integrator_name)
Print the warning message, called by print_warnings_integrator for constant step size dt integrators.
Definition: integrator.c:339
void copy_dmat_0(double **tab_src, double **tab_dest, int nb_r, int nb_c)
Copy the content of a [nb_r times nb_c] matrix of doubles to a second matrix, starting with index 0.
Definition: mbs_matrix.c:402
int nmax
maximal number of steps [-], default = 1e9
Definition: mbs_dirdyn_struct.h:132
double ** G
Damping matrix of size [njoint+1, njoint+1].
Definition: mbs_data.h:201
#define DIRDYN_INTEGRATOR_OPTION_DEFAULT_atoler
Definition: mbs_dirdyn_struct.h:30
@ WMethods
Definition: integrator.h:29
void free_dmat_0(double **mat)
release memory for a matrix of doubles, starting at index 0
Definition: mbs_matrix.c:332
#define DIRDYN_INTEGRATOR_OPTION_DEFAULT_toler
Definition: mbs_dirdyn_struct.h:28
void mbs_warning_msg(const char *msg,...)
Send a warning message.
Definition: mbs_message.c:100
int mbs_cholsl_0(double **A, int n, double **B, int nb)
Solves a symmetric positive definite system of linear equations AX=B (B being the right hand side wit...
Definition: cholesky.c:40
int Nux
Number of user variable.
Definition: mbs_data.h:387
This header defines global functions for rosenbrock integrator in C.
int loop_rosenbrock(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
Integrate the motion from t0 to tf.
Definition: mbs_rosenbrock.c:74
int mbs_freeze_jac(int freeze_index, int *next_freeze_index, double x, double h, double y[], double dydx[], int compute_dfdx, double dfdx[], double **dfdy, double dydx_freeze[], double dfdx_freeze[], double **dfdy_freeze, int n, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *mbs_dd)
Freeze the Jacobian of an integrator structure until mbs_dd->options->n_freeze.
Definition: integrator.c:129
MbsDp * mbs_dp
Structure to store the matrix of tangent analysis (symbolic).
Definition: mbs_data.h:416
Definition: cholesky.c:15
@ AlphaM
Definition: integrator.h:29
float x
Definition: cholesky.c:14
int choldc_0(double **a, int n, double *p)
Definition: choldc.c:47
int loop_euler_implicit(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
Integrate the motion from t0 to tf.
Definition: mbs_euler_implicit.c:65
#define DIRDYN_INTEGRATOR_OPTION_DEFAULT_rtoler
Definition: mbs_dirdyn_struct.h:29
finish_integrator_ptr finish_integrator
pointer to integrator closing function
Definition: mbs_dirdyn_struct.h:211
int mbs_freeze_jac(int freeze_index, int *next_freeze_index, double x, double h, double y[], double dydx[], int compute_dfdx, double dfdx[], double **dfdy, double dydx_freeze[], double dfdx_freeze[], double **dfdy_freeze, int n, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *mbs_dd)
Freeze the Jacobian of an integrator structure until mbs_dd->options->n_freeze.
Definition: integrator.c:129
double ** get_dmat_0(int nb_r, int nb_c)
create (with memory allocation) a [nb_r times nb_c] matrix of doubles, starting at index 0
Definition: mbs_matrix.c:277
double dt0
initial value of the integration step size, default = 0.001
Definition: mbs_dirdyn_struct.h:45
int rk4(double y[], double dydx[], int n, double x, double h, double yout[], int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *dd)
Runge Kutta 4 integrator implementation Given values for the variables y[1..n] and their derivatives ...
Definition: rk4.c:22
void finish_euler_implicit(MbsData *mbs_data, MbsDirdyn *dd)
Finalize and clear memory allocated for Euler Implicit integrator.
Definition: mbs_euler_implicit.c:140
int loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
Integrate the motion from t0 to tf with a custom integrator implemented by the user.
Definition: mbs_custom.c:45
int loop_eulerEx(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
Integrate the motion from t0 to tf.
Definition: mbs_euler_explicit.c:64
int mbs_choldc_0(double **A, int n)
Compute the Cholesky decomposition of A, i.e.
Definition: cholesky.c:25
double tsim
current simulation time
Definition: mbs_dirdyn_struct.h:171
void initialize_dopri5(MbsData *mbs_data, MbsDirdyn *mbs_dd, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), void(*loop_fct)(MbsData *, MbsDirdyn *), int(*save_realtime_update)(MbsDirdyn *dd, MbsData *mbs_data))
Initialize all needed fields in MbsDirdyn for Dopri5 integrator.
Definition: mbs_dopri5.c:23
@ Bader
Definition: integrator.h:29
void finish_dopri5(MbsData *mbs_data, MbsDirdyn *mbs_dd)
Finalize and clear memory allocated for dopri5 integrator.
Definition: mbs_dopri5.c:71
void initialize_bader(MbsData *mbs_data, MbsDirdyn *mbs_dd, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), void(*loop_fct)(MbsData *, MbsDirdyn *), int(*save_realtime_update)(MbsDirdyn *dd, MbsData *mbs_data))
Initialize all needed fields in MbsDirdyn for Bader integrator.
Definition: mbs_bader.c:23
int flag_symbolic_jacobian
1 to use symbolic computation of the jacobian. Only used with implicit integrators....
Definition: mbs_dirdyn_struct.h:141
@ EulerIm
Definition: integrator.h:29
@ Rosenbrock
Definition: integrator.h:29
float y
Definition: cholesky.c:14
int n_freeze
number of time step when the jacobian is freezed (computed once at the start of the n_freeze time ste...
Definition: mbs_dirdyn_struct.h:140
double y
Definition: cholesky.c:15
void finish_bader(MbsData *mbs_data, MbsDirdyn *dd)
Finalize and clear memory allocated for Bader integrator.
Definition: mbs_bader.c:165
double toler
mixed error tolerances [-], default = 1.0e-3
Definition: mbs_dirdyn_struct.h:133
void finish_eulerEx(MbsData *mbs_data, MbsDirdyn *dd)
Finalize and clear memory allocated for Euler Explicit integrator.
Definition: mbs_euler_explicit.c:128
int flag_oneshot
1 to compute the derivative function only once (no time integration !), default = 0 The time is the v...
Definition: mbs_dirdyn_struct.h:92
void print_warnings_constant_step_integrator(MbsDirdyn *mbs_dd, char *integrator_name)
Print the warning message, called by print_warnings_integrator for constant step size dt integrators.
Definition: integrator.c:339
Functions fct
Structure to store the pointer to the project-specific functions.
Definition: mbs_data.h:414
void finish_rosenbrock(MbsData *mbs_data, MbsDirdyn *dd)
Finalize and clear memory allocated for Rosenbrock integrator.
Definition: mbs_rosenbrock.c:158
void transpose_dmat_0(double **tab_src, double **tab_dest, int nb_r, int nb_c)
Transpose a [nb_r times nb_c] matrix of doubles and store the results in a [y times nb_r] matrix,...
Definition: mbs_matrix.c:482
void initialize_euler_implicit(MbsData *mbs_data, MbsDirdyn *mbs_dd, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), void(*loop_fct)(MbsData *, MbsDirdyn *), int(*save_realtime_update)(MbsDirdyn *dd, MbsData *mbs_data))
Initialize all needed fields in MbsDirdyn for Euler Implicit integrator.
Definition: mbs_euler_implicit.c:26
void initialize_w_methods(MbsData *mbs_data, MbsDirdyn *mbs_dd, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), void(*loop_fct)(MbsData *, MbsDirdyn *), int(*save_realtime_update)(MbsDirdyn *dd, MbsData *mbs_data))
Initialize all needed fields in MbsDirdyn for W Methods integrator.
Definition: mbs_w_methods.c:27
This header defines global functions for Euler Explicit integrator in C.
double ** K
Stiffness matrix of size [njoint+1, njoint+1].
Definition: mbs_data.h:200
#define _MBS_ERR_LOW_EIG_VAL
Low Level error number Eigen value problem = -4.
Definition: mbs_errors_names.h:132
int ThetaSC(double y[], double dydx[], int n, double x, double h, double yout[], int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *dd)
Unknown integrator.
Definition: thetaSC.c:25
double x
Definition: cholesky.c:15
Definition: mbs_data.h:246
void initialize_eulerEx(MbsData *mbs_data, MbsDirdyn *mbs_dd, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), void(*loop_fct)(MbsData *, MbsDirdyn *), int(*save_realtime_update)(MbsDirdyn *dd, MbsData *mbs_data))
Initialize all needed fields in MbsDirdyn for Euler Explicit integrator.
Definition: mbs_euler_explicit.c:23
double rtoler
relative error tolerances [-], default = 1.0e-3
Definition: mbs_dirdyn_struct.h:134
@ Custom
Definition: integrator.h:29
void set_integrator(MbsDirdyn *dd)
Set the function pointer in MbsDirdyn.
Definition: integrator.c:173
Structure defining the option of a direct dynamic.
Definition: mbs_dirdyn_struct.h:38
#define MBS_VERBOSE_WARNING
Definition: mbs_define.h:45
void cholsl_0(double **a, int n, double *p, double *b, double *x)
Definition: cholsl.c:25
void set_integrator(MbsDirdyn *mbs_dd)
Set the function pointer in MbsDirdyn.
Definition: integrator.c:173
@ Eulaire
Definition: integrator.h:29
int mbs_eig_0(double **A, int n, double *eval_a, double *eval_b, double **evec_a, double **evec_b)
Compute the eigen values and vector of a matrix with index starting a 0.
Definition: eig_lapack.c:32
void print_warnings_explicit_integrator(MbsDirdyn *mbs_dd, char *integrator_name)
Print the warning message, called by print_warnings_integrator for explicit integrators.
Definition: integrator.c:363
This header defines specific integrators functions in C.
int w_methods(double y[], double dydx[], int n, double x, double h, double yout[], int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *mbs_dd)
W Methods integrator implementation.
Definition: w_methods.c:25
initialize_integrator_ptr initialize_integrator
pointer to integrator initialize function
Definition: mbs_dirdyn_struct.h:207
int integrator
Set integrator to use, available value: RK4, Dopri5, Rosenbrock, EulerEx, Eulaire,...
Definition: mbs_dirdyn_struct.h:109
#define DIRDYN_INTEGRATOR_OPTION_DEFAULT_nmax
Define of default options for integrator in DirdynStruc.
Definition: mbs_dirdyn_struct.h:27
void initialize_rosenbrock(MbsData *mbs_data, MbsDirdyn *mbs_dd, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), void(*loop_fct)(MbsData *, MbsDirdyn *), int(*save_realtime_update)(MbsDirdyn *dd, MbsData *mbs_data))
Initialize all needed fields in MbsDirdyn for Rosenbrock integrator.
Definition: mbs_rosenbrock.c:31
void cholsl(double **a, int n, double p[], double b[], double x[])
Definition: cholsl.c:10
This header defines global functions for custom integrator in C. Advanced user can modify this file.
void print_warnings_integrator(MbsData *mbs_data, MbsDirdyn *mbs_dd, int type_of_integrator)
Check the options set by user to warn him when he modified an unused option for the integrator.
Definition: integrator.c:272
int choldc(double **a, int n, double p[])
Compute the Cholesky decomposition of a, i.e.
Definition: choldc.c:25
int verbose
Verbosity level propagated to other compatible module:
Definition: mbs_dirdyn_struct.h:111
void copy_dvec_0(double *vec_src, double *vec_dest, int l_v)
Copy the content of a vector (of sizel_v) of doubles to a second vector, starting with index 0.
Definition: mbs_1D_array.c:981
void free_dvec_0(double *vec)
release memory for a vector of doubles, starting at index 0
Definition: mbs_1D_array.c:971
int mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], int compute_dfdx, double dfdx[], double **dfdy, int n, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *dd)
Evaluate the Jacobian of a function accelerations using finite difference.
Definition: integrator.c:35
void finish_rk4(MbsData *mbs_data, MbsDirdyn *dd)
Finalize and clear memory allocated for RK4 integrator.
Definition: mbs_rk4.c:121
struct Functions::@4 symb
double atoler
absolute error tolerances [-], default = 1.0e-6
Definition: mbs_dirdyn_struct.h:135
General structure of the direct dynamic module.
Definition: mbs_dirdyn_struct.h:166
int flag_waypoint
1 to use waypoints, 0 otherwise, default =0
Definition: mbs_dirdyn_struct.h:127
int euler_implicit(double y[], double dydx[], int n, double *x, double h, double yout[], int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *mbs_dd)
Euler Implicit integrator implementation.
Definition: euler_implicit.c:25
This header defines global functions for Runge Kutta 4 integrator in C.
void initialize_custom(MbsData *mbs_data, MbsDirdyn *mbs_dd, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), void(*loop_fct)(MbsData *, MbsDirdyn *), int(*save_realtime_update)(MbsDirdyn *dd, MbsData *mbs_data))
Initialize all needed fields in MbsDirdyn for Custom integrator.
Definition: mbs_custom.c:24
int loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
Integrate the motion from t0 to tf.
Definition: mbs_dopri5.c:50
loop_integrator_ptr loop_integrator
pointer to integrator looping function
Definition: mbs_dirdyn_struct.h:209
void initialize_rk4(MbsData *mbs_data, MbsDirdyn *mbs_dd, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), void(*loop_fct)(MbsData *, MbsDirdyn *), int(*save_realtime_update)(MbsDirdyn *dd, MbsData *mbs_data))
Initialize all needed fields in MbsDirdyn for RK4 integrator.
Definition: mbs_rk4.c:24
MbsDirdynOptions * options
structure defining the option of a direct dynamic
Definition: mbs_dirdyn_struct.h:168
void finish_custom(MbsData *mbs_data, MbsDirdyn *mbs_dd)
Finalize and clear memory allocated for Custom integrator.
Definition: mbs_custom.c:122
mbs_invdynared_d_ptr mbs_invdynared_dq
Definition: mbs_data.h:80
int flag_ongoing_jac_computation
flag is ON (=1) when the implicit integrator is currently computing the jacobian, default is 0.
Definition: mbs_dirdyn_struct.h:201
void finish_w_methods(MbsData *mbs_data, MbsDirdyn *dd)
Finalize and clear memory allocated for W Methods integrator.
Definition: mbs_w_methods.c:165
int loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
Integrate the motion from t0 to tf.
Definition: mbs_rk4.c:43
#define _MBS_ERR_LOW_LAPACKE
Low Level error number problem with Lapacke system resolution = -5.
Definition: mbs_errors_names.h:137
@ EulerEx
Definition: integrator.h:29
int rosenbrock(int n, int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), double *x, double y[], double eps, double hmax, double htry, long nmax, double dydx[], double yscal[], double *hnext, MbsData *s, MbsDirdyn *dd, double *hdid)
Fourth-order Rosenbrock step for integrating stiff problems, with monitoring of local truncation erro...
Definition: rosenbrock.c:105
1.8.17