Skip to content

Commit d12e7fe

Browse files
author
Olivier Stasse
authored
Merge pull request #7 from flforget/topic/convergence_API
[Major] Add "getLastCommand" method to simlplify API
2 parents c850778 + 54fadde commit d12e7fe

31 files changed

+781
-133
lines changed

‎examples/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,10 @@ SET(source_files
1515
romeo_actuator/romeotorqueactuator.cpp
1616
temperature_control/costtemp.cpp
1717
temperature_control/dctemp.cpp
18+
linear/costLinear.cpp
19+
linear/modelLinear.cpp
20+
inverse_pendulum/costIP.cpp
21+
inverse_pendulum/modelIP.cpp
1822
)
1923

2024
SET(header_files
@@ -34,3 +38,5 @@ INSTALL(FILES ${header_files} DESTINATION include/ddp-actuator-solver/examples)
3438

3539
ADD_SUBDIRECTORY(romeo_actuator)
3640
ADD_SUBDIRECTORY(temperature_control)
41+
ADD_SUBDIRECTORY(linear)
42+
ADD_SUBDIRECTORY(inverse_pendulum)

‎examples/inverse_pendulum/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
_local_test_ddp( mainIP )

‎examples/inverse_pendulum/costIP.cpp‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
#include "costIP.hh"
2+
3+
CostIP::CostIP()
4+
{
5+
Q << 1.0,0.0,0.0,0.0,0.0,
6+
0.0,0.01,0.0,0.0,0.0,
7+
0.0,0.0,0.0,0.0,0.0,
8+
0.0,0.0,0.0,0.0,0.0,
9+
0.0,0.0,0.0,0.0,0.0;
10+
R << 0.0001;
11+
12+
lxx = Q;
13+
luu = R;
14+
lux << 0.0,0.0,0.0,0.0,0.0;
15+
lxu << 0.0,0.0,0.0,0.0,0.0;
16+
lx.setZero();
17+
final_cost = 0;
18+
running_cost = 0;
19+
}
20+
21+
void CostIP::computeCostAndDeriv(const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t& U)
22+
{
23+
running_cost = ((X - Xdes).transpose() * Q * (X - Xdes) + U.transpose() * R * U)
24+
(0, 0);
25+
lx = Q*(X-Xdes);
26+
lu = R*U;
27+
}
28+
29+
void CostIP::computeFinalCostAndDeriv(const stateVec_t& X,const stateVec_t& Xdes)
30+
{
31+
lx = 1.0*Q*(X-Xdes);
32+
lxx = 1.0*Q;
33+
}

‎examples/inverse_pendulum/costIP.hh‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
#ifndef COSTIP_H
2+
#define COSTIP_H
3+
4+
#include <ddp-actuator-solver/costfunction.hh>
5+
6+
class CostIP : public CostFunction<double,5,1>
7+
{
8+
public:
9+
CostIP();
10+
private:
11+
stateMat_t Q;
12+
commandMat_t R;
13+
double dt;
14+
protected:
15+
// attributes //
16+
public:
17+
private:
18+
19+
protected:
20+
// methods //
21+
public:
22+
void computeCostAndDeriv(const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t& U);
23+
void computeFinalCostAndDeriv(const stateVec_t& X,const stateVec_t& Xdes);
24+
private:
25+
protected:
26+
// accessors //
27+
public:
28+
29+
};
30+
31+
#endif // COST_H

‎examples/inverse_pendulum/mainIP.cpp‎

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
#include <iostream>
2+
#include <fstream>
3+
4+
#include <ddp-actuator-solver/ddpsolver.hh>
5+
#include "modelIP.hh"
6+
#include "costIP.hh"
7+
8+
#include <time.h>
9+
#include <sys/time.h>
10+
11+
12+
using namespace std;
13+
using namespace Eigen;
14+
15+
int main()
16+
{
17+
struct timeval tbegin,tend;
18+
double texec=0.0;
19+
DDPSolver<double,5,1>::stateVec_t xinit,xDes,x;
20+
DDPSolver<double,5,1>::commandVec_t u;
21+
22+
xinit << -1.0,0.0,-100.0,0.0,0.0;
23+
xDes << 0.5,0.0,0.0,0.0,0.0;
24+
25+
unsigned int i;
26+
unsigned int T = 3000;
27+
double dt=1e-3;
28+
unsigned int iterMax = 100;
29+
double stopCrit = 0.01;
30+
DDPSolver<double,5,1>::stateVecTab_t xList;
31+
DDPSolver<double,5,1>::commandVecTab_t uList;
32+
DDPSolver<double,5,1>::traj lastTraj;
33+
34+
ModelIP model(dt);
35+
ModelIP* noisyModel=NULL;
36+
CostIP cost;
37+
DDPSolver<double,5,1> solver(model,cost,DISABLE_FULLDDP,DISABLE_QPBOX);
38+
solver.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
39+
40+
int N = 100;
41+
gettimeofday(&tbegin,NULL);
42+
solver.solveTrajectory();
43+
gettimeofday(&tend,NULL);
44+
45+
lastTraj = solver.getLastSolvedTrajectory();
46+
xList = lastTraj.xList;
47+
uList = lastTraj.uList;
48+
unsigned int iter = lastTraj.iter;
49+
50+
51+
texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
52+
texec /= N;
53+
54+
cout << endl;
55+
cout << "temps d'execution total du solveur ";
56+
cout << texec << endl;
57+
cout << "temps d'execution par pas de temps ";
58+
cout << texec/T << endl;
59+
cout << "Nombre d'itérations : " << iter << endl;
60+
61+
62+
ofstream fichier1("results1.csv",ios::out | ios::trunc);
63+
if(fichier1)
64+
{
65+
fichier1 << "tau,tauDot,q,qDot,u" << endl;
66+
x = xinit;
67+
fichier1 << x(0, 0) << "," << x(1, 0) << "," << x(2, 0) << ","
68+
<< x(3, 0) << "," << uList[0] << endl;
69+
for (i = 1; i < T; i++)
70+
{
71+
x = model.computeNextState(dt, x, uList[i - 1]);
72+
fichier1 << x(0, 0) << "," << x(1, 0) << "," << x(2, 0) << ","
73+
<< x(3, 0) << "," << uList[i - 1] << endl;
74+
}
75+
fichier1 << xList[T](0, 0) << "," << xList[T](1, 0) << "," << xList[T](2, 0) << "," << xList[T](3, 0) << ","
76+
<< uList[T - 1](0, 0) << endl;
77+
fichier1.close();
78+
}
79+
else
80+
cerr << "erreur ouverte fichier" << endl;
81+
82+
ofstream fichier2("results2.csv",ios::out | ios::trunc);
83+
if(fichier2)
84+
{
85+
fichier2 << "tau,tauDot,q,qDot,u" << endl;
86+
fichier2 << T << ',' << 0 << endl;
87+
for(int j=0;j<0;j++)
88+
{
89+
noisyModel = new ModelIP(dt,1);
90+
fichier2 << xList[i](0, 0) << "," << xList[i](1, 0) << "," << xList[i](2, 0) << "," << xList[i](3, 0) << ","
91+
<< uList[i](0, 0) << endl;
92+
x = xinit;
93+
for (i = 1; i < T; i++)
94+
{
95+
x = noisyModel->computeNextState(dt, x, uList[i - 1]);
96+
fichier2 << x(0, 0) << "," << x(1, 0) << "," << x(2, 0) << ","
97+
<< x(3, 0) << "," << uList[i - 1] << endl;
98+
}
99+
fichier2 << xList[T](0, 0) << "," << xList[T](1, 0) << "," << xList[T](2, 0) << "," << xList[T](3, 0) << ","
100+
<< uList[T - 1](0, 0) << endl;
101+
delete noisyModel;
102+
}
103+
fichier2.close();
104+
}
105+
else
106+
cerr << "erreur ouverte fichier" << endl;
107+
108+
109+
return 0;
110+
111+
}

‎examples/inverse_pendulum/modelIP.cpp‎

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
#include <math.h>
2+
#include <eigen3/unsupported/Eigen/MatrixFunctions>
3+
#include <sys/time.h>
4+
5+
#include <iostream>
6+
7+
#include "modelIP.hh"
8+
9+
/*
10+
* x0 -> actuator position
11+
* x1 -> actuator speed
12+
* x2 -> motor temperature
13+
* x3 -> external torque
14+
* x4 -> ambiant temperature
15+
*/
16+
17+
ModelIP::ModelIP(double& mydt, bool noiseOnParameters)
18+
{
19+
stateNb = 5;
20+
commandNb = 1;
21+
dt = mydt;
22+
23+
if (!noiseOnParameters)
24+
{
25+
J = 119e-7;
26+
K_M = 77.1e-3;
27+
f_VL = 0.429e-6;
28+
R_th = 2.8;
29+
tau_th = 15.7;
30+
}
31+
else
32+
{
33+
J = 119e-17;
34+
K_M = 77.1e-3;
35+
f_VL = 0.429e-6;
36+
R_th = 2.8;
37+
tau_th = 15.7;
38+
}
39+
40+
Id.setIdentity();
41+
42+
fu.setZero();
43+
fx.setZero();
44+
45+
fxx[0].setZero();
46+
fxx[1].setZero();
47+
fxx[2].setZero();
48+
fxx[3].setZero();
49+
fxx[4].setZero();
50+
51+
fxu[0].setZero();
52+
fxu[0].setZero();
53+
54+
fuu[0].setZero();
55+
fux[0].setZero();
56+
fxu[0].setZero();
57+
58+
QxxCont.setZero();
59+
QuuCont.setZero();
60+
QuxCont.setZero();
61+
62+
lowerCommandBounds << -1.0;
63+
upperCommandBounds << 1.0;
64+
}
65+
66+
ModelIP::stateVec_t ModelIP::computeDeriv(double&, const stateVec_t& X,
67+
const commandVec_t &U)
68+
{
69+
dX[0] = X[1];
70+
dX[1] = (K_M / J) * U[0] - (f_VL / J) * X[1] - (1.0 / J) * X[3];
71+
dX[2] = R_th * U[0] * U[0] - (X[2] - X[4]) / tau_th;
72+
dX[3] = 0.0;
73+
dX[4] = 0.0;
74+
//std::cout << dX.transpose() << std::endl;
75+
return dX;
76+
}
77+
78+
ModelIP::stateVec_t ModelIP::computeNextState(double& dt, const stateVec_t& X,
79+
const commandVec_t& U)
80+
{
81+
k1 = computeDeriv(dt, X, U);
82+
k2 = computeDeriv(dt, X + (dt / 2) * k1, U);
83+
k3 = computeDeriv(dt, X + (dt / 2) * k2, U);
84+
k4 = computeDeriv(dt, X + dt * k3, U);
85+
x_next = X + (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4);
86+
return x_next;
87+
}
88+
89+
void ModelIP::computeModelDeriv(double& dt, const stateVec_t& X,
90+
const commandVec_t& U)
91+
{
92+
double dh = 1e-7;
93+
stateVec_t Xp, Xm;
94+
Xp = X;
95+
Xm = X;
96+
for (unsigned int i = 0; i < stateNb; i++)
97+
{
98+
Xp[i] += dh / 2;
99+
Xm[i] -= dh / 2;
100+
fx.col(i) = (computeNextState(dt, Xp, U) - computeNextState(dt, Xm, U))
101+
/ dh;
102+
Xp = X;
103+
Xm = X;
104+
}
105+
}
106+
107+
ModelIP::stateMat_t ModelIP::computeTensorContxx(const stateVec_t& )
108+
{
109+
return QxxCont;
110+
}
111+
112+
ModelIP::commandMat_t ModelIP::computeTensorContuu(const stateVec_t& )
113+
{
114+
return QuuCont;
115+
}
116+
117+
ModelIP::commandR_stateC_t ModelIP::computeTensorContux(const stateVec_t& )
118+
{
119+
return QuxCont;
120+
}

‎examples/inverse_pendulum/modelIP.hh‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
#ifndef MODEL_H
2+
#define MODEL_H
3+
4+
#include <ddp-actuator-solver/dynamicmodel.hh>
5+
6+
class ModelIP : public DynamicModel<double,5,1>
7+
{
8+
public:
9+
ModelIP(double& mydt,bool noiseOnParameters=0);
10+
virtual ~ModelIP() {};
11+
private:
12+
protected:
13+
14+
// attributes //
15+
public:
16+
private:
17+
double dt;
18+
private:
19+
double J;
20+
double K_M;
21+
double f_VL;
22+
double R_th;
23+
double tau_th;
24+
private:
25+
stateVec_t Xreal,dX;
26+
stateVec_t x_next,k1,k2,k3,k4;
27+
stateMat_t Id;
28+
29+
stateMat_t QxxCont;
30+
commandMat_t QuuCont;
31+
commandR_stateC_t QuxCont;
32+
33+
protected:
34+
// methods //
35+
public:
36+
stateVec_t computeDeriv(double& dt, const stateVec_t& X, const commandVec_t &U);
37+
stateVec_t computeNextState(double& dt, const stateVec_t& X, const commandVec_t &U);
38+
void computeModelDeriv(double& dt, const stateVec_t& X, const commandVec_t &U);
39+
stateMat_t computeTensorContxx(const stateVec_t& nextVx);
40+
commandMat_t computeTensorContuu(const stateVec_t& nextVx);
41+
commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);
42+
private:
43+
protected:
44+
// accessors //
45+
public:
46+
47+
};
48+
49+
#endif // MODEL_H

‎examples/linear/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
_local_test_ddp( mainLinear )

0 commit comments

Comments
 (0)