Commit 1a991dad authored by Nicolas Docquier's avatar Nicolas Docquier
Browse files

Add an intermediate file for call user/symbolic function via pointer (so as to...

Add an intermediate file for call user/symbolic function via pointer (so as to enable "monolithic" compiling)
parent e29afa69
......@@ -8,7 +8,7 @@
cmake_minimum_required(VERSION 2.8.7)
project(project_symbolic)
project(Project_symbolic)
# flags
if (UNIX)
......@@ -28,26 +28,34 @@ init_src()
increment_src( ${PROJECT_SOURCE_DIR} )
increment_void_symbolic( symbolicR )
add_library(Project_symbolic SHARED ${SOURCE_FILES})
# include directories
include_directories("./")
include_directories(${LIB_MBSYSC_INCLUDE_DIRS})
message(STATUS "hellouuuuuuuu " ${SOURCE_FILES})
add_library(project_symbolic SHARED ${SOURCE_FILES})
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
# LINK LIBRARIES
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
# Some symbolic routines call user functions (ext_forces, accelred, ...)
target_link_libraries(project_symbolic Project_userfct)
target_link_libraries(Project_symbolic Project_userfct)
# MBSysC libraries
if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
find_package( LibRobotranC REQUIRED )
add_definitions(${LIB_MBSYSC_DEFINITIONS})
endif()
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
# STUFF FOR WINDOWS (TO BE CLEANED)
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
include (GenerateExportHeader)
GENERATE_EXPORT_HEADER( project_symbolic )
GENERATE_EXPORT_HEADER( Project_symbolic )
include_directories(${CMAKE_CURRENT_BINARY_DIR})
install (TARGETS project_symbolic DESTINATION ${CMAKE_INSTALL_PREFIX})
install (TARGETS Project_symbolic DESTINATION ${CMAKE_INSTALL_PREFIX})
//
// MBsysTran - Release 8.1 (built: August 08, 2015)
//
// Copyright
// Universite catholique de Louvain (UCL)
// Center for Research in Energy and Mechatronics (CEREM)
// Copyright
// Universite catholique de Louvain (UCL)
// Center for Research in Energy and Mechatronics (CEREM)
// 2, Place du Levant
// 1348 Louvain-la-Neuve
// Belgium
// 1348 Louvain-la-Neuve
// Belgium
//
// http://www.robotran.be
// http://www.robotran.be
//
// ==> Generation Date: Thu Sep 03 09:41:26 2015
//
......@@ -19,7 +19,7 @@
// ==> Function: F10 - Direct Dynamics (Explicit formulation) - RNEA + CP
//
#include <math.h>
#include <math.h>
#include "mbs_data.h"
#include "mbs_project_interface.h"
......@@ -41,7 +41,7 @@ double *m;
double **In;
double *g;
int iter = 0;
int iter = 0;
q = s->q;
qd = s->qd;
......@@ -59,14 +59,14 @@ In = s->In;
g = s->g;
// Driven Variables
// Driven Variables
user_DrivenJoints(s,tsim);
NRh2 = 1.0;
while((NRh2 > NR_ERR) && (iter++ < MAX_NR_ITER))
{
s->fct.user.user_DrivenJoints(s,tsim);
NRh2 = 1.0;
while((NRh2 > NR_ERR) && (iter++ < MAX_NR_ITER))
{
// Trigonometric functions
S4 = sin(q[4]);
......@@ -77,14 +77,14 @@ S1 = sin(q[1]);
C1 = cos(q[1]);
S3 = sin(q[3]);
C3 = cos(q[3]);
// Augmented Joint Position Vectors
Dz23 = q[2]+dpt[3][3];
// Augmented Joint Position Vectors
// Constraints and Constraints Jacobian
ROlp1_12 = C4*C5-S4*S5;
......@@ -102,21 +102,21 @@ Dz23 = q[2]+dpt[3][3];
RLlp2_32 = dpt[3][4]*C1;
h_1 = POlp1_13-RLlp2_12;
h_3 = POlp1_33-RLlp2_32;
// Loop Closure Thresold
NRh2_1 = h_1*h_1;
NRh2 = NRh2_1+h_3*h_3;
// LU Factorisation of the Jacobian (Jv)
Jvlu2_1 = JTlp1_33_1/JTlp1_13_1;
Jvlu2_2 = -RLlp1_13-Jvlu2_1*RLlp1_33;
//
//
if(NRh2 > NR_ERR)
{
// Newton-Raphson: Loop Closure
Jv_hy2 = h_3-Jvlu2_1*h_1;
......@@ -124,24 +124,24 @@ if(NRh2 > NR_ERR)
Jv_hx1 = (h_1-Jv_hx2*RLlp1_33)/JTlp1_13_1;
q[4] = q[4]-Jv_hx1;
q[5] = q[5]-Jv_hx2;
//
}
}
//
}
}
if(iter > MAX_NR_ITER) return(-1);
// Bvu Matrix
Bvu2y1 = -RLlp2_12-Jvlu2_1*RLlp2_32;
Bvu2x1 = Bvu2y1/Jvlu2_2;
Bvu1x1 = (RLlp2_32-Bvu2x1*RLlp1_33)/JTlp1_13_1;
// Dependent Velocities
qd[4] = qd[1]*Bvu1x1;
qd[5] = qd[1]*Bvu2x1;
// Constraints Quadratic Terms
ROjdqd1_12 = C4*C5-S4*S5;
......@@ -167,19 +167,19 @@ if(iter > MAX_NR_ITER) return(-1);
Apqpjdqd2_32 = -qd[1]*ORjdqd2_12;
jdqd1 = Apqpjdqd1_13-Apqpjdqd2_12;
jdqd3 = Apqpjdqd1_33-Apqpjdqd2_32;
// b Vector
b1y2 = -jdqd3+Jvlu2_1*jdqd1;
b1x2 = b1y2/Jvlu2_2;
b1x1 = (-jdqd1-RLlp1_33*b1x2)/JTlp1_13_1;
// Forces and Torques computation
// Joint Forces
// Forces and Torques computation
// Joint Forces
Qq = user_JointForces(s,tsim);
Qq = s->fct.user.user_JointForces(s,tsim);
// Link Kinematics: Distance Z , Relative Velocity ZD
RLlnk2_12 = dpt[3][4]*S1;
......@@ -193,18 +193,18 @@ if(iter > MAX_NR_ITER) return(-1);
e31 = RLlnk2_32/Z1;
Zd1 = ORlnk2_12*e11+ORlnk2_32*e31;
// Link Forces
// Link Forces
Flnk1 = user_LinkForces(Z1,Zd1,s,tsim,1);
Flnk1 = s->fct.user.user_LinkForces(Z1,Zd1,s,tsim,1);
// Link Dynamics : Forces projection on body-fixed frames
fSlnk11 = Flnk1*(e11*C1-e31*S1);
trqlnk1_1_2 = -fSlnk11*(dpt[3][4]-l[3][1]);
// Sensor Kinematics
// Forward Kinematics
BS91 = -qd[1]*qd[1];
......@@ -227,7 +227,7 @@ if(iter > MAX_NR_ITER) return(-1);
AlF35 = AlF14*S5+C5*(AlF34+BS94*dpt[3][6]);
AlM15_4 = dpt[3][6]*C5;
AlM35_4 = dpt[3][6]*S5;
// Backward Dynamics
FA15 = m[5]*(AlF15+BS15*l[1][5]);
......@@ -256,7 +256,7 @@ if(iter > MAX_NR_ITER) return(-1);
CF21 = CF22-trqlnk1_1_2+FA11*l[3][1]+FF12*Dz23;
FB11_1 = m[1]*l[3][1];
CM21_1 = In[5][1]+FB11_1*l[3][1]+FM12_1*Dz23;
// Reduction (Coordinate Partitioning)
MMB1_1 = Bvu1x1*CM24_4+Bvu2x1*CM25_4;
......@@ -267,31 +267,31 @@ if(iter > MAX_NR_ITER) return(-1);
Fr1 = CF21-Qq[1]+Bvu1x1*FMB1+Bvu2x1*FMB2;
Fr2 = FA33-Qq[2];
Fr3 = -Qq[3];
// Linear System Resolution
qppux3 = -Fr3/In[9][3];
qppux2 = -Fr2/m[3];
qppux1 = -Fr1/Mr1_1;
// Symbolic Output number 1
qdd[1] = qppux1;
qdd[2] = qppux2;
qdd[3] = qppux3;
// Symbolic Output number 2
udd[1] = qppux1;
udd[2] = qppux2;
udd[3] = qppux3;
// Symbolic Output number 3
qdd[4] = b1x1+Bvu1x1*qdd[1];
qdd[5] = b1x2+Bvu2x1*qdd[1];
return iter;
return iter;
// Number of continuation lines = 0
......
......@@ -3,22 +3,22 @@
//
// ROBOTRAN - Version 6.6 (build : february 22, 2008)
//
// Copyright
// Universite catholique de Louvain
// Departement de Mecanique
// Unite de Production Mecanique et Machines
// 2, Place du Levant
// 1348 Louvain-la-Neuve
// http://www.robotran.be//
// Copyright
// Universite catholique de Louvain
// Departement de Mecanique
// Unite de Production Mecanique et Machines
// 2, Place du Levant
// 1348 Louvain-la-Neuve
// http://www.robotran.be//
//
// ==> Generation Date : Fri Jan 23 18:26:26 2015
//
// ==> Project name : PendulumSpringC
// ==> using XML input file
// ==> using XML input file
//
// ==> Number of joints : 5
//
// ==> Function : F 7 : Point to point Link Forces (frc,trq,Flnk)
// ==> Function : F 7 : Point to point Link Forces (frc,trq,Flnk)
// ==> Flops complexity : 25
//
// ==> Generation Time : 0.000 seconds
......@@ -26,14 +26,14 @@
//
//-------------------------------------------------------------
//
#include <math.h>
#include <math.h>
#include "MBSdataStructR7.h"
#include "mbs_project_interface.h"
void mbs_link(double **frc,double **trq,double *Flnk,double *Z,double *Zd,
MbsData *s, double tsim)
......@@ -42,52 +42,52 @@ MbsData *s, double tsim)
// double Flnk[1];
// double Z[1];
// double Zd[1];
{
#include "mbs_link_PendulumSpringC.h"
#define q s->q
#define qd s->qd
#define qdd s->qdd
{
#include "mbs_link_PendulumSpringC.h"
#define q s->q
#define qd s->qd
#define qdd s->qdd
// === begin imp_aux ===
// === begin imp_aux ===
// === end imp_aux ===
// === end imp_aux ===
// ===== BEGIN task 0 =====
// ===== BEGIN task 0 =====
// = = Block_0_0_0_0_0_1 = =
// = = Block_0_0_0_0_0_1 = =
// Trigonometric Variables
// Trigonometric Variables
C1 = cos(q[1]);
S1 = sin(q[1]);
// = = Block_0_1_0_0_1_1 = =
// Link Kinematics: Distance Z , Relative Velocity ZD
// = = Block_0_1_0_0_1_1 = =
// Link Kinematics: Distance Z , Relative Velocity ZD
RLlnk1_19 = s->dpt[3][4]*S1;
RLlnk1_39 = s->dpt[3][4]*C1;
// = = Block_0_1_0_1_1_1 = =
// Link Kinematics: Distance Z , Relative Velocity ZD
// = = Block_0_1_0_1_1_1 = =
// Link Kinematics: Distance Z , Relative Velocity ZD
Plnk11 = RLlnk1_19-s->dpt[1][1];
Z1 = sqrt(Plnk11*Plnk11+RLlnk1_39*RLlnk1_39);
e11 = Plnk11/Z1;
e31 = RLlnk1_39/Z1;
Zd1 = -qd[1]*(RLlnk1_19*e31-RLlnk1_39*e11);
// Link Force Computation
Flink1 = s->fct.user.user_LinkForces(Z1,Zd1,s,tsim,1);
// Link Force Computation
Flink1 = user_LinkForces(Z1,Zd1,s,tsim,1);
// = = Block_0_1_0_2_2_1 = =
// Link Dynamics : Forces projection on body-fixed frames
// = = Block_0_1_0_2_2_1 = =
// Link Dynamics : Forces projection on body-fixed frames
fSlnk11 = Flink1*(e11*C1-e31*S1);
fSlnk31 = Flink1*(e11*S1+e31*C1);
......@@ -95,9 +95,9 @@ MbsData *s, double tsim)
frc[3][1] = s->frc[3][1]-fSlnk31;
trq[2][1] = s->trq[2][1]-fSlnk11*(s->dpt[3][4]-s->l[3][1]);
// = = Block_0_2_0_0_0_0 = =
// Symbolic Outputs
// = = Block_0_2_0_0_0_0 = =
// Symbolic Outputs
frc[2][1] = s->frc[2][1];
frc[1][3] = s->frc[1][3];
......@@ -124,9 +124,9 @@ MbsData *s, double tsim)
Z[1] = Z1;
Zd[1] = Zd1;
// ====== END Task 0 ======
// ====== END Task 0 ======
}
......@@ -7,7 +7,7 @@
# define PROJECT_SYMBOLIC_NO_EXPORT
#else
# ifndef PROJECT_SYMBOLIC_EXPORT
# ifdef project_symbolic_EXPORTS
# ifdef Project_symbolic_EXPORTS
/* We are building this library */
# define PROJECT_SYMBOLIC_EXPORT
# else
......
......@@ -111,6 +111,9 @@ include_directories (${PROJECT_BINARY_DIR}/conf)
init_src()
increment_src( ${PROJECT_SOURCE_DIR}/src )
#increment_src( ${PROJECT_SOURCE_DIR}/../symbolicR )
#increment_void_symbolic( symbolicR )
# list include directories (to find headers)
init_include()
......@@ -171,6 +174,7 @@ else ( )
#user functions library
target_link_libraries ( ${Executable} Project_userfct )
target_link_libraries ( ${Executable} Project_symbolic )
#Libxml2 and GSL external libraries
target_link_libraries ( ${Executable} ${LIBXML2_LIBRARIES} ${GSL_LIBRARIES} )
......@@ -196,11 +200,15 @@ include_directories(${LIB_MBSYSC_INCLUDE_DIRS})
#increment_src( ${PROJECT_SOURCE_DIR}/../userfctR )
add_subdirectory( ${PROJECT_SOURCE_DIR}/../userfctR ${CMAKE_CURRENT_BINARY_DIR}/userfctR)
add_subdirectory( ${PROJECT_SOURCE_DIR}/../symbolicR ${CMAKE_CURRENT_BINARY_DIR}/symbolicR)
if (NOT FLAG_SEPARATE_SYMBOLIC)
#increment_src( ${PROJECT_SOURCE_DIR}/../symbolicR )
#increment_void_symbolic( symbolicR )
add_subdirectory( ${PROJECT_SOURCE_DIR}/../symbolicR ${CMAKE_CURRENT_BINARY_DIR}/symbolicR)
MESSAGE(STATUS "Hellooooooooooooooooo")
#increment_src( ${PROJECT_SOURCE_DIR}/../symbolicR )
#add_subdirectory( ${PROJECT_SOURCE_DIR}/../symbolicR ${CMAKE_CURRENT_BINARY_DIR}/symbolicR)
endif ( )
if (UNIX)
......
......@@ -28,9 +28,9 @@ extern "C" {
int main(int argc, char const *argv[])
{
MbsData *mbs_data;
MbsPart *mbs_part;
MbsDirdyn *mbs_dirdyn;
......@@ -40,11 +40,11 @@ int main(int argc, char const *argv[])
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* 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 *
......@@ -54,7 +54,7 @@ int main(int argc, char const *argv[])
mbs_part->options->rowperm=1;
mbs_part->options->verbose = 1;
mbs_run_part(mbs_part, mbs_data);
mbs_delete_part(mbs_part);
......@@ -71,17 +71,17 @@ int main(int argc, char const *argv[])
mbs_dirdyn->options->respath = PROJECT_SOURCE_DIR"/../resultsR";
mbs_dirdyn->options->animpath = PROJECT_SOURCE_DIR"/../animationR";
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;
}
......@@ -22,6 +22,10 @@ option (FLAG_RELEASE
function(mbsysc_specific_flags) # flag for lib_mbsysC only (not set by project)
# run the simulation in real-time
option (FLAG_PRJ_FCT_PTR
"Call symbolic and user function via function pointer" OFF)
## -- Real-time features related -- ##
# run the simulation in real-time
......@@ -126,6 +130,11 @@ function(definitions)
add_definitions( -DCMEX ) # note: CMEX was defined for Simulink stuff, it should be modified because the Standalone version was added !
set(LIB_MBSYSC_DEFINITIONS ${LIB_MBSYSC_DEFINITIONS} -DCMEX)
if(FLAG_PRJ_FCT_PTR)
add_definitions( -DPRJ_FCT_PTR )
set(LIB_MBSYSC_DEFINITIONS ${LIB_MBSYSC_DEFINITIONS} -DPRJ_FCT_PTR)
endif( )
if(FLAG_REAL_TIME)
add_definitions( -DREAL_TIME )
set(LIB_MBSYSC_DEFINITIONS ${LIB_MBSYSC_DEFINITIONS} -DREAL_TIME)
......
......@@ -49,6 +49,9 @@ FIND_LIBRARY(LIB_MBSYSC_REALTIME @LIB_MBSYSC_REALTIME@
# Path to Robotran common files
SET(ROBOTRAN_SOURCE_DIR @ROBOTRAN_SOURCE_DIR@)
# Call project funstion (symbolic and user) via function pointers
SET(FLAG_PRJ_FCT_PTR @FLAG_PRJ_FCT_PTR@)
# Realtime options
SET(FLAG_REAL_TIME @FLAG_REAL_TIME@)
SET(FLAG_PLOT @FLAG_PLOT@)
......
......@@ -101,9 +101,9 @@ void mbs_load_symbolic_functions(MbsData* mbs_data, const char* mbs_filename){
lib_path = (char*)malloc((strlen(mbs_filename)+50)*sizeof(char));
#ifdef UNIX
sprintf(lib_path, "%s/workR/build/symbolicR/libproject_symbolic.so", find_project_path(mbs_filename));
sprintf(lib_path, "%s/workR/build/symbolicR/libProject_symbolic.so", find_project_path(mbs_filename));
#else
sprintf(lib_path, "%s\\workR\\build\\Debug\\project_symbolic.dll", find_project_path(mbs_filename));
sprintf(lib_path, "%s\\workR\\build\\Debug\\Project_symbolic.dll", find_project_path(mbs_filename));
#endif
symbLibInfo = mbs_load_dynamic_library(lib_path) ;
......
......@@ -14,9 +14,14 @@ MbsData* mbs_load(const char* mbs_filename){
mds = MDS_mbs_reader(mbs_filename);
mbs_data = (MbsData*) malloc(sizeof(MbsData));
// if we call project and user function pointer, init them
#ifdef PRJ_FCT_PTR
printf("load user and symbolic function pointers");
mbs_load_symbolic_functions(mbs_data, mbs_filename);
mbs_load_user_functions(mbs_data, mbs_filename);
mbs_data = MDS_create_MBSdataStruct(mds, mbs_data);
#endif
free_MDS_gen_strct(mds);
......