Commit e4ec6f31 authored by alexbuset's avatar alexbuset

Test

parent 3dd3a8a8
Pipeline #5448 failed with stages
This diff is collapsed.
the data fits exactly the simulink example from :
https://mathworks.com/help/mpc/examples/control-of-an-inverted-pendulum-on-a-cart.html
if the link is not working, look for "Control of an Inverted Pendulum on a Cart matlab"
\ No newline at end of file
#
# Universite catholique de Louvain
# CEREM : Centre for research in mechatronics
# http://www.robotran.be
# Contact : info@robotran.be
#
#
# CMake for compiling the symbolic files of a robotran project in C
#
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
# LIBRARY MAIN CONFIGURATIONS
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
if (UNIX)
cmake_minimum_required(VERSION 2.8.7)
else()
cmake_minimum_required(VERSION 3.4)
endif ()
if(NOT DEFINED SYMBOLIC_LIB_NAME)
set (SYMBOLIC_LIB_NAME "Project_symbolic")
endif ( )
project(${SYMBOLIC_LIB_NAME})
# flags
if (UNIX)
set(CMAKE_C_FLAGS "-fPIC")
set(CMAKE_EXE_LINKER_FLAGS "-fPIC")
endif (UNIX)
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
# SEPARATE COMPILATION
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
if ( FLAG_SEPARATE_SYMBOLIC )
set(CMAKE_AUX ${ROBOTRAN_SOURCE_DIR}/cmake_aux)
set(CMAKE_AUX_BIN ${PROJECT_BINARY_DIR}/cmake_aux)
add_subdirectory ( ${CMAKE_AUX}/flags/ ${CMAKE_AUX_BIN}/flags/ )
add_subdirectory ( ${CMAKE_AUX}/listing/ ${CMAKE_AUX_BIN}/listing/ )
endif ( )
# release of debug
release_debug()
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
# SET LIBRARY
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
# list source files to compile
init_src()
increment_src( ${PROJECT_SOURCE_DIR} )
increment_void_symbolic( ${PROJECT_SOURCE_DIR} )
# list include directories (to find headers)
init_include()
increment_include( ${ROBOTRAN_SOURCE_DIR}/mbs_common )
# include these directories
include_directories ( ${INCLUDE_DIR} )
include_directories ( ${CMAKE_CURRENT_BINARY_DIR} )
if( FLAG_SHARED_LIB OR FLAG_SEPARATE_SYMBOLIC )
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
add_library(${SYMBOLIC_LIB_NAME} SHARED ${SOURCE_FILES})
# MacOS: Force extension to be .so rather than .dylib
if(APPLE)
set_target_properties(${SYMBOLIC_LIB_NAME} PROPERTIES SUFFIX .so)
endif()
# Windows, copy dll next to executable
if((NOT UNIX) AND (NOT FLAG_SEPARATE_SYMBOLIC))
add_custom_command(TARGET ${SYMBOLIC_LIB_NAME} POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy
${CMAKE_CURRENT_BINARY_DIR}/$<CONFIGURATION>/${SYMBOLIC_LIB_NAME}.dll
${CMAKE_CURRENT_BINARY_DIR}/../$<CONFIGURATION>/${SYMBOLIC_LIB_NAME}.dll
)
endif()
else()
add_library(${SYMBOLIC_LIB_NAME} STATIC ${SOURCE_FILES})
# some symbolic routines call user functions (ext_forces, accelred, ...)
target_link_libraries(${SYMBOLIC_LIB_NAME} Project_userfct)
endif()
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
# LINK LIBRARIES
# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
# MBSysC libraries
if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
find_package( LibRobotranC 1.11.2 REQUIRED )
add_definitions(${LIB_MBSYSC_DEFINITIONS})
target_link_libraries(${SYMBOLIC_LIB_NAME} ${LIB_MBSYSC_MODULES})
endif()
# Windows M_PI definitions
if (WIN32)
add_definitions(-D _USE_MATH_DEFINES)
endif (WIN32)
//
//-------------------------------------------------------------
//
// 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//
//
// ==> Generation Date : Wed Feb 13 13:58:10 2019
//
// ==> Project name : CartPendulum
// ==> using XML input file
//
// ==> Number of joints : 3
//
// ==> Function : F 1 : Direct Dynamics (Semi-Explicit formulation) : RNEA
// ==> Flops complexity : 60
//
// ==> Generation Time : 0.000 seconds
// ==> Post-Processing : 0.000 seconds
//
//-------------------------------------------------------------
//
#include <math.h>
#include "mbs_data.h"
#include "mbs_project_interface.h"
void mbs_dirdyna(double **M,double *c,
MbsData *s, double tsim)
// double M[3][3];
// double c[3];
{
#include "mbs_dirdyna_CartPendulum.h"
#define q s->q
#define qd s->qd
#define qdd s->qdd
// === begin imp_aux ===
// === end imp_aux ===
// ===== BEGIN task 0 =====
// = = Block_0_0_0_0_0_1 = =
// Trigonometric Variables
C2 = cos(q[2]);
S2 = sin(q[2]);
C3 = cos(q[3]);
S3 = sin(q[3]);
// = = Block_0_1_0_0_0_1 = =
// Forward Kinematics
OM13 = -qd[2]*S3;
OM33 = qd[2]*C3;
// = = Block_0_2_0_1_0_1 = =
// Backward Dynamics
FA13 = -(s->frc[1][3]-s->m[3]*(OM13*OM33*s->l[3][3]+s->g[3]*S3));
FA23 = -(s->frc[2][3]-qd[3]*s->m[3]*s->l[3][3]*(OM33+qd[2]*C3));
CF23 = -(s->trq[2][3]-FA13*s->l[3][3]);
CM23_1 = s->m[3]*s->l[3][3]*C2*C3;
CM23_3 = s->m[3]*s->l[3][3]*s->l[3][3];
CF2_33 = -(s->trq[3][3]*C3-S3*(s->trq[1][3]+FA23*s->l[3][3]));
CM21_33 = -s->m[3]*s->l[3][3]*S2*S3;
CM22_33 = s->m[3]*s->l[3][3]*s->l[3][3]*S3*S3;
FF11 = -(s->frc[1][1]+FA23*S2-C2*(FA13*C3-S3*(s->frc[3][3]+s->m[3]*(s->g[3]*C3+s->l[3][3]*(qd[3]*qd[3]+OM13*OM13)))));
FM11_1 = s->m[1]+s->m[3];
// = = Block_0_3_0_0_0_0 = =
// Symbolic Outputs
M[1][1] = FM11_1;
M[1][2] = CM21_33;
M[1][3] = CM23_1;
M[2][1] = CM21_33;
M[2][2] = CM22_33;
M[3][1] = CM23_1;
M[3][3] = CM23_3;
c[1] = FF11;
c[2] = CF2_33;
c[3] = CF23;
// ====== END Task 0 ======
}
double C2;
double S2;
double C3;
double S3;
double OM13;
double OM33;
double FA13;
double FA23;
double CF23;
double CM23_1;
double CM23_3;
double CF2_33;
double CM21_33;
double CM22_33;
double FF11;
double FM11_1;
//
//-------------------------------------------------------------
//
// 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//
//
// ==> Generation Date : Wed Feb 13 13:58:10 2019
//
// ==> Project name : CartPendulum
// ==> using XML input file
//
// ==> Number of joints : 3
//
// ==> Function : F19 : External Forces
// ==> Flops complexity : 89
//
// ==> Generation Time : 0.000 seconds
// ==> Post-Processing : 0.000 seconds
//
//-------------------------------------------------------------
//
#include <math.h>
#include "mbs_data.h"
#include "mbs_project_interface.h"
void mbs_extforces(double **frc,double **trq,
MbsData *s, double tsim)
// double frc[3][3];
// double trq[3][3];
{
double PxF1[4];
double RxF1[4][4];
double VxF1[4];
double OMxF1[4];
double AxF1[4];
double OMPxF1[4];
double *SWr1;
#include "mbs_extforces_CartPendulum.h"
#define q s->q
#define qd s->qd
#define qdd s->qdd
// === begin imp_aux ===
// === end imp_aux ===
// ===== BEGIN task 0 =====
// Sensor Kinematics
// = = Block_0_0_0_0_0_1 = =
// Trigonometric Variables
C2 = cos(q[2]);
S2 = sin(q[2]);
C3 = cos(q[3]);
S3 = sin(q[3]);
// = = Block_0_0_1_1_0_1 = =
// Sensor Kinematics
ROcp0_13 = C2*C3;
ROcp0_23 = S2*C3;
ROcp0_73 = C2*S3;
ROcp0_83 = S2*S3;
OMcp0_13 = -qd[3]*S2;
OMcp0_23 = qd[3]*C2;
OPcp0_13 = -(qd[2]*qd[3]*C2+qdd[3]*S2);
OPcp0_23 = -(qd[2]*qd[3]*S2-qdd[3]*C2);
RLcp0_14 = ROcp0_73*s->dpt[3][1];
RLcp0_24 = ROcp0_83*s->dpt[3][1];
RLcp0_34 = s->dpt[3][1]*C3;
ORcp0_14 = -(qd[2]*RLcp0_24-OMcp0_23*RLcp0_34);
ORcp0_24 = qd[2]*RLcp0_14-OMcp0_13*RLcp0_34;
ORcp0_34 = -qd[3]*s->dpt[3][1]*S3;
PxF1[1] = q[1]+RLcp0_14;
PxF1[2] = RLcp0_24;
PxF1[3] = RLcp0_34;
RxF1[1][1] = ROcp0_13;
RxF1[1][2] = ROcp0_23;
RxF1[1][3] = -S3;
RxF1[2][1] = -S2;
RxF1[2][2] = C2;
RxF1[2][3] = 0;
RxF1[3][1] = ROcp0_73;
RxF1[3][2] = ROcp0_83;
RxF1[3][3] = C3;
VxF1[1] = qd[1]+ORcp0_14;
VxF1[2] = ORcp0_24;
VxF1[3] = ORcp0_34;
OMxF1[1] = OMcp0_13;
OMxF1[2] = OMcp0_23;
OMxF1[3] = qd[2];
AxF1[1] = qdd[1]-qd[2]*ORcp0_24-qdd[2]*RLcp0_24+OMcp0_23*ORcp0_34+OPcp0_23*RLcp0_34;
AxF1[2] = qd[2]*ORcp0_14+qdd[2]*RLcp0_14-OMcp0_13*ORcp0_34-OPcp0_13*RLcp0_34;
AxF1[3] = OMcp0_13*ORcp0_24-OMcp0_23*ORcp0_14+OPcp0_13*RLcp0_24-OPcp0_23*RLcp0_14;
OMPxF1[1] = OPcp0_13;
OMPxF1[2] = OPcp0_23;
OMPxF1[3] = qdd[2];
// Sensor Forces Computation
SWr1 = user_ExtForces(PxF1,RxF1,VxF1,OMxF1,AxF1,OMPxF1,s,tsim,1);
// Sensor Dynamics : Forces projection on body-fixed frames
xfrc11 = ROcp0_13*SWr1[1]+ROcp0_23*SWr1[2]-SWr1[3]*S3;
xfrc21 = -(SWr1[1]*S2-SWr1[2]*C2);
xfrc31 = ROcp0_73*SWr1[1]+ROcp0_83*SWr1[2]+SWr1[3]*C3;
frc[1][3] = s->frc[1][3]+xfrc11;
frc[2][3] = s->frc[2][3]+xfrc21;
frc[3][3] = s->frc[3][3]+xfrc31;
xtrq11 = ROcp0_13*SWr1[4]+ROcp0_23*SWr1[5]-SWr1[6]*S3;
xtrq21 = -(SWr1[4]*S2-SWr1[5]*C2);
xtrq31 = ROcp0_73*SWr1[4]+ROcp0_83*SWr1[5]+SWr1[6]*C3;
trq[1][3] = s->trq[1][3]+xtrq11-xfrc21*(SWr1[9]-s->l[3][3])+xfrc31*SWr1[8];
trq[2][3] = s->trq[2][3]+xtrq21+xfrc11*(SWr1[9]-s->l[3][3])-xfrc31*SWr1[7];
trq[3][3] = s->trq[3][3]+xtrq31-xfrc11*SWr1[8]+xfrc21*SWr1[7];
// = = Block_0_0_1_1_1_0 = =
// Symbolic Outputs
frc[1][1] = s->frc[1][1];
frc[2][1] = s->frc[2][1];
frc[3][1] = s->frc[3][1];
trq[1][1] = s->trq[1][1];
trq[2][1] = s->trq[2][1];
trq[3][1] = s->trq[3][1];
// ====== END Task 0 ======
}
double C2;
double S2;
double C3;
double S3;
double ROcp0_13;
double ROcp0_23;
double ROcp0_73;
double ROcp0_83;
double OMcp0_13;
double OMcp0_23;
double OPcp0_13;
double OPcp0_23;
double RLcp0_14;
double RLcp0_24;
double RLcp0_34;
double ORcp0_14;
double ORcp0_24;
double ORcp0_34;
double xfrc11;
double xfrc21;
double xfrc31;
double xtrq11;
double xtrq21;
double xtrq31;
//
//-------------------------------------------------------------
//
// 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//
//
// ==> Generation Date : Wed Feb 13 13:58:10 2019
//
// ==> Project name : CartPendulum
// ==> using XML input file
//
// ==> Number of joints : 3
//
// ==> Function : F 6 : Sensors Kinematical Informations (sens)
// ==> Flops complexity : 20
//
// ==> Generation Time : 0.000 seconds
// ==> Post-Processing : 0.000 seconds
//
//-------------------------------------------------------------
//
#include <math.h>
#include "mbs_data.h"
#include "mbs_project_interface.h"
#include "mbs_sensor.h"
void mbs_gensensor(MbsSensor *sens,
MbsData *s,
int isens)
{
#include "mbs_gensensor_CartPendulum.h"
#define q s->q
#define qd s->qd
#define qdd s->qdd
// === begin imp_aux ===
// === end imp_aux ===
// ===== BEGIN task 0 =====
// Sensor Kinematics
// = = Block_0_0_0_0_0_1 = =
// Trigonometric Variables
C2 = cos(q[2]);
S2 = sin(q[2]);
C3 = cos(q[3]);
S3 = sin(q[3]);
// ====== END Task 0 ======
// ===== BEGIN task 1 =====
switch(isens)
{
//
break;
case 1:
// = = Block_1_0_0_1_1_0 = =
// Symbolic Outputs
sens->P[1] = q[1];
sens->R[1][1] = (1.0);
sens->R[2][2] = (1.0);
sens->R[3][3] = (1.0);
sens->V[1] = qd[1];
sens->A[1] = qdd[1];
//
break;
case 2:
// = = Block_1_0_0_2_1_0 = =
// Symbolic Outputs
sens->P[1] = q[1];
sens->R[1][1] = C2;
sens->R[1][2] = S2;
sens->R[2][1] = -S2;
sens->R[2][2] = C2;
sens->R[3][3] = (1.0);
sens->V[1] = qd[1];
sens->OM[3] = qd[2];
sens->A[1] = qdd[1];
sens->OMP[3] = qdd[2];
//
break;
case 3:
// = = Block_1_0_0_3_0_1 = =
// Sensor Kinematics
ROcp2_13 = C2*C3;
ROcp2_23 = S2*C3;
ROcp2_73 = C2*S3;
ROcp2_83 = S2*S3;
OMcp2_13 = -qd[3]*S2;
OMcp2_23 = qd[3]*C2;
OPcp2_13 = -(qdd[3]*S2+qd[2]*qd[3]*C2);
OPcp2_23 = qdd[3]*C2-qd[2]*qd[3]*S2;
// = = Block_1_0_0_3_1_0 = =
// Symbolic Outputs
sens->P[1] = q[1];
sens->R[1][1] = ROcp2_13;
sens->R[1][2] = ROcp2_23;
sens->R[1][3] = -S3;
sens->R[2][1] = -S2;
sens->R[2][2] = C2;
sens->R[3][1] = ROcp2_73;
sens->R[3][2] = ROcp2_83;
sens->R[3][3] = C3;
sens->V[1] = qd[1];
sens->OM[1] = OMcp2_13;
sens->OM[2] = OMcp2_23;
sens->OM[3] = qd[2];
sens->A[1] = qdd[1];
sens->OMP[1] = OPcp2_13;
sens->OMP[2] = OPcp2_23;
sens->OMP[3] = qdd[2];
break;
default:
break;
}
// ====== END Task 1 ======
}
double C2;
double S2;
double C3;
double S3;
double ROcp2_13;
double ROcp2_23;
double ROcp2_73;
double ROcp2_83;
double OMcp2_13;
double OMcp2_23;
double OPcp2_13;
double OPcp2_23;
//
//-------------------------------------------------------------
//
// 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//
//
// ==> Generation Date : Wed Feb 13 13:58:10 2019
//
// ==> Project name : CartPendulum
// ==> using XML input file
//
// ==> Number of joints : 3
//
// ==> Function : F 2 : Inverse Dynamics : RNEA
// ==> Flops complexity : 58
//
// ==> Generation Time : 0.000 seconds
// ==> Post-Processing : 0.000 seconds
//
//-------------------------------------------------------------
//
#include <math.h>
#include "mbs_data.h"
#include "mbs_project_interface.h"
void mbs_invdyna(double *Qq,
MbsData *s, double tsim)
// double Qq[3];
{
#include "mbs_invdyna_CartPendulum.h"
#define q s->q
#define qd s->qd
#define qdd s->qdd
// === begin imp_aux ===
// === end imp_aux ===
// ===== BEGIN task 0 =====
// = = Block_0_0_0_0_0_1 = =
// Trigonometric Variables