Skip to content

Commit 835df5b

Browse files
author
Olivier Stasse
authored
Merge pull request #5 from olivier-stasse/master
Format package for inclusion in robotpkg.
2 parents 91e7f27 + fd3e076 commit 835df5b

File tree

6 files changed

+240
-223
lines changed

6 files changed

+240
-223
lines changed

‎examples/CMakeLists.txt‎

Lines changed: 14 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ macro(_local_test_ddp)
33
set(list_var "${ARGN}")
44
foreach(loop_var IN LISTS list_var)
55
ADD_EXECUTABLE(${loop_var} ${loop_var}.cpp)
6-
TARGET_LINK_LIBRARIES(${loop_var} ${PROJECT_NAME} ${qpOASES_LIBRARIES})
6+
TARGET_LINK_LIBRARIES(${loop_var} ${PROJECT_NAME}-examples ${qpOASES_LIBRARIES})
77
PKG_CONFIG_USE_DEPENDENCY(${loop_var} eigen3)
88
endforeach()
99
endmacro()
@@ -16,9 +16,20 @@ SET(source_files
1616
temperature_control/dctemp.cpp
1717
)
1818

19-
ADD_LIBRARY(${PROJECT_NAME} SHARED ${source_files})
19+
SET(header_files
20+
romeo_actuator/costfunctionromeoactuator.hh
21+
romeo_actuator/romeosimpleactuator.hh
22+
romeo_actuator/romeotorqueactuator.hh
23+
temperature_control/costtemp.hh
24+
temperature_control/dctemp.hh
25+
)
2026

21-
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} eigen3)
27+
ADD_LIBRARY(${PROJECT_NAME}-examples SHARED ${source_files})
28+
29+
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME}-examples eigen3)
30+
31+
INSTALL(TARGETS ${PROJECT_NAME}-examples LIBRARY DESTINATION lib)
32+
INSTALL(FILES ${header_files} DESTINATION include)
2233

2334
ADD_SUBDIRECTORY(romeo_actuator)
2435
ADD_SUBDIRECTORY(temperature_control)

‎examples/temperature_control/dctemp.cpp‎

Lines changed: 59 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -14,30 +14,32 @@
1414
* x4 -> ambiant temperature
1515
*/
1616

17-
DCTemp::DCTemp(double& mydt, bool noiseOnParameters)
17+
DCTemp::DCTemp(bool noiseOnParameters)
1818
{
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-
}
19+
stateNb=5;
20+
commandNb=1;
21+
22+
if(!noiseOnParameters)
23+
{
24+
J_ = 119e-7;
25+
K_M_=77.1e-3;
26+
f_VL_=0.429e-6;
27+
R_th_=2.8;
28+
tau_th_=15.7;
29+
}
3130
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-
}
31+
{
32+
J_ = 119e-17;
33+
K_M_=77.1e-3;
34+
f_VL_=0.429e-6;
35+
R_th_=2.8;
36+
tau_th_=15.7;
37+
}
3938

40-
Id.setIdentity();
39+
Id_.setIdentity();
40+
41+
fu.setZero();
42+
fx.setZero();
4143

4244
fu.setZero();
4345
fx.setZero();
@@ -55,66 +57,68 @@ DCTemp::DCTemp(double& mydt, bool noiseOnParameters)
5557
fux[0].setZero();
5658
fxu[0].setZero();
5759

58-
QxxCont.setZero();
59-
QuuCont.setZero();
60-
QuxCont.setZero();
60+
QxxCont_.setZero();
61+
QuuCont_.setZero();
62+
QuxCont_.setZero();
6163

6264
lowerCommandBounds << -1.0;
6365
upperCommandBounds << 1.0;
6466
}
6567

66-
DCTemp::stateVec_t DCTemp::computeDeriv(double&, const stateVec_t& X,
67-
const commandVec_t &U)
68+
DCTemp::stateVec_t DCTemp::computeDeriv(double& ,
69+
const stateVec_t& X,
70+
const commandVec_t &U)
6871
{
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;
72+
dX_[0] = X[1];
73+
dX_[1] = (K_M_/J_)*U[0] - (f_VL_/J_)*X[1] - (1.0/J_)*X[3];
74+
dX_[2] = R_th_*U[0]*U[0] - (X[2]-X[4])/tau_th_;
75+
dX_[3] = 0.0;
76+
dX_[4] = 0.0;
7477
//std::cout << dX.transpose() << std::endl;
75-
return dX;
78+
return dX_;
7679
}
7780

78-
DCTemp::stateVec_t DCTemp::computeNextState(double& dt, const stateVec_t& X,
79-
const commandVec_t& U)
81+
DCTemp::stateVec_t DCTemp::computeNextState(double& dt,
82+
const stateVec_t& X,
83+
const commandVec_t& U)
8084
{
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;
85+
k1_ = computeDeriv(dt,X,U);
86+
k2_ = computeDeriv(dt,X+(dt/2)*k1_,U);
87+
k3_ = computeDeriv(dt,X+(dt/2)*k2_,U);
88+
k4_ = computeDeriv(dt,X+dt*k3_,U);
89+
x_next_ = X + (dt_/6)*(k1_+2*k2_+2*k3_+k4_);
90+
return x_next_;
8791
}
8892

89-
void DCTemp::computeAllModelDeriv(double& dt, const stateVec_t& X,
90-
const commandVec_t& U)
93+
void DCTemp::computeAllModelDeriv(double& dt,
94+
const stateVec_t& X,
95+
const commandVec_t& U)
9196
{
9297
double dh = 1e-7;
93-
stateVec_t Xp, Xm;
98+
stateVec_t Xp,Xm;
9499
Xp = X;
95100
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-
}
101+
for(unsigned int i=0;i<stateNb;i++)
102+
{
103+
Xp[i] += dh/2;
104+
Xm[i] -= dh/2;
105+
fx.col(i) = (computeNextState(dt, Xp, U) - computeNextState(dt, Xm, U))/dh;
106+
Xp = X;
107+
Xm = X;
108+
}
105109
}
106110

107111
DCTemp::stateMat_t DCTemp::computeTensorContxx(const stateVec_t& )
108112
{
109-
return QxxCont;
113+
return QxxCont_;
110114
}
111115

112116
DCTemp::commandMat_t DCTemp::computeTensorContuu(const stateVec_t& )
113117
{
114-
return QuuCont;
118+
return QuuCont_;
115119
}
116120

117121
DCTemp::commandR_stateC_t DCTemp::computeTensorContux(const stateVec_t& )
118122
{
119-
return QuxCont;
123+
return QuxCont_;
120124
}

‎examples/temperature_control/dctemp.hh‎

Lines changed: 36 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -5,45 +5,45 @@
55

66
class DCTemp : public DynamicModel<double,5,1>
77
{
8-
public:
9-
DCTemp(double& mydt,bool noiseOnParameters=0);
10-
virtual ~DCTemp() {};
11-
private:
12-
protected:
8+
public:
9+
DCTemp(bool noiseOnParameters=0);
10+
virtual ~DCTemp(){};
11+
private:
12+
protected:
1313

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;
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_;
2828

29-
stateMat_t QxxCont;
30-
commandMat_t QuuCont;
31-
commandR_stateC_t QuxCont;
29+
stateMat_t QxxCont_;
30+
commandMat_t QuuCont_;
31+
commandR_stateC_t QuxCont_;
3232

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 computeAllModelDeriv(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:
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 computeAllModelDeriv(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:
4646

4747
};
4848

49-
#endif // ROMEOSIMPLEACTUATOR_H
49+
#endif // DCTEMP_H

0 commit comments

Comments
 (0)