Commit 95893499 authored by Timothee Habra's avatar Timothee Habra
Browse files

use function pointer to remove dependency of mbs module to project

parent c577e89b
/**
*
* Author: Nicolas Docquier
* Creation date: 14/11/2011
*
* (c) Universite catholique de Louvain
*
* To turn this file as a C++ file, just change its extension to .cc (or .cpp).
* If you plan to use some C++ files, it is usually better that the main is compiled as a C++ function.
* Currently, most compilers do not require this, but it is a safer approach to port your code to other computers.
*/
#include <stdio.h>
#ifdef __cplusplus
extern "C" {
#endif
#include "mbs_part.h"
#include "mbs_load_xml.h"
#include "mbs_dirdyn.h"
#include "mbs_data.h"
#include "realtime.h"
#include "cmake_config.h"
#ifdef __cplusplus
}
#endif
int main(int argc, char const *argv[])
{
MbsData *mbs_data;
MbsPart *mbs_part;
MbsDirdyn *mbs_dirdyn;
printf("Hello MBS!\n");
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* LOADING *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
printf("Loading the PendulumSpring data file !\n");
mbs_data = mbs_load(PROJECT_SOURCE_DIR"/../dataR/PendulumSpringC.mbs");
printf("*.mbs file loaded!\n");
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* COORDINATE PARTITIONING *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_part = mbs_new_part(mbs_data);
mbs_part->options->rowperm=1;
mbs_part->options->verbose = 1;
mbs_run_part(mbs_part, mbs_data);
mbs_delete_part(mbs_part);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* DIRECT DYNANMICS *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_dirdyn = mbs_new_dirdyn(mbs_data);
// dirdyn options (see documentations for additional options)
mbs_dirdyn->options->dt0 = 1e-3;
mbs_dirdyn->options->tf = 10.0;
mbs_dirdyn->options->save2file = 1;
mbs_dirdyn->options->respath = PROJECT_SOURCE_DIR"/../resultsR";
mbs_run_dirdyn(mbs_dirdyn, mbs_data);
mbs_delete_dirdyn(mbs_dirdyn, mbs_data);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* CLOSING OPERATIONS *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_delete_data(mbs_data);
return 0;
}
/**
*
* Author: Nicolas Docquier
* Creation date: 14/11/2011
*
* (c) Universite catholique de Louvain
*
* To turn this file as a C++ file, just change its extension to .cc (or .cpp).
* If you plan to use some C++ files, it is usually better that the main is compiled as a C++ function.
* Currently, most compilers do not require this, but it is a safer approach to port your code to other computers.
*/
#include <stdio.h>
#ifdef __cplusplus
extern "C" {
#endif
#include "mbs_part.h"
#include "mbs_load_xml.h"
#include "mbs_dirdyn.h"
#include "mbs_data.h"
#include "realtime.h"
#include "cmake_config.h"
#include "mbs_project_interface.h"
#ifdef __cplusplus
}
#endif
int main(int argc, char const *argv[])
{
MbsData *mbs_data;
MbsPart *mbs_part;
MbsDirdyn *mbs_dirdyn;
printf("Hello MBS!\n");
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* LOADING *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
printf("Loading the PendulumSpring data file !\n");
mbs_data = mbs_load(PROJECT_SOURCE_DIR"/../dataR/PendulumSpringC.mbs");
printf("*.mbs file loaded!\n");
mbs_get_project_functions(mbs_data);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* COORDINATE PARTITIONING *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_part = mbs_new_part(mbs_data);
mbs_part->options->rowperm=1;
mbs_part->options->verbose = 1;
mbs_run_part(mbs_part, mbs_data);
mbs_delete_part(mbs_part);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* DIRECT DYNANMICS *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_dirdyn = mbs_new_dirdyn(mbs_data);
// dirdyn options (see documentations for additional options)
mbs_dirdyn->options->dt0 = 1e-3;
mbs_dirdyn->options->tf = 10.0;
mbs_dirdyn->options->save2file = 1;
mbs_dirdyn->options->respath = PROJECT_SOURCE_DIR"/../resultsR";
mbs_dirdyn->options->realtime = 1;
mbs_run_dirdyn(mbs_dirdyn, mbs_data);
mbs_delete_dirdyn(mbs_dirdyn, mbs_data);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* CLOSING OPERATIONS *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_delete_data(mbs_data);
return 0;
}
void mbs_get_project_functions(MbsData *mbs_data)
{
// user function pointers
mbs_data->user_JointForces = user_JointForces;
mbs_data->user_init = user_init;
mbs_data->user_loop = user_loop;
mbs_data->user_finish = user_finish;
mbs_data->user_Derivative = user_Derivative;
mbs_data->user_DrivenJoints = user_DrivenJoints;
mbs_data->user_cons_hJ = user_cons_hJ;
mbs_data->user_cons_jdqd = user_cons_jdqd;
// symbolic function pointers
mbs_data->mbs_link = mbs_link;
mbs_data->mbs_link3D = mbs_link3D;
mbs_data->mbs_extforces = mbs_extforces;
mbs_data->mbs_accelred = mbs_accelred;
mbs_data->mbs_dirdyna = mbs_dirdyna;
mbs_data->mbs_cons_hJ = mbs_cons_hJ;
mbs_data->mbs_cons_jdqd = mbs_cons_jdqd;
}
\ No newline at end of file
......@@ -38,7 +38,6 @@ if(FLAG_SHARED_LIB)
include (GenerateExportHeader)
add_library(MBsysC_module SHARED ${SOURCE_FILES} ${INCLUDE_DIR})
target_link_libraries(MBsysC_module MBsysC_struct MBsysC_numerics MBsysC_utilities MBsysC_realtime ${LIBXML2_LIBRARIES} ${GSL_LIBRARIES})
target_link_libraries(MBsysC_module Project_userfct project_symbolic)
GENERATE_EXPORT_HEADER( MBsysC_module )
include_directories(${CMAKE_CURRENT_BINARY_DIR})
......
......@@ -7,7 +7,6 @@
*/
#include "mbs_data.h"
#include "mbs_project_interface.h"
void mbs_calc_force(MbsData* s){
......@@ -23,12 +22,12 @@ void mbs_calc_force(MbsData* s){
}
}
if(s->Nlink > 0) mbs_link(s->frc,s->trq,s->Fl,s->Z,s->Zd,s,s->tsim);
if(s->Nlink > 0) s->mbs_link(s->frc,s->trq,s->Fl,s->Z,s->Zd,s,s->tsim);
if(s->Nlink3D > 0) mbs_link3D(s->frc,s->trq,s,s->tsim);
if(s->Nlink3D > 0) s->mbs_link3D(s->frc,s->trq,s,s->tsim);
if(s->Nxfrc > 0) mbs_extforces(s->frc,s->trq,s,s->tsim);
if(s->Nxfrc > 0) s->mbs_extforces(s->frc,s->trq,s,s->tsim);
s->Qq = user_JointForces(s,s->tsim);
s->Qq = s->user_JointForces(s,s->tsim);
}
......@@ -12,7 +12,6 @@
//
#include "MBSfun.h"
#include "mbs_project_interface.h"
#include "nrfct.h"
double norm_vector(double *v, int n);
......@@ -126,12 +125,13 @@ void mbs_calc_hJ(MbsData *s, MbsAux *mbs_aux)
{
int i,j;
// contraintes de fermeture de boucles
mbs_cons_hJ(mbs_aux->h,mbs_aux->Jac,s,s->tsim);
s->mbs_cons_hJ(mbs_aux->h,mbs_aux->Jac,s,s->tsim);
// contraintes user
if (s->Nuserc>0) {
user_cons_hJ(mbs_aux->huserc, mbs_aux->Juserc, s, s->tsim);
s->user_cons_hJ(mbs_aux->huserc, mbs_aux->Juserc, s, s->tsim);
// ajout des contraintes user aux contraintes de fermeture
for (i=1;i<=s->Nuserc;i++) {
......@@ -147,11 +147,11 @@ void mbs_calc_jdqd(MbsData *s, MbsAux *mbs_aux)
int i;
// compute contribution of symbolic constraints to jdqd
mbs_cons_jdqd(mbs_aux->jdqd,s,s->tsim);
s->mbs_cons_jdqd(mbs_aux->jdqd,s,s->tsim);
// compute contribution of user constraints to jdqd
if (s->Nuserc>0) {
user_cons_jdqd(mbs_aux->jdqduserc, s, s->tsim);
s->user_cons_jdqd(mbs_aux->jdqduserc, s, s->tsim);
// ajout des contraintes user aux contraintes de fermeture
for (i=1;i<=s->Nuserc;i++) {
......
......@@ -16,7 +16,6 @@
#include <stdlib.h>
#include <stdio.h>
#include "integrator.h"
#include "mbs_project_interface.h"
#include "string.h"
#include "realtime.h"
#include "set_output.h"
......@@ -197,7 +196,7 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
dd->dt = dd->options->dt0;
// user intialization
user_init(mbs_data, dd);
mbs_data->user_init(mbs_data, dd);
// Simulation state initialization
for(i=1; i<=mbs_data->nqu; i++)
......@@ -341,7 +340,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
{
// user loop
user_loop(mbs_data, dd);
mbs_data->user_loop(mbs_data, dd);
mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd);
rk4(dd->y, dd->yd, dd->nState, dd->tsim, dd->dt, dd->yout, mbs_fct_dirdyn, mbs_data, dd);
......@@ -377,12 +376,12 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
#ifdef REAL_TIME
if (dd->options->realtime)
{
mbs_realtime_finish(mbs_data->realtime);
mbs_realtime_finish((Simu_realtime*) mbs_data->realtime);
}
#endif
// user finalization
user_finish(mbs_data, dd);
mbs_data->user_finish(mbs_data, dd);
// release memory allocated for dopri5
if (dd->options->dopri5->flag_use)
......@@ -443,6 +442,8 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
{
int i;
user_Derivative_ptr user_Derivative = s->user_Derivative;
mbs_accelred_ptr mbs_accelred = s->mbs_accelred;
s->tsim = tsim;
......@@ -604,6 +605,8 @@ void fcn_dopri5(unsigned n, long nr, double tsim, double y[], double dydt[], Mbs
*/
void solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, int init_flag, MbsData *s, MbsDirdyn *dd)
{
user_loop_ptr user_loop = s->user_loop;
// when using waypoints, only waypoints are used if asked
if ((!init_flag) && dd->options->dopri5->flag_waypoint && dd->options->dopri5->flag_solout_wp)
{
......
......@@ -14,7 +14,6 @@
#ifdef DIRDYNARED
#include "MBSfun.h"
#include "mbs_project_interface.h"
#include "math.h"
#include "nrfct.h"
......@@ -29,7 +28,7 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
int has_a_line_of_zeros;
// Compute driven variable -> user function
if (s->nqc>0) user_DrivenJoints(s,s->tsim);
if (s->nqc>0) s->user_DrivenJoints(s,s->tsim);
// Solve constraints
if (s->Ncons > 0)
......@@ -57,7 +56,7 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
// calcul de la matrice de masse et du vecteur des forces non linéaires
mbs_dirdyna(mbs_aux->M,mbs_aux->c,s,s->tsim);
s->mbs_dirdyna(mbs_aux->M,mbs_aux->c,s,s->tsim);
for(i=1;i<=s->njoint;i++)
{
......
#include "mbs_equil.h"
#include "mbs_project_interface.h"
int compute_Fr_uc( double *Fr_uc, MDS_gen_strct *mds_gen_strct, MbsPart *mbs_part, MbsAux *mbs_aux, MbsData *mbs_data )
{
......@@ -40,7 +39,7 @@ int compute_Fr_uc( double *Fr_uc, MDS_gen_strct *mds_gen_strct, MbsPart *mbs_par
if(mds_gen_strct->bodytree->n_qc > 0)
{
user_DrivenJoints(mbs_data, mbs_data->tsim); // appel routine Robotran
mbs_data->user_DrivenJoints(mbs_data, mbs_data->tsim); // appel routine Robotran
for(i=0; i<mds_gen_strct->bodytree->n_qc; i++) // warning a envisager si diff de zero
{
if (mbs_data->qdd[mds_gen_strct->bodytree->qc[i]+1] != 0.0) // +1 for convention change it
......@@ -91,18 +90,18 @@ int compute_Fr_uc( double *Fr_uc, MDS_gen_strct *mds_gen_strct, MbsPart *mbs_par
}
}
if(mbs_data->Nlink > 0) mbs_link(mbs_data->frc,mbs_data->trq,mbs_data->Fl,mbs_data->Z,mbs_data->Zd,mbs_data,mbs_data->tsim);
if(mbs_data->Nlink > 0) mbs_data->mbs_link(mbs_data->frc,mbs_data->trq,mbs_data->Fl,mbs_data->Z,mbs_data->Zd,mbs_data,mbs_data->tsim);
if(mbs_data->Nxfrc > 0) mbs_extforces(mbs_data->frc,mbs_data->trq,mbs_data,mbs_data->tsim);
if(mbs_data->Nxfrc > 0) mbs_data->mbs_extforces(mbs_data->frc,mbs_data->trq,mbs_data,mbs_data->tsim);
mbs_data->Qq = user_JointForces(mbs_data,mbs_data->tsim);
mbs_data->Qq = mbs_data->user_JointForces(mbs_data,mbs_data->tsim);
// 5. Equations of motion (unconstrained MBS)
zeros_double_vec(&(mbs_data->qdd[1]), mbs_data->njoint); // ... sécurité en attente d'une fonction symb c(q,qd)
mbs_dirdyna(mbs_aux->M,mbs_aux->c,mbs_data,mbs_data->tsim); // futur : c(q,qd)
mbs_data->mbs_dirdyna(mbs_aux->M,mbs_aux->c,mbs_data,mbs_data->tsim); // futur : c(q,qd)
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
copy_double_vec(&(mbs_aux->c[1]), c, mds_gen_strct->bodytree->n_joint); // change it
......
#include "mbs_part.h"
#include "mbs_project_interface.h"
#include "gsl/gsl_linalg.h"
void readmatrix (double** matrix,int* n); //function to read a matrix
......@@ -452,7 +451,7 @@ void PART_calc_hJ(MbsData* mbs_data, double* h, double** J)
if(mbs_data->Nloopc)
{
mbs_cons_hJ(h_loopc, J_loopc, mbs_data, mbs_data->tsim);
mbs_data->mbs_cons_hJ(h_loopc, J_loopc, mbs_data, mbs_data->tsim);
for (i=0; i<mbs_data->Nloopc; i++)
{
h[i] = h_loopc[i+1];
......@@ -464,7 +463,7 @@ void PART_calc_hJ(MbsData* mbs_data, double* h, double** J)
}
if(mbs_data->Nuserc)
{
user_cons_hJ(h_userc, J_userc, mbs_data, mbs_data->tsim);
mbs_data->user_cons_hJ(h_userc, J_userc, mbs_data, mbs_data->tsim);
for (i=0; i<mbs_data->Nuserc; i++)
{
h[mbs_data->Nloopc+i] = h_userc[i+1];
......@@ -743,7 +742,7 @@ int mbs_run_part(MbsPart* mbs_part, MbsData* mbs_data)
// call user_drivenJoints
user_DrivenJoints(mbs_data, mbs_data->tsim);
mbs_data->user_DrivenJoints(mbs_data, mbs_data->tsim);
/*
for ( i=0; i<100; i++)
{
......
......@@ -24,5 +24,6 @@ set(INCLUDE_DIR ${INCLUDE_DIR} ${PROJECT_SOURCE_DIR} PARENT_SCOPE)
add_library(MBsysC_struct ${SOURCE_FILES} ${INCLUDE_DIR})
# include directories
include_directories("./")
include_directories("../mbs_utilities/")
include_directories("../mbs_numerics/")
//---------------------------
// UCL-CEREM-MBS
//
// @version MBsysLab_s 1.7.a
//
// Creation : 2006
// Last update : 01/10/2008
//---------------------------
//
// Gestion via Bugzilla :
// 01/10/2008 : JFC : Bug n°37
//
#ifndef mbs_data_h
#define mbs_data_h
/*--------------------*/
//#ifndef CMEX
//#include "user_sf_IO.h"
//#endif
#include "stdio.h"
#include "stdlib.h"
#include "mbs_user_interface.h"
typedef struct MbsData
{
// MATLAB FIELDS //
// Données géométriques et dynamiques //
int npt; ///< Number of anchor points.
double *dpt[3+1]; ///< Array containing the coordinate of all anchor points.
double *l[3+1]; ///< Array containing the centre of mass coordinates (in the body fixed frame, one column per body)
double *m; ///< Array containing the mass of each body
double *In[9+1]; ///< Array containing the inertia tensor component of each body (in the body fixed frame, relative to the center of mass). 1 column containing 9 rows for each body as follow: [I11;I12;I13;I21;I22;I23;I31;I32;I33]
double g[3+1]; ///< The 3 gravity components
int nbody, njoint;
// Infos partitionnement //
int nqu, nqc, nqlocked, nqdriven, nqa, nqv, nhu; // JFC : 15/01/2008 : ajout de nhu
int *qu, *qc, *qlocked, *qdriven, *qa, *qv, *hu; // JFC : 15/01/2008 : ajout de hu
// Variables articulaires, valeurs initiales et limites //
double *q, *qd, *qdd;
double *q0, *qd0, *qdd0;
double *qmin, *qmax;
// Frc, Trq, Qq, tsim
double *frc[3+1], *trq[3+1], *Qq;
double tsim;
/// initial time of the simulation [s] (for the user)
double t0;
/// final time of the simulation [s] (for the user)
double tf;
/// initial value of the integration step size [s] (for the user)
double dt0;
// set to 1 to stop the simulation
int flag_stop;
// Constraints
double *lrod;
int Nloopc, Ncons, Nuserc;
double NRerr;
// Links
int Nlink, Nlink3D;
double *Z, *Zd, *Fl;
double **l3DWr; // JFC : Attention: la convention des indices est inversée par rapport aux habitudes Robotran
// Sensors
int Nsensor;
// Ext Forces
int Nxfrc, *xfidpt;
double **SWr; // JFC : Attention: la convention des indices est inversée par rapport aux habitudes Robotran
// Wheel
int Nwheel;
double *rnom;
#if !defined SENSORKIN
// User Model
int Nuser_model;
UserModel *user_model;
//#ifndef CMEX
// User Variable
UserIO *user_IO;
//#endif
#endif
// User State
double *ux, *uxd;
double *ux0;
int Nux;
// OTHER FIELDS //
double *udd;
int DonePart;
int DoneEquil;
int DoneModal;
int process; // 1 = part, 2 = equil, 3 = dirdynared,
int simu_end;
char *mbs_filename;
#ifdef REAL_TIME
void *realtime;
#endif
} MbsData;
/*
* Print values of mbs_data (used for debug)
*/
void mbs_print_data(MbsData *mbs_data);
/*--------------------*/
#endif
//---------------------------
// UCL-CEREM-MBS
//
// @version MBsysLab_s 1.7.a
//
// Creation : 2006
// Last update : 01/10/2008
//---------------------------
//
// Gestion via Bugzilla :
// 01/10/2008 : JFC : Bug n°37
//
#ifndef mbs_data_h
#define mbs_data_h
/*--------------------*/
//#ifndef CMEX
//#include "user_sf_IO.h"
//#endif
#include "stdio.h"
#include "stdlib.h"
#include "mbs_user_interface.h"
#include "mbs_project_fct_ptr.h"
struct MbsData
{
// MATLAB FIELDS //
// Données géométriques et dynamiques //
int npt; ///< Number of anchor points.
double *dpt[3+1]; ///< Array containing the coordinate of all anchor points.
double *l[3+1]; ///< Array containing the centre of mass coordinates (in the body fixed frame, one column per body)
double *m; ///< Array containing the mass of each body
double *In[9+1]; ///< Array containing the inertia tensor component of each body (in the body fixed frame, relative to the center of mass). 1 column containing 9 rows for each body as follow: [I11;I12;I13;I21;I22;I23;I31;I32;I33]
double g[3+1]; ///< The 3 gravity components
int nbody, njoint;
// Infos partitionnement //
int nqu, nqc, nqlocked, nqdriven, nqa, nqv, nhu; // JFC : 15/01/2008 : ajout de nhu
int *qu, *qc, *qlocked, *qdriven, *qa, *qv, *hu; // JFC : 15/01/2008 : ajout de hu
// Variables articulaires, valeurs initiales et limites //
double *q, *qd, *qdd;
double *q0, *qd0, *qdd0;
double *qmin, *qmax;
// Frc, Trq, Qq, tsim
double *frc[3+1], *trq[3+1], *Qq;
double tsim;