Commit 5a612109 authored by flforget's avatar flforget

[minor][temporary]add an offset on the command

actual version of DDP always compute a command trajectory finishing by a null command
it is a problem for the pneumatic actuator model using QPBox
this fix is temporary and will require more deep modifications of the DDP
parent 1e616c36
......@@ -15,6 +15,7 @@ public:
// attributes //
public:
commandVec_t commandOffset;
protected:
unsigned int stateNb;
unsigned int commandNb;
......
......@@ -22,6 +22,7 @@ protected:
// attributes //
public:
private:
double dt;
// Muscle parameters
......
......@@ -6,14 +6,22 @@ using namespace std;
CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
{
Q <<1e-2*1, 0.0, 0.0, 0.0, 0,0,0,0,
/*Q <<1e-2*1, 0.0, 0.0, 0.0, 0,0,0,0,
0.0,1e-2*1, 0.0, 0.0, 0,0,0,0,
2.0e-2*0.0, 0.0, 1e-5*0.0, 0.0, 0,0,0,0,
0.0, 0.0, 0.0, 1e-5*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,0,0,0, 0,0,0,0;
0,0,0,0, 0,0,0,0;*/
Q <<1e-2*1, 0.0, 0.0, 0.0, 0,0,0,0,
0.0,1e-2*1, 0.0, 0.0, 0,0,0,0,
2.0e-2*0.0, 0.0, 1e-5*0.0, 0.0, 0,0,0,0,
0.0, 0.0, 0.0, 1e-5*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,0,0,0, 0,0,0,0;
/* Qf <<8e4*1.0, 0.0, 0.0, 0.0,
0.0,1e-3*0.0, 0.0, 0.0,
0.0, 0.0, 5e-5*1.0, 0.0,
......@@ -27,8 +35,10 @@ CostFunctionPneumaticarmElbow::CostFunctionPneumaticarmElbow()
0,0,0,0, 0,0,0,0,
0,0,0,0, 0,0,0,0;*/
Qf = Q; Qf(0,0) = 0; Qf(1,1) = 0;
R << 1e-6,0,
0, 1e-6;
/*R << 1e-6,0,
0, 1e-6;*/
R << 1e-5,0,
0, 5e-5;
lxx = Q;
luu = R;
//lux << 0.0,0.0;
......
......@@ -217,6 +217,7 @@ ILQRSolver::traj ILQRSolver::getLastSolvedTrajectory()
lastTraj.xList = updatedxList;
for(int i=0;i<T+1;i++)lastTraj.xList[i] += xDes;
lastTraj.uList = updateduList;
for(int i=0;i<T;i++) lastTraj.uList[i] += dynamicModel->commandOffset;
lastTraj.iter = iter;
return lastTraj;
}
......
......@@ -86,8 +86,8 @@ stateVec_t computejointderiv(double& dt, const stateVec_t& Xe,const stateVec_t&
//%% Delta P Pressure Dynamics
////%%%%%%% 2nd order %%%%%%%%%%%%%%
////%wnb2 = wnb1;a
double Pdes1 = U(0);
double Pdes2 = U(1);
double Pdes1 = U(0)+1.5;
double Pdes2 = U(1)+2.0;
jointstate_deriv(4) = X(6);
jointstate_deriv(5) = X(7);
jointstate_deriv(6) = -pow(wnb1,2)*X(4) - 2*wnb1*X(6) + pow(wnb1,2)*Pdes1;
......@@ -179,6 +179,10 @@ PneumaticarmNonlinearModel::PneumaticarmNonlinearModel(double& mydt)
QxxCont.setZero();
QuuCont.setZero();
QuxCont.setZero();*/
lowerCommandBounds << -1.5,-2.0;
upperCommandBounds << 1.5,2.0;
commandOffset << 1.5,2.0;
}
stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateVec_t& Xe,const stateVec_t& xDes,const commandVec_t& U)
......@@ -201,8 +205,8 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV
//%% Delta P Pressure Dynamics
////%%%%%%% 2nd order %%%%%%%%%%%%%%
////%wnb2 = wnb1;a
double Pdes1 = U(0);
double Pdes2 = U(1);
double Pdes1 = U(0)+commandOffset(0);
double Pdes2 = U(1)+commandOffset(1);
jointstate_deriv(4) = X(6);
jointstate_deriv(5) = X(7);
jointstate_deriv(6) = -pow(wnb1,2)*X(4) - 2*wnb1*X(6) + pow(wnb1,2)*Pdes1;
......@@ -268,7 +272,9 @@ stateVec_t PneumaticarmNonlinearModel::computeNextState(double& dt, const stateV
void PneumaticarmNonlinearModel::computeAllModelDeriv(double& dt, const stateVec_t& Xe,const stateVec_t& Xdes,const commandVec_t& U)
{
//fx = fxBase;
commandVec_t Ureal;
Ureal(0) = U(0) +commandOffset(0);
Ureal(1) = U(1) +commandOffset(1);
Xreal = Xe + Xdes;
stateVec_t X = Xe + Xdes;
double dh = 1e-5;
......@@ -281,10 +287,10 @@ void PneumaticarmNonlinearModel::computeAllModelDeriv(double& dt, const stateVec
{
tempX = X;
tempX(j) = X(j) + dh;
derivplus = computejointderiv(dt, tempX,Xdes, U); //cout<< "deriv:" <<derivplus(0)<< '\n' << endl;
derivplus = computejointderiv(dt, tempX,Xdes, Ureal); //cout<< "deriv:" <<derivplus(0)<< '\n' << endl;
tempX = X;
tempX(j) = X(j) - dh;
derivminus = computejointderiv(dt, tempX,Xdes, U);
derivminus = computejointderiv(dt, tempX,Xdes, Ureal);
fx(i,j) = (derivplus(i) - derivminus(i))/(2*dh);
}
}
......@@ -293,11 +299,11 @@ void PneumaticarmNonlinearModel::computeAllModelDeriv(double& dt, const stateVec
{
for(unsigned int j=0; j<2;j++)
{
tempU = U;
tempU(j) = U(j) + dh;
tempU = Ureal;
tempU(j) = Ureal(j) + dh;
derivplus = computejointderiv(dt, X,Xdes, tempU);
tempU = U;
tempU(j) = U(j) - dh;
tempU = Ureal;
tempU(j) = Ureal(j) - dh;
derivminus = computejointderiv(dt, X,Xdes, tempU);
fu(i,j) = (derivplus(i) - derivminus(i))/(2*dh);
}
......
......@@ -17,6 +17,7 @@ using namespace Eigen;
int main()
{
struct timeval tbegin,tend;
commandVec_t commandOffset;
double texec=0.0;
double tsec = 0.0; long double tusec = 0.0;
stateVec_t xinit,xDes;
......@@ -24,7 +25,9 @@ int main()
xinit << 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0;
xDes << 0.5,1.0,0.0,0.0,0.0,0.0,0.0,0.0;
unsigned int T = 400;
commandOffset << 1.5,2.0;
unsigned int T = 800;
double dt=10e-3;
unsigned int iterMax = 100;
double stopCrit = 1e-5;
......@@ -36,7 +39,7 @@ int main()
CostFunctionPneumaticarmElbow cost;
ILQRSolver solver(model,cost,DISABLE_FULLDDP,DISABLE_QPBOX);
ILQRSolver solver(model,cost,DISABLE_FULLDDP,ENABLE_QPBOX);
solver.FirstInitSolver(xinit,xDes,T,dt,iterMax,stopCrit);
......@@ -51,6 +54,11 @@ int main()
uList1 = lastTraj.uList;
unsigned int iter = lastTraj.iter;
/*for(int i=0;i<N;i++)
{
uList1[i] += commandOffset;
}*/
/*texec=((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000.;
texec /= N;
......@@ -77,7 +85,7 @@ int main()
{
fichier << "pos1,pos2,vel1,vel2,u1,u2" << endl;
for(int i=0;i<T;i++) fichier << xList1[i](0,0) << "," << xList1[i](1,0) << "," << xList1[i](2,0) << "," << xList1[i](3,0) << "," << uList1[i](0,0) << "," << uList1[i](1,0) << endl;
fichier << xList1[T](0,0) << "," << xList1[T](1,0) << "," << xList1[T](2,0) << "," << xList1[T](3,0) << "," << 0.0 << "," << 0.0 << endl;
fichier << xList1[T](0,0) << "," << xList1[T](1,0) << "," << xList1[T](2,0) << "," << xList1[T](3,0) << "," << uList1[T-1](0,0) << "," << uList1[T-1](1,0) << endl;
fichier.close();
}
else
......
......@@ -16,7 +16,7 @@ dt = 5e-3; % dt for dynamics
ToT = 4; %(in seconds)
N_tot = ToT/dt;
%T_horizon = 0.5; %(in seconds)
T = 40; %N_tot; %10; %T_horizon/dt;
T = 400; %N_tot; %10; %T_horizon/dt;
%N_DT = ToT/T_horizon;
......
......@@ -7,29 +7,60 @@ fprintf(['A demonstration of the iLQG/DDP algorithm\n'...
'\"Control-Limited Differential Dynamic Programming\"\n'])
% make stable linear dynamics
h = .01; % time step
n = 10; % state dimension
m = 2; % control dimension
A = randn(n,n);
A = A-A'; % skew-symmetric = pure imaginary eigenvalues
A = expm(h*A); % discrete time
B = h*randn(n,m);
h = .0001; % time step
n = 4; % state dimension
m = 1; % control dimension
A = [1 0.0001 0 0;
-1.18116 0.927536 0 -72.4638;
0 0 1 0.0001;
0.001 0 0 1];
B = [ 0;
36.2319;
0;
0];
% quadratic costs
Q = h*eye(n);
R = .1*h*eye(m);
Q = [100,0,0,0;
0,0,0,0;
0,0,0,0;
0,0,0,0;];
R = .1*eye(m);
% control limits
Op.lims = ones(m,1)*[-1 1]*.6;
%Op.lims = ones(m,1)*[-1 1]*.6;
% optimization problem
DYNCST = @(x,u,i) lin_dyn_cst(x,u,A,B,Q,R);
T = 1000; % horizon
x0 = randn(n,1); % initial state
T = 150; % horizon
x0 = [-3.0;0.0;0.0;0.0]; % initial state
u0 = .1*randn(m,T); % initial controls
% run the optimization
iLQG(DYNCST, x0, u0, Op);
[x,u] = iLQG(DYNCST, x0, u0);
figure
subplot(221)
plot(x(1,:,:))
grid on
subplot(222)
plot(x(2,:,:))
grid on
subplot(223)
plot(x(3,:,:))
grid on
subplot(224)
plot(x(4,:,:))
grid on
figure
plot(u)
grid on
function [f,c,fx,fu,fxx,fxu,fuu,cx,cu,cxx,cxu,cuu] = lin_dyn_cst(x,u,A,B,Q,R)
......
Markdown is supported
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