Merge branch 'smlk_invdyn' into 'dev'

WIP Smlk invdyn

See merge request robotran/mbsysc!330

CHANGELOG.md

@@ -58,9 +58,11 @@
     * [New] Changing user state ids is now forbidden.
     * [Template] user_ExtForces documentation improved.
     * [DEV] files updated to respect pep8 (except for E116, E126, E127, E402, E501, E722, E743, W503, W504)
-    * [DOC] files updated to respect numpydoc style (with spyder 4 lining)  
-    
 
+ * [DOC] files updated to respect numpydoc style (with spyder 4 lining)  
+    
+ * [Simulink] Inverse dynamics module added, basic use.
+ * 
  * [ROS] compatibility with ROS in UNIX OS is done. Path to open GL process is put to catkin_ws.
     Functions used in main are put as extern cpp to be compiled by ROS environment.
     See the ROS tutorial on www.robotran.be for more informations

ExampleProjects/PendulumSpringC/userfctR/user_invdyn.c

+//---------------------------
+// UCL-CEREM-MBS
+//
+// @version MBsysLab_s 1.7.a
+//
+// Creation : 2015
+// Last update : 08/05/2015
+//---------------------------
+
+#include "math.h"
+
+#include "mbs_data.h"
+#include "mbs_invdyn_struct.h"
+
+
+void user_invdyn_init(MbsData *mbs_data, MbsInvdyn *mbs_invdyn)
+{
+
+}
+
+void user_invdyn_loop(MbsData *mbs_data, MbsInvdyn *mbs_invdyn)
+{
+
+}
+
+
+void user_invdyn_finish(MbsData *mbs_data, MbsInvdyn *mbs_invdyn)
+{
+  
+}
+

MBsysC/cmake_aux/flags/CMakeLists.txt

@@ -232,12 +232,6 @@ function(definitions)
         set(LIB_MBSYSC_DEFINITIONS ${LIB_MBSYSC_DEFINITIONS} -DUNIX)
     endif( )
 
-    add_definitions( -DDIRDYNARED )
-    set(LIB_MBSYSC_DEFINITIONS ${LIB_MBSYSC_DEFINITIONS} -DDIRDYNARED)
-
-    add_definitions( -DINVDYNARED )
-    set(LIB_MBSYSC_DEFINITIONS ${LIB_MBSYSC_DEFINITIONS} -DINVDYNARED)
-
     if(FLAG_PRJ_FCT_PTR)
         add_definitions( -DPRJ_FCT_PTR )
         set(LIB_MBSYSC_DEFINITIONS ${LIB_MBSYSC_DEFINITIONS} -DPRJ_FCT_PTR)

MBsysC/cmake_aux/listing/CMakeLists.txt

@@ -19,7 +19,10 @@ function(list_files arg0 arg1 arg2)
         string(REPLACE ${arg2} "" END_FILE ${FILE_PATH})
         string(FIND ${END_FILE} "build" BUILD_PLACE)
         if (${BUILD_PLACE} LESS 0)
-            set(CUR_SRC_LIST ${CUR_SRC_LIST} ${FILE_PATH})
+            string(FIND ${END_FILE} "simulinkR" BUILD_PLACE)
+            if (${BUILD_PLACE} LESS 0)
+                set(CUR_SRC_LIST ${CUR_SRC_LIST} ${FILE_PATH})
+            endif ()
         endif ()
     endforeach(FILE_PATH)
     set(${arg0} ${CUR_SRC_LIST} PARENT_SCOPE)
@@ -255,6 +258,13 @@ function(increment_void_user arg0)
         set(VOID_USER ${VOID_USER} ${ROBOTRAN_SOURCE_DIR}/mbs_workspace_template/C/user_dirdyn.c) 
     endif()
 
+    file(GLOB USER_FILES_C "${arg0}/user_invdyn.c")
+    
+    if (NOT USER_FILES_C)
+        message("Information: using empty user_invdyn.c file.")
+        set(VOID_USER ${VOID_USER} ${ROBOTRAN_SOURCE_DIR}/mbs_workspace_template/C/user_invdyn.c) 
+    endif()
+
     file(GLOB USER_FILES_C "${arg0}/user_equil.c")
     
     if (NOT USER_FILES_C)

