[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

cpp/include/dynamicmodel.h

@@ -15,6 +15,7 @@ public:
 
 // attributes //
 public:
+    commandVec_t commandOffset;
 protected:
     unsigned int stateNb;
     unsigned int commandNb;

cpp/include/pneumaticarm_2linkmodel.hh

@@ -22,6 +22,7 @@ protected:
 
     // attributes //
 public:
+
 private:
     double dt;
    // Muscle parameters

cpp/src/costfunctionpneumaticarmelbow.cpp

@@ -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;

cpp/src/ilqrsolver.cpp

@@ -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;
 }

cpp/src/pneumaticarm_2linkmodel.cpp

@@ -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);
         }

cpp/test/main.cpp

@@ -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

matlab/demo_pneumatic2link_delP_MPC3.m

@@ -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;
 
 

matlab/iLQG/demo_linear.m

@@ -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)