Skip to content
GitLab
Commit affd1541 authored by Fnadi Mohamed's avatar Fnadi Mohamed
Browse files

DynIbex Application to solve high-degree ODEs

parent d62ddeed
Show whitespace changes
Inline Side-by-side
Simulations/Makefile 0 → 100644
View file @ affd1541
# Directories
SRCDIR=src
TESTDIR=tests
DEPDIR=deps
OBJDIR=objs
# Find all sources
SRC = $(shell find $(SRCDIR) -name "*.cpp")
OTMP = $(patsubst %.cpp,%.o,$(SRC:$(SRCDIR)/%=%))
OBJS = $(patsubst %,$(OBJDIR)/%,$(OTMP))
DEPS = $(patsubst %.o,$(DEPDIR)/%.d,$(OTMP))
# Variables
CC = g++
CPPFLAGS =-O3 -Iheaders -std=c++11 $(shell pkg-config --cflags ibex) # -Wall -Wextra
LIBS = -lpthread $(shell pkg-config --libs ibex) -fopenmp
TESTLIBS = -lgtest -L/usr/lib -pthread
all: init $(OBJS)
@echo Building the executable...
$(CC) -o main $(OBJS) $(LIBS)
init:
@echo Initilizing...
mkdir -p $(DEPDIR)
mkdir -p $(OBJDIR)
build_test:
g++ $(TESTDIR)/tests.cpp -o test $(filter-out $(OBJDIR)/main.o,$(OBJS)) $(LIBS) $(TESTLIBS) -Iheaders $(CPPFLAGS)
test: build_test init
./test
clean:
@echo Clearing...
rm -fr $(OBJDIR)/ $(DEPDIR)/
.PHONY: all clean test
-include $(DEPS)
$(OBJDIR)/%.o: $(SRCDIR)/%.cpp
mkdir -p $(dir $@)
mkdir -p $(DEPDIR)/$(dir $(@:$(OBJDIR)/%=%))
$(eval CPP = $(shell find $(SRCDIR)/$(dir $*) -name "$(notdir $*).cpp"))
$(CC) -c $(CPPFLAGS) $(CPP) -o $@ $(LIBS)
$(CC) -MM $(CPPFLAGS) $(CPP) $(LIBS) > $(DEPDIR)/$*.d
Simulations/src/opened_loop_pendule.cpp 0 → 100644
View file @ affd1541
/* ============================================================================
* D Y N I B E X - Example for a first validated simulation : system 61 of vericomp
* ============================================================================
* ---------------------------------------------------------------------------- */
#include "ibex.h"
#include <math.h>
using namespace ibex;
using namespace std;
#define __PREC__ 1e-6 // Précision
#define __METH__ HEUN // Méthode de résolution de ODE RK4 HEUN
#define __DURATION__ 6 // Duréé de simulation
int main(){
//========================================================================
// Déclaration des constantes (Pendule inverse + Moteur)
//========================================================================
Interval mp(0.08); // masse du pendule [kg]
Interval ma(0.2); // masse du bras rotatif [kg]
Interval M(0.01); // masse accrochée [kg]
Interval la(0.1); // longueur du bras rotatif [m]
Interval lp(0.15); // longeur du pendule [m]
Interval r0(0.05); // longeur par rapport à la base
Interval N(5.0); // Rapport de réduction engrenage
Interval Ja(1834.0*0.68*1e-6); // inertie bras rotatif [kg.m^2]
Interval Jp(562.802*0.68*1e-6); // inertie pendule [kg.m^2]
Interval g(9.81); // gravité
// Moteur courant continu DCX22LG - MDP
Interval Ke(1.0/((981.0*2.0*M_PI)/60.0)); // Constante de vitesse [981 tr/min/V]
Interval Kc(9.73e-3); // Constante de couple [9.73 mNm/A]
Interval L(0.0346e-3); // Inductance [Heneri]
Interval f(0); // coeficient frottement visqueux
Interval Rm(0.335); // Résistance de l'induit (ohm)
Interval Jm(9.06e-7); // Inertie Moteur [g.cm^2]
Interval mu_m(0.906); // Rendement Moteur
Interval Im(0.0818); // Courant à vide
Interval Cr(Kc*Im); // Couple résistif à vide
Interval Nm(5.2); // Rapport du réduction moteur
Interval Rv(34.0*1e3); // Pente vitesse/couple [tr/mn/mN]
Interval w0(2052.0/2.0); // Vitesse à vide
Interval eta_m(0.9); // rendement moteur
// Paramètres dynmaiques
Interval mu1(pow(lp,2)*(mp/4.0+M)+Jp); // Dynamic parameter
Interval mu2(pow(la,2)*(mp+M)+Jm/(pow(N,2))+Ja); // Dynamic parameter
Interval mu3(lp*la*(mp/2+M)); // Dynamic parameter
Interval mu4(pow(lp,2)*(mp/4.0+M)+Jp); // Dynamic parameter
Interval mug(lp*g*(mp/2+M));
Interval al(0.0033472); // Dynamic parameter
Interval be(0.0038852); // Dynamic parameter
Interval ga(0.0024879); // Dynamic parameter
Interval del(0.097625); // Dynamic parameter
cout << "mu1 = " << mu1 << "\t mu2 = " << mu2 << "\t mu3 = " << mu3 << "\t mu4 = " << mu4 << "\t mug = " << mug <<endl;
Interval fv1(0.16); // Viscous Colomb friction coefficient joint 1 0.07619 (50 deg : IA : 0.13 LSMI : 0.06) 0.085
Interval fc1(0.0); // Static Colomb friction coefficient joint 1
Interval fv2(0.0006); // Viscous Colomb friction coefficient joint 2 (50 deg : IA: 0.00052 LSMI : 0.000382) 0.00043
Interval fc2(0.000); // Static Colomb friction coefficient joint 2
//========================================================================
// Relation between velocity/torque of the motor (Inputs) ((n0-n)/(Rv*Nm))
//========================================================================
Interval w(0.170); // Vitesse moteur souhaitée rad/s
Interval GAMMA1(-(N*w0*(1-w))/(Rv*eta_m)); // Troque of the joint 1 including friction
cout <<"Couple moteur en N.m : \t" << GAMMA1 <<endl;
Interval GAMMA2(-0.0,0.); // Troque of the joint 2 including friction
const int n= 4;
Variable y(n);
IntervalVector yinit(n);
yinit[0] = Interval(13.4*(M_PI/180.0));
yinit[1] = Interval(0.0);
yinit[2] = Interval(-79.25*(M_PI/180.0));
yinit[3] = Interval(0.0);
//=============================================================================
// Dynamique du pendule inverse avec incertitude sur ses paramètres
//=============================================================================
Function ydot = Function(y,Return( y[1],
(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])),
y[3],
(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]))
));
// Function ydot = Function(y,Return( y[1],
// (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),
// y[3],
// (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)
// ));
ivp_ode problem = ivp_ode(ydot,0.0,yinit);
// Integration numerique ensembliste
simulation simu = simulation(&problem,__DURATION__, __METH__, __PREC__);
simu.run_simulation();
//For an export in order to plot
simu.export_y0("/home/mohamed/Desktop/NMPC_Pendule/export-q1.txt");
simu.export1d_yn("/home/mohamed/Desktop/NMPC_Pendule/export-opend-loop-q2.txt",2);
return 0;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment