Commit f5588b23 authored by flforget's avatar flforget

[WIP] test several actuators

parent eea9359e
......@@ -12,25 +12,28 @@ FILE(
src/*
)
ADD_EXECUTABLE(main test/main.cpp ${source_files})
#ADD_EXECUTABLE(main test/main.cpp ${source_files})
#ADD_EXECUTABLE(mainVIA test/mainVIA.cpp ${source_files})
ADD_EXECUTABLE(mainTorque test/mainTorque.cpp ${source_files})
ADD_EXECUTABLE(testModel test/testModel.cpp ${source_files})
#ADD_EXECUTABLE(testModel test/testModel.cpp ${source_files})
#ADD_EXECUTABLE(testAsserv test/testAsserv.cpp ${source_files})
ADD_EXECUTABLE(benchControl test/benchControl.cpp ${source_files} test/BenchCom.cpp test/BenchCom.h)
ADD_EXECUTABLE(benchControlMPC test/benchControlMPC.cpp ${source_files} test/BenchCom.cpp test/BenchCom.h)
#ADD_EXECUTABLE(benchControl test/benchControl.cpp ${source_files} test/BenchCom.cpp test/BenchCom.h)
#ADD_EXECUTABLE(benchControlMPC test/benchControlMPC.cpp ${source_files} test/BenchCom.cpp test/BenchCom.h)
#ADD_EXECUTABLE(simulateBench test/simulateBench.cpp ${source_files} test/BenchCom.cpp test/BenchCom.h)
ADD_EXECUTABLE(benchTorqueControl test/benchTorqueControl.cpp ${source_files} test/BenchCom.cpp test/BenchCom.h)
#ADD_EXECUTABLE(benchTorqueControl test/benchTorqueControl.cpp ${source_files} test/BenchCom.cpp test/BenchCom.h)
#ADD_EXECUTABLE(mainLQR test/mainLQR.cpp ${source_files})
ADD_EXECUTABLE(dccontrol test/dccontrol.cpp ${source_files})
#ADD_EXECUTABLE(dccontrol test/dccontrol.cpp ${source_files})
ADD_EXECUTABLE(estimateBW test/estimateBW.cpp ${source_files})
TARGET_LINK_LIBRARIES(main ${qpOASES_LIBRARIES})
#TARGET_LINK_LIBRARIES(main ${qpOASES_LIBRARIES})
#TARGET_LINK_LIBRARIES(mainVIA ${qpOASES_LIBRARIES})
TARGET_LINK_LIBRARIES(mainTorque ${qpOASES_LIBRARIES})
TARGET_LINK_LIBRARIES(testModel ${qpOASES_LIBRARIES})
#TARGET_LINK_LIBRARIES(testModel ${qpOASES_LIBRARIES})
#TARGET_LINK_LIBRARIES(testAsserv ${qpOASES_LIBRARIES})
TARGET_LINK_LIBRARIES(benchControl ${qpOASES_LIBRARIES})
TARGET_LINK_LIBRARIES(benchControlMPC ${qpOASES_LIBRARIES})
#TARGET_LINK_LIBRARIES(benchControl ${qpOASES_LIBRARIES})
#TARGET_LINK_LIBRARIES(benchControlMPC ${qpOASES_LIBRARIES})
#TARGET_LINK_LIBRARIES(simulateBench ${qpOASES_LIBRARIES})
TARGET_LINK_LIBRARIES(benchTorqueControl ${qpOASES_LIBRARIES})
TARGET_LINK_LIBRARIES(dccontrol ${qpOASES_LIBRARIES})
\ No newline at end of file
#TARGET_LINK_LIBRARIES(benchTorqueControl ${qpOASES_LIBRARIES})
#TARGET_LINK_LIBRARIES(dccontrol ${qpOASES_LIBRARIES})
TARGET_LINK_LIBRARIES(estimateBW ${qpOASES_LIBRARIES})
\ No newline at end of file
......@@ -13,6 +13,8 @@ SET(${PROJECT_NAME}_HEADERS
types.h
dcmotor.h
costDC.h
strainwaveactuator.h
maccepa.h
)
INSTALL(FILES ${${PROJECT_NAME}_HEADER} DESTINATION include)
......
#ifndef AWAS_H
#define AWAS_H
#ifndef MACCEPA_H
#define MACCEPA_H
#include "dynamicmodel.h"
class Awas : public DynamicModel<double,4,2>
class Maccepa : public DynamicModel<double,4,1>
{
public:
Awas(double& mydt,bool noiseOnParameters=0);
Maccepa(double& mydt);
private:
protected:
......@@ -14,9 +14,9 @@ protected:
public:
private:
double dt;
private:
public:
double k;
double M;
//double B,C,P;
private:
stateVec_t Xreal;
stateMat_t Id;
......@@ -43,11 +43,11 @@ public:
stateMat_t computeTensorContxx(const stateVec_t& nextVx);
commandMat_t computeTensorContuu(const stateVec_t& nextVx);
commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);
private:
public:
stateVec_t computeStateDeriv(double &dt, const stateVec_t& X, const commandVec_t &U);
protected:
};
#endif
#endif // MACCEPA_H
......@@ -6,7 +6,7 @@
class RomeoSimpleActuator : public DynamicModel<double,4,1>
{
public:
RomeoSimpleActuator(double& mydt,bool noiseOnParameters=0);
RomeoSimpleActuator(double& mydt,int noiseOnParameters=0);
private:
protected:
......
#ifndef STRAINWAVEACTUATOR_H
#define STRAINWAVEACTUATOR_H
#include "dynamicmodel.h"
class StrainWaveActuator : public DynamicModel<double,2,1>
{
public:
StrainWaveActuator(double& mydt);
private:
protected:
// attributes //
public:
private:
double dt;
public:
double Kt,Kv,Kf,Ka;
double a,J;
private:
stateVec_t Xreal;
stateMat_t Id;
stateMat_t A;
stateR_commandC_t B;
double A13atan;
double A33atan;
stateMat_t fxBase;
stateR_commandC_t fuBase;
stateMat_t QxxCont;
commandMat_t QuuCont;
commandR_stateC_t QuxCont;
public: //temp
stateMat_t Ad;
stateR_commandC_t Bd;
protected:
// methods //
public:
stateVec_t computeNextState(double& dt, const stateVec_t& X, const commandVec_t &U);
void computeAllModelDeriv(double& dt, const stateVec_t& X, const commandVec_t &U);
stateMat_t computeTensorContxx(const stateVec_t& nextVx);
commandMat_t computeTensorContuu(const stateVec_t& nextVx);
commandR_stateC_t computeTensorContux(const stateVec_t& nextVx);
public:
stateVec_t computeStateDeriv(double &dt, const stateVec_t& X, const commandVec_t &U);
protected:
};
#endif // STRAINWAVEACTUATOR_H
......@@ -2,7 +2,7 @@
CostRomeoPos::CostRomeoPos()
{
Q << 500.0,0.0,0.0,0.0,
Q << 50.0,0.0,0.0,0.0,
0.0,0.0,0.0,0.0,
0.0,0.0,0.0,0.0,
0.0,0.0,0.0,0.0;
......@@ -17,7 +17,7 @@ CostRomeoPos::CostRomeoPos()
void CostRomeoPos::computeAllCostDeriv(const stateVec_t& X,const stateVec_t& Xdes, const commandVec_t& U)
{
lx = Q*(X-Xdes);
lx = 0.0*Q*(X-Xdes);
lu = R*U;
}
......
#include "awas.h"
#include "maccepa.h"
#include <math.h>
#include <eigen3/unsupported/Eigen/MatrixFunctions>
#include <sys/time.h>
#include <iostream>
#define pi M_PI
/*
*
* x0 -> actuator position
* x1 -> spring torque
* x2 -> actuator speed
* x3 -> motor speed
*/
Awas::Awas(double& mydt,bool noiseOnParameters)
Maccepa::Maccepa(double& mydt)
{
stateNb=4;
commandNb=2;
commandNb=1;
dt = mydt;
struct timeval tv;
if(!noiseOnParameters)
{
k = 10000.0;
M = 0.018;
}
else
{
gettimeofday(&tv,NULL);
srand(tv.tv_usec);
k = 10000.0 + 10000.0*0.1*(2.0*(rand()/(double)RAND_MAX)-1.0);
M = 0.018 + 0.018*0.1*(2.0*(rand()/(double)RAND_MAX)-1.0);
}
k = 2520;
//B = 0.01;
//C = 0.04;
//P = 0.04;
Id.setIdentity();
A.setZero();
Ad = (dt*A).exp();
B << 0.0,0.0,
1.0,0.0,
0.0,0.0,
0.0,1.0;
Bd = dt*B;
fu = Bd;
fxx[0].setZero();
fxx[1].setZero();
fxx[2].setZero();
fxx[3].setZero();
fxu[0].setZero();
fxu[0].setZero();
......@@ -57,23 +42,26 @@ Awas::Awas(double& mydt,bool noiseOnParameters)
QuuCont.setZero();
QuxCont.setZero();
lowerCommandBounds << 0.01;
upperCommandBounds << 0.1;
lowerCommandBounds << -5.0;
upperCommandBounds << 5.0;
}
Awas::stateVec_t Awas::computeStateDeriv(double &dt, const stateVec_t& X, const commandVec_t &U)
Maccepa::stateVec_t Maccepa::computeStateDeriv(double &dt, const stateVec_t& X, const commandVec_t &U)
{
stateVec_t x_dot;
x_dot << X(1,0),
U(0,0) - (k/M)*X(2,0)*X(2,0)*sin(X(0,0)),
X(3,0),
U(1,0);
x_dot << X(1,0);
return x_dot;
}
Awas::stateVec_t Awas::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)
Maccepa::stateVec_t Maccepa::computeNextState(double& dt, const stateVec_t& X,const commandVec_t& U)
{
stateVec_t x_next,k1,k2,k3,k4;
/*k1 = A*X + B*U;
k2 = A*(X+(dt/2)*k1) + B*U;
k3 = A*(X+(dt/2)*k2) + B*U;
k4 = A*(X+dt*k3) + B*U;
x_next = X + (dt/6)*(k1+2*k2+2*k3+k4);*/
k1 = computeStateDeriv(dt,X,U);
k2 = computeStateDeriv(dt,X+(dt/2.0)*k1,U);
......@@ -81,10 +69,11 @@ Awas::stateVec_t Awas::computeNextState(double& dt, const stateVec_t& X,const co
k4 = computeStateDeriv(dt,X+dt*k3,U);
x_next = X + (dt/6.0)*(k1+2.0*k2+2.0*k3+k4);
//x_next = Ad*X + Bd*U;
return x_next;
}
void Awas::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)
void Maccepa::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec_t& U)
{
double dh = 1e-7;
stateVec_t Xp,Xm;
......@@ -94,23 +83,23 @@ void Awas::computeAllModelDeriv(double& dt, const stateVec_t& X,const commandVec
{
Xp[i] += dh/2;
Xm[i] -= dh/2;
fx.col(i) = (computeNextState(dh, Xp, U) - computeNextState(dh, Xm, U))/dh;
fx.col(i) = (computeNextState(dt, Xp, U) - computeNextState(dt, Xm, U))/dh;
Xp = X;
Xm = X;
}
}
Awas::stateMat_t Awas::computeTensorContxx(const stateVec_t& nextVx)
Maccepa::stateMat_t Maccepa::computeTensorContxx(const stateVec_t& nextVx)
{
return QxxCont;
}
Awas::commandMat_t Awas::computeTensorContuu(const stateVec_t& nextVx)
Maccepa::commandMat_t Maccepa::computeTensorContuu(const stateVec_t& nextVx)
{
return QuuCont;
}
Awas::commandR_stateC_t Awas::computeTensorContux(const stateVec_t& nextVx)
Maccepa::commandR_stateC_t Maccepa::computeTensorContux(const stateVec_t& nextVx)
{
return QuxCont;
}
......@@ -13,63 +13,231 @@
* x3 -> motor speed
*/
RomeoSimpleActuator::RomeoSimpleActuator(double& mydt,bool noiseOnParameters)
RomeoSimpleActuator::RomeoSimpleActuator(double& mydt,int noiseOnParameters)
{
stateNb=4;
commandNb=1;
dt = mydt;
struct timeval tv;
if(!noiseOnParameters)
{
k = 588.0;
R = 96.1;
Jm = 183 * 1e-7;
Jl = 0.000085;
fvm = 5.65e-5;
fvl = 0.278;
Kt = 0.0578;
mu = 0.52;
Cf0 = 0.0;
a = 0.0;
Cc = 0.0;
/*k = 588.0;
R = 96.1;
Jm = 183 * 1e-7;
Jl = 0.085;
fvm = 5.65e-5;
fvl = 0.28;
Kt = 0.07;
Cf0 = 0.01;
a = 1.0;*/
}
else
switch (noiseOnParameters)
{
gettimeofday(&tv,NULL);
srand(tv.tv_usec);
/*k = 588.0 + 1.0*588.0*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
R = 96.1 + 1.0*96.1*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
Jm = 183*1e-7 + 1.0*183*1e-7*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
Jl = 0.000085 + 1.0*0.000085*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
fvm = 5.65e-5 + 1.0*5.65e-5*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
fvl = 0.278 + 1.0*0.278*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
Kt = 0.0578 + 1.0*0.0578*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
mu = 0.52;
Cf0 = 0.0;
a = 0.0;*/
k = 588.0 - 588.0*0.5;
R = 96.1 + 0.5*96.1*0.5;
Jm = 183*1e-7 + 0.5*183*1e-7*0.5;
Jl = 0.000085 + 0.000085*0.5;
fvm = 5.65e-5 - 5.65e-5*0.5;
fvl = 0.278 - 0.5*0.278*0.5;
Kt = 0.0578 - 0.0578*0.5;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
case 0:
{
k = 588.0;
R = 96.1;
Jm = 183 * 1e-7;
Jl = 0.000085;
fvm = 5.65e-5;
fvl = 0.278;
Kt = 0.0578;
mu = 0.52;
Cf0 = 0.0;
a = 0.0;
Cc = 0.0;
break;
}
case 1:
{
k = 588.0 - 588.0*0.1;
R = 96.1 + 0.5*96.1*0.1;
Jm = 183*1e-7 + 0.5*183*1e-7*0.1;
Jl = 0.000085 + 0.000085*0.1;
fvm = 5.65e-5 - 5.65e-5*0.1;
fvl = 0.278 - 0.5*0.278*0.1;
Kt = 0.0578 - 0.0578*0.1;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
break;
}
case 2:
{
k = 588.0 - 588.0*0.2;
R = 96.1 + 0.5*96.1*0.2;
Jm = 183*1e-7 + 0.5*183*1e-7*0.2;
Jl = 0.000085 + 0.000085*0.2;
fvm = 5.65e-5 - 5.65e-5*0.2;
fvl = 0.278 - 0.5*0.278*0.2;
Kt = 0.0578 - 0.0578*0.2;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
break;
}
case 3:
{
k = 588.0 - 588.0*0.5;
R = 96.1 + 0.5*96.1*0.5;
Jm = 183*1e-7 + 0.5*183*1e-7*0.5;
Jl = 0.000085 + 0.000085*0.5;
fvm = 5.65e-5 - 5.65e-5*0.5;
fvl = 0.278 - 0.5*0.278*0.5;
Kt = 0.0578 - 0.0578*0.5;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
break;
}
case 4:
{
k = 588.0 - 588.0*0.8;
R = 96.1 + 0.5*96.1*0.8;
Jm = 183*1e-7 + 0.5*183*1e-7*0.8;
Jl = 0.000085 + 0.000085*0.8;
fvm = 5.65e-5 - 5.65e-5*0.8;
fvl = 0.278 - 0.5*0.278*0.8;
Kt = 0.0578 - 0.0578*0.8;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
break;
}
case 5:
{
k = 588.0 - 588.0*1.0;
R = 96.1 + 0.5*96.1*1.0;
Jm = 183*1e-7 + 0.5*183*1e-7*1.0;
Jl = 0.000085 + 0.000085*1.0;
fvm = 5.65e-5 - 5.65e-5*1.0;
fvl = 0.278 - 0.5*0.278*1.0;
Kt = 0.0578 - 0.0578*1.0;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
break;
}
case 6:
{
k = 588.0 - 588.0*0.6;
R = 96.1 + 0.5*96.1*0.6;
Jm = 183*1e-7 + 0.5*183*1e-7*0.6;
Jl = 0.000085 + 0.000085*0.6;
fvm = 5.65e-5 - 5.65e-5*0.6;
fvl = 0.278 - 0.5*0.278*0.6;
Kt = 0.0578 - 0.0578*0.6;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
break;
}
case 7:
{
k = 588.0 - 588.0*0.7;
R = 96.1 + 0.5*96.1*0.7;
Jm = 183*1e-7 + 0.5*183*1e-7*0.7;
Jl = 0.000085 + 0.000085*0.7;
fvm = 5.65e-5 - 5.65e-5*0.7;
fvl = 0.278 - 0.5*0.278*0.7;
Kt = 0.0578 - 0.0578*0.7;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
break;
}
case 8:
{
k = 180.0;
R = 96.0;
Jm = 183*1e-7;
Jl = 0.000085;
fvm = 5.65e-5;
fvl = 0.278;
Kt = 0.0578;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
break;
}
case 9:
{
k = 588.0;
R = 96.1;
Jm = 183*1e-7;
Jl = 0.000085;
fvm = 5.65e-5;
fvl = 0.278;
Kt = 0.0578;
mu = 0.52;
Cf0 = 0.5;
a = 10.0;
Cc = 0.0;
break;
}
default:
{
k = 588.0;
R = 96.1;
Jm = 183 * 1e-7;
Jl = 0.000085;
fvm = 5.65e-5;
fvl = 0.278;
Kt = 0.0578;
mu = 0.52;
Cf0 = 0.0;
a = 0.0;
Cc = 0.0;
break;
}
}
// if(noiseOnParameters==0)
// {
// k = 588.0;
// R = 96.1;
// Jm = 183 * 1e-7;
// Jl = 0.000085;
// fvm = 5.65e-5;
// fvl = 0.278;
// Kt = 0.0578;
// mu = 0.52;
// Cf0 = 0.0;
// a = 0.0;
// Cc = 0.0;
//
// /*k = 588.0;
// R = 96.1;
// Jm = 183 * 1e-7;
// Jl = 0.085;
// fvm = 5.65e-5;
// fvl = 0.28;
// Kt = 0.07;
// Cf0 = 0.01;
// a = 1.0;*/
// }
// else
// {
// gettimeofday(&tv,NULL);
// srand(tv.tv_usec);
// /*k = 588.0 + 1.0*588.0*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
// R = 96.1 + 1.0*96.1*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
// Jm = 183*1e-7 + 1.0*183*1e-7*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
// Jl = 0.000085 + 1.0*0.000085*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
// fvm = 5.65e-5 + 1.0*5.65e-5*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
// fvl = 0.278 + 1.0*0.278*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
// Kt = 0.0578 + 1.0*0.0578*0.5*(2.0*(rand()/(double)RAND_MAX)-1.0);
// mu = 0.52;
// Cf0 = 0.0;
// a = 0.0;*/
// k = 588.0 /*- 588.0*0.5*/;
// R = 96.1 /*+ 0.5*96.1*0.5*/;
// Jm = 183*1e-7 /*+ 0.5*183*1e-7*0.5*/;
// Jl = 0.000085 /*+ 0.000085*0.5*/;
// fvm = 5.65e-5 /*- 5.65e-5*0.5*/;
// fvl = 0.278 /*- 0.5*0.278*0.5*/;
// Kt = 0.0578 - 0.0578*0.5;
// mu = 0.52;
// Cf0 = 0.5;
// a = 10.0;
// Cc = 0.0;
// }
Id.setIdentity();
......
#include "strainwaveactuator.h"
#include <math.h>
#include <eigen3/unsupported/Eigen/MatrixFunctions>
#include <sys/time.h>
#include <iostream>
#define pi M_PI
/*
* x0 -> actuator position
* x1 -> spring torque
* x2 -> actuator speed
* x3 -> motor speed
*/
StrainWaveActuator::StrainWaveActuator(double& mydt)
{
stateNb=2;
commandNb=1;
dt = mydt;
struct timeval tv;
Kt = 0.0724015;
Kv = 0.381202;
Kf = 0.635621;
Ka = 0.0;
a = 10.0;
J = 0.85;
Id.setIdentity();
fxx[0].setZero();
fxx[1].setZero();
fxx[2].setZero();
fxu[0].setZero();
fxu[0].setZero();
fuu[0].setZero();
fux[0].setZero();
fxu[0].setZero();