Commit affd1541723093ce67d8ff55071f01bd9e78273c

Authored by Fnadi Mohamed
1 parent d62ddeed

DynIbex Application to solve high-degree ODEs

Simulations/Makefile 0 → 100644
... ... @@ -0,0 +1,47 @@
  1 +# Directories
  2 +SRCDIR=src
  3 +TESTDIR=tests
  4 +DEPDIR=deps
  5 +OBJDIR=objs
  6 +
  7 +# Find all sources
  8 +SRC = $(shell find $(SRCDIR) -name "*.cpp")
  9 +OTMP = $(patsubst %.cpp,%.o,$(SRC:$(SRCDIR)/%=%))
  10 +OBJS = $(patsubst %,$(OBJDIR)/%,$(OTMP))
  11 +DEPS = $(patsubst %.o,$(DEPDIR)/%.d,$(OTMP))
  12 +
  13 +# Variables
  14 +CC = g++
  15 +CPPFLAGS =-O3 -Iheaders -std=c++11 $(shell pkg-config --cflags ibex) # -Wall -Wextra
  16 +LIBS = -lpthread $(shell pkg-config --libs ibex) -fopenmp
  17 +TESTLIBS = -lgtest -L/usr/lib -pthread
  18 +
  19 +all: init $(OBJS)
  20 + @echo Building the executable...
  21 + $(CC) -o main $(OBJS) $(LIBS)
  22 +
  23 +init:
  24 + @echo Initilizing...
  25 + mkdir -p $(DEPDIR)
  26 + mkdir -p $(OBJDIR)
  27 +
  28 +build_test:
  29 + g++ $(TESTDIR)/tests.cpp -o test $(filter-out $(OBJDIR)/main.o,$(OBJS)) $(LIBS) $(TESTLIBS) -Iheaders $(CPPFLAGS)
  30 +
  31 +test: build_test init
  32 + ./test
  33 +
  34 +clean:
  35 + @echo Clearing...
  36 + rm -fr $(OBJDIR)/ $(DEPDIR)/
  37 +
  38 +.PHONY: all clean test
  39 +
  40 +-include $(DEPS)
  41 +
  42 +$(OBJDIR)/%.o: $(SRCDIR)/%.cpp
  43 + mkdir -p $(dir $@)
  44 + mkdir -p $(DEPDIR)/$(dir $(@:$(OBJDIR)/%=%))
  45 + $(eval CPP = $(shell find $(SRCDIR)/$(dir $*) -name "$(notdir $*).cpp"))
  46 + $(CC) -c $(CPPFLAGS) $(CPP) -o $@ $(LIBS)
  47 + $(CC) -MM $(CPPFLAGS) $(CPP) $(LIBS) > $(DEPDIR)/$*.d