MBsysC/mbs_common/mbs_load_xml/mbs_load_libraries.c

@@ -116,6 +116,44 @@ FARPROC WINAPI mbs_load_function(MbsDataLibInfo* li, char* fct_name) {
 
 #endif
 
+#ifdef UNIX
+void* mbs_try_load_function(MbsDataLibInfo* li, char* fct_name) {
+
+    char *error;
+    void* fct;
+
+    fct = dlsym(li->lib_handle, fct_name);
+
+    if ((error = dlerror()) != NULL)
+    {
+        mbs_msg("No function %s\n", fct_name);
+        fct=NULL;
+    }
+    else{
+        mbs_msg("Function %s loaded\n", fct_name);
+    }
+
+    return fct;
+}
+#else
+
+FARPROC WINAPI mbs_try_load_function(MbsDataLibInfo* li, char* fct_name) {
+
+    FARPROC Proc;
+    DWORD error;
+
+    Proc = GetProcAddress(li->lib_handle, fct_name);
+    if (Proc == NULL)
+    {
+        error = GetLastError();
+        mbs_msg("No function %s\n", fct_name);
+    }
+    return Proc;
+
+}
+
+#endif
+
 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
 int mbs_load_symbolic_functions(MbsData* mbs_data, const char* symbolicLib_path, const char* symbolicLib_name)
@@ -192,6 +230,9 @@ int mbs_load_user_functions(MbsData* mbs_data, const char* userfctLib_path, cons
     mbs_data->fct.user.user_dirdyn_init = (user_dirdyn_init_ptr)mbs_load_function(userLibInfo, "user_dirdyn_init");
     mbs_data->fct.user.user_dirdyn_loop = (user_dirdyn_loop_ptr)mbs_load_function(userLibInfo, "user_dirdyn_loop");
     mbs_data->fct.user.user_dirdyn_finish = (user_dirdyn_finish_ptr)mbs_load_function(userLibInfo, "user_dirdyn_finish");
+    mbs_data->fct.user.user_invdyn_init = (user_invdyn_init_ptr)mbs_load_function(userLibInfo, "user_invdyn_init");
+    mbs_data->fct.user.user_invdyn_loop = (user_invdyn_loop_ptr)mbs_load_function(userLibInfo, "user_invdyn_loop");
+    mbs_data->fct.user.user_invdyn_finish = (user_invdyn_finish_ptr)mbs_load_function(userLibInfo, "user_invdyn_finish");
     mbs_data->fct.user.user_equil_init = (user_equil_init_ptr)mbs_load_function(userLibInfo, "user_equil_init");
     mbs_data->fct.user.user_equil_loop = (user_equil_loop_ptr)mbs_load_function(userLibInfo, "user_equil_loop");
     mbs_data->fct.user.user_equil_finish = (user_equil_finish_ptr)mbs_load_function(userLibInfo, "user_equil_finish");

MBsysC/mbs_common/mbs_load_xml/mbs_load_libraries.h

@@ -3,6 +3,40 @@
 
 #include "mbs_data.h"
 
+/** Utility functions */
+#ifdef PRJ_FCT_PTR
+
+MbsDataLibInfo* mbs_load_dynamic_library(char* libpath);
+void mbs_close_dynamic_library(MbsDataLibInfo* li);
+/**
+ * @brief mbs_load_function: dynamic loading of a function from a given library. Produce an
+ *           error message if the function does not exist
+ * @param li the libray where to load the function
+ * @param fct_name the name of the function to load
+ * @return the pointer to the loaded function
+ */
+#ifdef UNIX
+void* mbs_load_function(MbsDataLibInfo* li, char* fct_name);
+#else
+FARPROC WINAPI mbs_load_function(MbsDataLibInfo* li, char* fct_name);
+#endif
+
+/**
+* @brief mbs_load_function: dynamic loading of a function from a given library. Return NULL
+*         if the function does not exist
+* @param li the libray where to load the function
+* @param fct_name the name of the function to load
+* @return the pointer to the loaded function
+*/
+#ifdef UNIX
+void* mbs_try_load_function(MbsDataLibInfo* li, char* fct_name);
+#else
+FARPROC WINAPI mbs_try_load_function(MbsDataLibInfo* li, char* fct_name);
+#endif
+
+
+/** specific functions */
+
 /**
  * \brief Loads the user functions from library into MbsData structure.
  *
@@ -25,6 +59,7 @@ int mbs_load_user_functions(MbsData* mbs_data, const char* user_path, const char
  */
 int mbs_load_symbolic_functions(MbsData* mbs_data, const char* symbolic_path, const char* symbolicLib_name);
 
+#endif
 /**
  * \brief Close the symbolic and userfct libraries associated to the given
  * MbsData

MBsysC/mbs_common/mbs_load_xml/mbs_xml_binder.c

@@ -279,6 +279,11 @@ int mbs_info_joint_binder(MbsInfoJoint *mbs_info_joint, xmlNodePtr node, int mis
     xmlNodePtr sub_cur_node = NULL;
     xmlChar* elementValue = NULL;
 
+    // put all value to true by default
+    mbs_info_joint->symb_q=1;
+    mbs_info_joint->symb_qd=1;
+    mbs_info_joint->symb_qdd=1;
+
     while (cur_node != NULL)
     {
         if (cur_node->type == XML_ELEMENT_NODE || cur_node->type == XML_COMMENT_NODE)
@@ -335,6 +340,10 @@ int mbs_info_joint_binder(MbsInfoJoint *mbs_info_joint, xmlNodePtr node, int mis
                 {
                     mbs_info_joint->nature = 3;
                 }
+                else if (!strcmp((char*)elementValue, "locked"))
+                {
+                    mbs_info_joint->nature = 4;
+                }
                 else
                 {
                     mbs_info_joint->type = 0;
@@ -385,6 +394,42 @@ int mbs_info_joint_binder(MbsInfoJoint *mbs_info_joint, xmlNodePtr node, int mis
                     sub_cur_node = sub_cur_node->next;
                 }
             }
+            else if (!strcmp((const char*)cur_node->name, "symbols"))
+            {
+                // if symbols node is present: put all value to false by default
+                mbs_info_joint->symb_q=0;
+                mbs_info_joint->symb_qd=0;
+                mbs_info_joint->symb_qdd=0;
+                sub_cur_node = cur_node->children;
+                while (sub_cur_node != NULL)
+                {
+                    if (sub_cur_node->type == XML_ELEMENT_NODE || sub_cur_node->type == XML_COMMENT_NODE)
+                    {
+                        if (!strcmp((const char*)sub_cur_node->name, "q"))
+                        {
+                            if (mission == MBS_INFO_READER)
+                            {
+                                mbs_info_joint->symb_q = 1;
+                            }
+                        }
+                        else if (!strcmp((const char*)sub_cur_node->name, "qd"))
+                        {
+                            if (mission == MBS_INFO_READER)
+                            {
+                                mbs_info_joint->symb_qd = 1;
+                            }
+                        }
+                        else if (!strcmp((const char*)sub_cur_node->name, "qdd"))
+                        {
+                            if (mission == MBS_INFO_READER)
+                            {
+                                mbs_info_joint->symb_qdd = 1;
+                            }
+                        }
+                    }
+                    sub_cur_node = sub_cur_node->next;
+                }
+            }
             else if (!strcmp((const char*)cur_node->name, "actuated"))
             {
                 mbs_info_joint->actuated = 1;
@@ -396,6 +441,12 @@ int mbs_info_joint_binder(MbsInfoJoint *mbs_info_joint, xmlNodePtr node, int mis
         }
         cur_node = cur_node->next;
     }
+
+    // adapte joint nature if velocity and acc symbols are not generated
+    if(mbs_info_joint->nature==3 && mbs_info_joint->symb_qd==0 && mbs_info_joint->symb_qdd==0){
+        mbs_info_joint->nature=4;
+    }
+
     return MBS_INFO_SUCCESS;
 }
 
@@ -1554,6 +1605,10 @@ void mbs_info_structurer(MbsInfos *mbs_infos)
                 (mbs_infos->bodytree->n_qc)++;
                 (mbs_infos->bodytree->n_qdriven)++;
                 break;
+            case 4:
+                (mbs_infos->bodytree->n_qc)++;
+                (mbs_infos->bodytree->n_qlocked)++;
+                break;
             }
             if (mbs_infos->bodytree->body_list[i]->joint_list[j]->actuated == 1)
             {
@@ -1642,6 +1697,12 @@ void mbs_info_structurer(MbsInfos *mbs_infos)
                 mbs_infos->bodytree->qdriven[ind_qdriven] = ind_joint;
                 ind_qdriven++;
                 break;
+            case 4:
+                mbs_infos->bodytree->qc[ind_qc] = ind_joint;
+                ind_qc++;
+                mbs_infos->bodytree->qlocked[ind_qlocked] = ind_joint;
+                ind_qlocked++;
+                break;
             }
             if (mbs_infos->bodytree->body_list[i]->joint_list[j]->actuated == 1)
             {

MBsysC/mbs_common/mbs_load_xml/mbs_xml_binder_public.h

@@ -104,7 +104,7 @@ typedef struct MbsInfoJoint
 {
     char *name; //!< Name of the joint
     int type;   //!< Joint type: T1=1, T2=2, T3=3, R1=4, R2=5, R3=6
-    int nature; //!< Nature of the joint: independent=1, dependent=2, driven=3
+    int nature; //!< Nature of the joint: independent=1, dependent=2, driven=3, locked=4
 
     double q0;  //!< Initial position of the joint
     double qd0; //!< Initial velocity of the joint
@@ -116,6 +116,9 @@ typedef struct MbsInfoJoint
     int is_symmmetric;      //!< asymmetric = 0, symmetric master = 1, symmetric salve = 2
     char *symmetric_joint_name; //!< the name of the symmetric joint (filed if master, NULL if slave or non symetric)
 
+    int symb_q; //!< Flag to generate symbol for position
+    int symb_qd; //!< Flag to generate symbol for velocity
+    int symb_qdd; //!< Flag to generate symbol for acceleration
 
 } MbsInfoJoint;
 

MBsysC/mbs_common/mbs_module/mbs_Rred.c

@@ -108,7 +108,7 @@ int mbs_Rred_core(MbsAux *mbs_aux, MbsData *s) {
     }
 
     // Second reduction:    [nqu+nqc x 1] --> [nqu x 1]
-    // (Simply a selection of the first qu element of Rruc) as they have been orded.
+    // (Simply a selection of the first qu element of Rruc) as they have been ordered.
     copy_dvec_0(mbs_aux->Rruc+1, mbs_aux->Rred+1, s->nqu);
 
     for (i = 1; i <= s->nqu; i++)

MBsysC/mbs_common/mbs_module/mbs_aux.c

@@ -18,12 +18,6 @@ MbsAux * initMbsAux(MbsData *s)
 {
     MbsAux *mbs_aux = (MbsAux*)malloc(sizeof(MbsAux));
     int i;
-
-#ifdef SENSORKIN
-    int j;
-#endif
-
-#if defined DIRDYNARED || defined INVDYNARED
     int njoint, nqu, nqv, nqc, nquc, Ncons, Nuserc;
 
     njoint = s->njoint;
@@ -106,25 +100,6 @@ MbsAux * initMbsAux(MbsData *s)
     mbs_aux->q_save = get_dvec_1(s->njoint);
     mbs_aux->close_anim = 0;
 
-#elif defined SENSORKIN
-    if (nsensor) {
-        mbs_aux->psensorStruct = (MbsSensor**)calloc(nsensor + 1, sizeof(MbsSensor*));
-        mbs_aux->psensorStruct[0] = NULL;
-        for (i = 1; i <= nsensor; i++) {
-            mbs_aux->psensorStruct[i] = (MbsSensor*)malloc(sizeof(MbsSensor));
-            //double *J[7]; //attention: necessite allocation dynamique en J[7][njoint+1]
-            mbs_aux->psensorStruct[i]->J[0] = NULL;
-            for (j = 1; j <= 6; j++) {
-                mbs_aux->psensorStruct[i]->J[j] = (double*)calloc(s->njoint + 1, sizeof(double));
-                mbs_aux->psensorStruct[i]->J[j][0] = s->njoint;
-            }
-        }
-    }
-    else
-        mbs_aux->psensorStruct = NULL;
-#endif
-
-#if defined DIRDYNARED || defined INVDYNARED
     mbs_aux->M = get_dmat_1(njoint, njoint);
     mbs_aux->c = get_dvec_1(njoint);
     mbs_aux->F = get_dvec_1(njoint);
@@ -185,7 +160,6 @@ MbsAux * initMbsAux(MbsData *s)
     mbs_aux->R = get_dvec_1(njoint);
     mbs_aux->Rruc = get_dvec_1(nquc);
     mbs_aux->Rred = get_dvec_1(nqu);
-#endif
 
 
 #ifndef MBS_SIMULINK
@@ -207,10 +181,7 @@ MbsAux * initMbsAux(MbsData *s)
 /******************************************************************************/
 void freeMbsAux(MbsAux *mbs_aux, MbsData *s)
 {
-    if (!mbs_aux){
-        return;
-    }
-#if defined DIRDYNARED || defined INVDYNARED
+
     int Ncons, Nuserc;
 
     Nuserc = s->Nuserc;
@@ -245,20 +216,7 @@ void freeMbsAux(MbsAux *mbs_aux, MbsData *s)
 
     free_dvec_1(mbs_aux->q_save);
 
-#elif defined SENSORKIN
-    int i, j;
 
-    if (nsensor) {
-        for (i = 1; i <= nsensor; i++)
-            for (j = 1; j <= 6; j++)
-                free(mbs_aux->psensorStruct[i]->J[j]);
-        free(mbs_aux->psensorStruct[i]);
-    }
-
-    free(mbs_aux->psensorStruct);
-#endif
-
-#if defined DIRDYNARED || defined INVDYNARED
     free_dmat_1(mbs_aux->M);
     free_dvec_1(mbs_aux->c);
     free_dvec_1(mbs_aux->F);
@@ -287,15 +245,6 @@ void freeMbsAux(MbsAux *mbs_aux, MbsData *s)
     free_dvec_1(mbs_aux->Qc);
 
     free_ivec_1(mbs_aux->compute_Qc_vec);
-    /*
-        if (Ncons>0)
-        {
-            free_dvec_1(mbs_aux->lambda);
-            free_dmat_1(mbs_aux->mJvt);
-            free_ivec_1(mbs_aux->ind_mJvt);
-        }
-    */
-
 
     free_dvec_1(mbs_aux->Qact);
 
@@ -310,7 +259,7 @@ void freeMbsAux(MbsAux *mbs_aux, MbsData *s)
     free_dvec_1(mbs_aux->R);
     free_dvec_1(mbs_aux->Rruc);
     free_dvec_1(mbs_aux->Rred);
-#endif
+
 
 
 #ifndef MBS_SIMULINK

MBsysC/mbs_common/mbs_module/mbs_invdyn.c

@@ -372,7 +372,7 @@ void mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
         init_set_output(invd->options->max_save_user);
     }
     // Not yet implemented
-    //user_invdyn_init(mbs_data, invd);
+    user_invdyn_init(mbs_data, invd);
 
     if (invd->options->save2file)
     {

MBsysC/mbs_common/mbs_module/mbs_project_fct_ptr.c

@@ -119,6 +119,20 @@ void user_dirdyn_finish(MbsData* mbs_data, MbsDirdyn *mbs_dd) {
     mbs_data->fct.user.user_dirdyn_finish(mbs_data, mbs_dd);
 }
 
+// User functions specific to invdyn module
+
+void user_invdyn_init(MbsData *mbs_data, MbsInvdyn *mbs_invd) {
+    mbs_data->fct.user.user_invdyn_init(mbs_data, mbs_invd);
+}
+
+void user_invdyn_loop(MbsData *mbs_data, MbsInvdyn *mbs_invd) {
+    mbs_data->fct.user.user_invdyn_loop(mbs_data, mbs_invd);
+}
+
+void user_invdyn_finish(MbsData* mbs_data, MbsInvdyn *mbs_invd) {
+    mbs_data->fct.user.user_invdyn_finish(mbs_data, mbs_invd);
+}
+
 // User functions specific to equil module
 
 void user_equil_init(MbsData *mbs_data, MbsEquil *mbs_equil) {

MBsysC/mbs_common/mbs_struct/mbs_aux.h

@@ -18,11 +18,8 @@
 #include "mbs_sensor_struct.h"
 #include "mbs_data.h"
 
-#ifndef ACCELRED
-
 typedef struct MbsAux_tag
 {
-#if defined DIRDYNARED || defined INVDYNARED
         double norm_h;
         double NRerr;
         int MAX_NR_ITER; //!< max number of iteration for NR procedure
@@ -36,12 +33,10 @@ typedef struct MbsAux_tag
         double *Qc;
 
         double *mJv_h; //!< Value of the independant constraints (\f$h_u(q)\f$)
-//      double **mJv_h;
 
         int nquc; //!< store nqu+nqc
         int *iquc;
         double **Juct; // JFC: 15/01/2008 : changement nom
-//      double **Juc; // JFC: ajout temporaire
 
         double **Bvuc;
 
@@ -56,19 +51,12 @@ typedef struct MbsAux_tag
         double *jdqduserc;
 
         double * bp;
-//      double **bp;
 
         MbsSensor *psens; // temporary sensor pointer
 
         double *q_save;
         int     close_anim;
 
-#elif defined SENSORKIN
-    MbsSensor **psensorStruct;
-#endif
-/**/
-
-#if defined DIRDYNARED || defined INVDYNARED
         double *c, **M;
         double *F;      //!< vector of size njoint that containts c(q,qd)-Q(q,qd)
 
@@ -93,18 +81,10 @@ typedef struct MbsAux_tag
         double *w;
         double **v;
 
-//      double **mJvt;
-//      int    *ind_mJvt;
-//      double *lambda;
-
-
-// voluntarily defined outside of INVDYNARED flag as it does not refer to a specified analysis (module).
-// Compilation dedicated to a specific module will be disable.
         double *phi;   //!< vector of size [njoint x 1] that contains the results of symbolic file invdyna.
         double *R;     //!< vector of size [njoint x 1] that contains R=phi-c; (unconstrained equations of motion for the mbs on residual form)
         double *Rruc;  //!< vector of size [nqu+nqc x 1] that contains the reduced residue for the equations of motions after the first reduction (dependent variables)
         double *Rred;  //!< vector of size [nqu x 1] that contains the reduced residue of the equations of motion after the second reduction (driven variables)
-#endif
 
 #ifndef MBS_SIMULINK
         double x, *y, *dydx;
@@ -116,7 +96,5 @@ MbsAux* initMbsAux(MbsData *mbs_data);
 
 void freeMbsAux(MbsAux* mbs_aux, MbsData *mbs_data);
 
-#else
-typedef void* MbsAux;
-#endif
+
 #endif

MBsysC/mbs_common/mbs_struct/mbs_data.h

@@ -85,6 +85,10 @@ typedef struct{
         user_dirdyn_init_ptr user_dirdyn_init;
         user_dirdyn_loop_ptr user_dirdyn_loop;
         user_dirdyn_finish_ptr user_dirdyn_finish;
+        // user invdyn
+        user_invdyn_init_ptr user_invdyn_init;
+        user_invdyn_loop_ptr user_invdyn_loop;
+        user_invdyn_finish_ptr user_invdyn_finish;
         // user equil
         user_equil_init_ptr user_equil_init;
         user_equil_loop_ptr user_equil_loop;

MBsysC/mbs_common/mbs_struct/mbs_project_fct_ptr.h

@@ -13,6 +13,7 @@
 typedef struct MbsData MbsData;
 typedef struct MbsSensor MbsSensor;
 typedef struct MbsDirdyn MbsDirdyn;
+typedef struct MbsInvdyn MbsInvdyn;
 typedef struct MbsEquil MbsEquil;
 
 typedef double* (*user_JointForces_ptr)(MbsData*, double);
@@ -20,6 +21,9 @@ typedef void    (*user_load_post_ptr)(MbsData *MBSdata);
 typedef void    (*user_dirdyn_init_ptr)(MbsData *MBSdata, MbsDirdyn *mbs_dd);
 typedef void    (*user_dirdyn_loop_ptr)(MbsData *MBSdata, MbsDirdyn *mbs_dd);
 typedef void    (*user_dirdyn_finish_ptr)(MbsData*, MbsDirdyn*);
+typedef void    (*user_invdyn_init_ptr)(MbsData *MBSdata, MbsInvdyn *mbs_invd);
+typedef void    (*user_invdyn_loop_ptr)(MbsData *MBSdata, MbsInvdyn *mbs_invd);
+typedef void    (*user_invdyn_finish_ptr)(MbsData*MBSdata, MbsInvdyn* mbs_invd);
 typedef void    (*user_equil_init_ptr)(MbsData *MBSdata, MbsEquil *mbs_equil);
 typedef void    (*user_equil_loop_ptr)(MbsData *MBSdata, MbsEquil *mbs_equil);
 typedef void    (*user_equil_finish_ptr)(MbsData*, MbsEquil*);

MBsysC/mbs_common/mbs_struct/mbs_project_interface.h

@@ -25,6 +25,10 @@ void user_dirdyn_init(MbsData *MBSdata, MbsDirdyn *mbs_dd);
 void user_dirdyn_loop(MbsData *MBSdata, MbsDirdyn *mbs_dd);
 void user_dirdyn_finish(MbsData *MBSdata, MbsDirdyn *mbs_dd);
 
+void user_invdyn_init(MbsData *MBSdata, MbsInvdyn *mbs_invd);
+void user_invdyn_loop(MbsData *MBSdata, MbsInvdyn *mbs_invd);
+void user_invdyn_finish(MbsData *MBSdata, MbsInvdyn *mbs_invd);
+
 void user_equil_init(MbsData *MBSdata, MbsEquil *mbs_equil);
 void user_equil_loop(MbsData *MBSdata, MbsEquil *mbs_equil);
 void user_equil_finish(MbsData *MBSdata, MbsEquil *mbs_equil);

MBsysC/mbs_common/mbs_utilities/lut.h

@@ -165,6 +165,7 @@ int mbs_lut_1Dvec_interp(MbsLut1Dvec *lut, double x, double *output);
 int mbs_lut_2Dvec_interp(MbsLut2Dvec *lut, double x, double y, double *output);
 
 #ifdef MATLAB_MEX_FILE
+#include "matrix.h"
 /*! \brief Load an mxArray and return a new allocated 1D LUT structure
  *
  * \param[in] LUT_1D_ptr mxArray structure with an "x" field containing the input values and "y" field with the output values. Both are rows vectors, size = 1 x n

MBsysC/mbs_interface/mbs_simulink/CMakeLists.txt

@@ -19,7 +19,8 @@ cmake_minimum_required(VERSION 2.8.7)
 # project name
 project (MBsysC_SF_dirdyn)
 
-set(SFUNCTION_NAME MBsysC_SF_dirdyn)
+set(SFUNCTION_NAME_DIRDYN MBsysC_SF_dirdyn)
+set(SFUNCTION_NAME_INVDYN MBsysC_SF_invdyn)
 
 
 # Variable for storing the path to Robotran common files (should be adapted depending on the location of those source)
@@ -87,8 +88,20 @@ message (STATUS "Matlab_SLX_LIBRARY_PATH -> " ${Matlab_SLX_LIBRARY_PATH})
 # list source files to compile
 init_src()
 
-increment_src( ${PROJECT_SOURCE_DIR}/src )
+# source files common for all blocks
+set(SRC_ALL "")
+set(SRC_ALL ${SRC_ALL}  ${PROJECT_SOURCE_DIR}/src/mbs_sf_fct.c)
 
+# source files for DIRDYN block
+set(SRC_DIRDYN ${SRC_ALL})
+set(SRC_DIRDYN ${SRC_DIRDYN}  ${PROJECT_SOURCE_DIR}/src/mbs_sf_fct_dirdyn.c)
+set(SRC_DIRDYN ${SRC_DIRDYN}  ${PROJECT_SOURCE_DIR}/src/mbs_sf_main.c)
+set(SRC_DIRDYN ${SRC_DIRDYN}  ${PROJECT_SOURCE_DIR}/src/mbs_sf_user.c)
+
+# source files for INVDYN block
+set(SRC_INVDYN ${SRC_ALL})
+set(SRC_INVDYN ${SRC_INVDYN}  ${PROJECT_SOURCE_DIR}/src/mbs_sf_fct_invdyn.c)