... ...
Simulations/src/opened_loop_pendule.cpp 0 → 100644
... ... @@ -0,0 +1,122 @@
  1 +/* ============================================================================
  2 + * D Y N I B E X - Example for a first validated simulation : system 61 of vericomp
  3 + * ============================================================================
  4 + * ---------------------------------------------------------------------------- */
  5 +
  6 +#include "ibex.h"
  7 +#include <math.h>
  8 +
  9 +using namespace ibex;
  10 +using namespace std;
  11 +
  12 +#define __PREC__ 1e-6 // Précision
  13 +#define __METH__ HEUN // Méthode de résolution de ODE RK4 HEUN
  14 +#define __DURATION__ 6 // Duréé de simulation
  15 +
  16 +int main(){
  17 +
  18 + //========================================================================
  19 + // Déclaration des constantes (Pendule inverse + Moteur)
  20 + //========================================================================
  21 + Interval mp(0.08); // masse du pendule [kg]
  22 + Interval ma(0.2); // masse du bras rotatif [kg]
  23 + Interval M(0.01); // masse accrochée [kg]
  24 + Interval la(0.1); // longueur du bras rotatif [m]
  25 + Interval lp(0.15); // longeur du pendule [m]
  26 + Interval r0(0.05); // longeur par rapport à la base
  27 + Interval N(5.0); // Rapport de réduction engrenage
  28 + Interval Ja(1834.0*0.68*1e-6); // inertie bras rotatif [kg.m^2]
  29 + Interval Jp(562.802*0.68*1e-6); // inertie pendule [kg.m^2]
  30 + Interval g(9.81); // gravité
  31 +
  32 +
  33 +
  34 +
  35 + // Moteur courant continu DCX22LG - MDP
  36 +
  37 + Interval Ke(1.0/((981.0*2.0*M_PI)/60.0)); // Constante de vitesse [981 tr/min/V]
  38 + Interval Kc(9.73e-3); // Constante de couple [9.73 mNm/A]
  39 + Interval L(0.0346e-3); // Inductance [Heneri]
  40 + Interval f(0); // coeficient frottement visqueux
  41 + Interval Rm(0.335); // Résistance de l'induit (ohm)
  42 + Interval Jm(9.06e-7); // Inertie Moteur [g.cm^2]
  43 + Interval mu_m(0.906); // Rendement Moteur
  44 + Interval Im(0.0818); // Courant à vide
  45 + Interval Cr(Kc*Im); // Couple résistif à vide
  46 + Interval Nm(5.2); // Rapport du réduction moteur
  47 + Interval Rv(34.0*1e3); // Pente vitesse/couple [tr/mn/mN]
  48 + Interval w0(2052.0/2.0); // Vitesse à vide
  49 + Interval eta_m(0.9); // rendement moteur
  50 +
  51 + // Paramètres dynmaiques
  52 +
  53 +
  54 + Interval mu1(pow(lp,2)*(mp/4.0+M)+Jp); // Dynamic parameter
  55 + Interval mu2(pow(la,2)*(mp+M)+Jm/(pow(N,2))+Ja); // Dynamic parameter
  56 + Interval mu3(lp*la*(mp/2+M)); // Dynamic parameter
  57 + Interval mu4(pow(lp,2)*(mp/4.0+M)+Jp); // Dynamic parameter
  58 + Interval mug(lp*g*(mp/2+M));
  59 +
  60 +
  61 + Interval al(0.0033472); // Dynamic parameter
  62 + Interval be(0.0038852); // Dynamic parameter
  63 + Interval ga(0.0024879); // Dynamic parameter
  64 + Interval del(0.097625); // Dynamic parameter
  65 +
  66 + cout << "mu1 = " << mu1 << "\t mu2 = " << mu2 << "\t mu3 = " << mu3 << "\t mu4 = " << mu4 << "\t mug = " << mug <<endl;
  67 +
  68 + Interval fv1(0.16); // Viscous Colomb friction coefficient joint 1 0.07619 (50 deg : IA : 0.13 LSMI : 0.06) 0.085
  69 + Interval fc1(0.0); // Static Colomb friction coefficient joint 1
  70 + Interval fv2(0.0006); // Viscous Colomb friction coefficient joint 2 (50 deg : IA: 0.00052 LSMI : 0.000382) 0.00043
  71 + Interval fc2(0.000); // Static Colomb friction coefficient joint 2
  72 +
  73 +
  74 +
  75 + //========================================================================
  76 + // Relation between velocity/torque of the motor (Inputs) ((n0-n)/(Rv*Nm))
  77 + //========================================================================
  78 + Interval w(0.170); // Vitesse moteur souhaitée rad/s
  79 + Interval GAMMA1(-(N*w0*(1-w))/(Rv*eta_m)); // Troque of the joint 1 including friction
  80 + cout <<"Couple moteur en N.m : \t" << GAMMA1 <<endl;
  81 + Interval GAMMA2(-0.0,0.); // Troque of the joint 2 including friction
  82 +
  83 +
  84 + const int n= 4;
  85 + Variable y(n);
  86 + IntervalVector yinit(n);
  87 + yinit[0] = Interval(13.4*(M_PI/180.0));
  88 + yinit[1] = Interval(0.0);
  89 + yinit[2] = Interval(-79.25*(M_PI/180.0));
  90 + yinit[3] = Interval(0.0);
  91 +
  92 + //=============================================================================
  93 + // Dynamique du pendule inverse avec incertitude sur ses paramètres
  94 + //=============================================================================
  95 +
  96 + Function ydot = Function(y,Return( y[1],
  97 + (1/(-pow(mu3,2)*pow(cos(y[2]),2)+mu1*mu4*pow(sin(y[2]),2)+mu2*mu4))*(-mu3*cos(y[2])*(mu1*sin(y[2])*cos(y[2])*pow(y[1],2)+GAMMA2 - fv2*y[3]-mug*sin(y[2]))+mu4*(mu3*sin(y[2])*pow(y[3],2)-mu1*sin(2*y[2])*y[1]*y[3]+GAMMA1 - fv1*y[1])),
  98 + y[3],
  99 + (1/(-pow(mu3,2)*pow(cos(y[2]),2)+mu1*mu4*pow(sin(y[2]),2)+mu2*mu4))*((mu1*pow(sin(y[2]),2)+mu2)*(mu1*cos(y[2])*sin(y[2])*pow(y[1],2)+GAMMA2-fv2*y[3]-mug*sin(y[2]))-mu3*cos(y[2])*(mu3*sin(y[2])*pow(y[3],2)-2*mu1*cos(y[2])*sin(y[2])*y[1]*y[3]+GAMMA1 -fv1*y[1]))
  100 + ));
  101 +
  102 + // Function ydot = Function(y,Return( y[1],
  103 + // (1/(al*be-pow(ga,2)+(pow(be,2)+pow(ga,2))*pow(sin(y[2]),2)))*(be*ga*(pow(sin(y[2]),2)-1)*sin(y[2])*y[1]-2*pow(be,2)*cos(y[2])*sin(y[2])*y[3]+be*ga*sin(y[2])*pow(y[3],2)-ga*del*cos(y[2])*sin(y[2])+be*GAMMA1-ga*cos(y[2])*GAMMA2),
  104 + // y[3],
  105 + // (1/(al*be-pow(ga,2)+(pow(be,2)+pow(ga,2))*pow(sin(y[2]),2)))*(be*(al+be*pow(sin(y[2]),2))*cos(y[2])*sin(y[2])*pow(y[1],2)-2*be*ga*(1-pow(sin(y[2]),2))*sin(y[2])*y[1]*y[3]-pow(ga,2)*cos(y[2])*sin(y[2])*y[3]+del*(al+be*pow(sin(y[2]),2))*sin(y[2])-ga*cos(y[2])*GAMMA1+(al+be*pow(sin(y[2]),2))*GAMMA1)
  106 + // ));
  107 +
  108 + ivp_ode problem = ivp_ode(ydot,0.0,yinit);
  109 +
  110 +
  111 + // Integration numerique ensembliste
  112 + simulation simu = simulation(&problem,__DURATION__, __METH__, __PREC__);
  113 + simu.run_simulation();
  114 +
  115 + //For an export in order to plot
  116 + simu.export_y0("/home/mohamed/Desktop/NMPC_Pendule/export-q1.txt");
  117 + simu.export1d_yn("/home/mohamed/Desktop/NMPC_Pendule/export-opend-loop-q2.txt",2);
  118 +
  119 +
  120 + return 0;
  121 +
  122 +}
... ...