pre-allocate matrices

README.md

@@ -6,17 +6,12 @@ See `notes.md` for comments on algorithm, implementation, etc.
 
 ### Usage
 
+* Install eigen or download into `include` (https://eigen.tuxfamily.org/index.php?title=Main_Page#Download)
 * `mkdir build; cd build`
-* `cmake ..` or `cmake -DCMAKE_BUILD_TYPE=Debug ..` to compile with debug flags
+* `cmake ..` 
 * `make`
 * Define a dynamics and cost model based on Model (see double_integrator example)
 
-### Other implementations
-
-* [Yuval Tassa's Matlab code](https://www.mathworks.com/matlabcentral/fileexchange/52069-ilqg-ddp-trajectory-optimization)
-* [DDP-Generator](https://github.com/jgeisler0303/DDP-Generator) - best iLQR implementation I've found. Generates problem-specific code with analytic gradients for dynamics and cost functions.
-* [Tglad's ILQR implementation](https://github.com/TGlad/ILQR)
-
 ### Papers
 
 * Tassa, Yuval, Nicolas Mansard, and Emo Todorov. "Control-limited differential dynamic programming." Robotics and Automation (ICRA), 2014 IEEE International Conference on. IEEE, 2014.

include/acrobot.h

@@ -33,8 +33,10 @@ class Acrobot : public Model {
           0, 0, 0, 0.2;
     Hu << 1, 0,
           0, 1;
-    u_min = Vector1d(-5); // TODO try (-1.5, 1.5)
-    u_max = Vector1d(5); 
+
+    u_min = Vector1d(-5); u_max = Vector1d(5); 
+    // u_min = Vector1d(-1.5); u_max = Vector1d(1.5);  // TODO try tighter control bounds
+
   }
 
   // lagrangian dynamics:

include/ilqr.h

@@ -27,9 +27,18 @@ static Eigen::Map<VectorXd> Alpha(alpha_vec.data(), alpha_vec.size());
 
 class iLQR {
 public:
-  iLQR(Model* p_dyn, double timeDelta): dt(timeDelta)
-  {
+  iLQR(Model* p_dyn, double timeDelta): dt(timeDelta) {
     model.reset(p_dyn);
+
+    Qx.resize(model->x_dims); 
+    Qu.resize(model->u_dims);
+    Qxx.resize(model->x_dims, model->x_dims);
+    Qux.resize(model->u_dims, model->x_dims); 
+    Quu.resize(model->u_dims, model->u_dims);;
+    k_i.resize(model->u_dims);
+    K_i.resize(model->u_dims, model->x_dims);
+    Qux_reg.resize(model->x_dims, model->u_dims);
+    QuuF.resize(model->u_dims, model->u_dims);
   }
   iLQR() = default;
 
@@ -70,6 +79,9 @@ class iLQR {
   VecOfVecXd k; //mxnxT
   VecOfMatXd K; //mxT
 
+  VectorXd Qx, Qu, k_i;
+  MatrixXd Qxx, Qux, Quu, K_i, Qux_reg, QuuF;
+
   double forward_pass(const VectorXd& x0, const VecOfVecXd& u);
   int backward_pass();
   VecOfVecXd add_bias_to_u(const VecOfVecXd &u, const VecOfVecXd &l, const double alpha);

src/boxqp.cpp

@@ -45,16 +45,17 @@ boxQPResult boxQP(const MatrixXd& Q, const VectorXd& c, const VectorXd& x0,
 
   VectorXd clamped_dims(n_dims); // TODO change this to ints - need to deal with casting when interacting with other VectorXd's
   VectorXd old_clamped_dims(n_dims);
+  VectorXd grad(n_dims), grad_clamped(n_dims), search(n_dims);
 
   for(int iter=0; iter<=qp_maxIter; iter++) {
-    if(res.result != 0) break;
+    if(res.result != 0) break; // TODO we might not need this
 
     // Check if we've stopped improving
     if(iter>0 && (oldvalue - val) < minRelImprove*std::abs(oldvalue)) {
       res.result = 4;
       break;
     }
-    VectorXd grad = Q*x + c;
+    grad = Q*x + c;
     oldvalue = val;
 
     // Find clamped dimensions
@@ -95,16 +96,20 @@ boxQPResult boxQP(const MatrixXd& Q, const VectorXd& c, const VectorXd& x0,
     }
 
     // get new search direction
-    VectorXd grad_clamped = Q*(x.cwiseProduct(clamped_dims)) + c;
+    grad_clamped = Q*(x.cwiseProduct(clamped_dims)) + c;
 
-    VectorXd search = VectorXd::Zero(x.size());
+    search.setZero();
 
   // search(free) = -Hfree \ (Hfree' \ grad_clamped(free)) - x(free);
   if (res.v_free.all()) {
-    search = -res.H_free.inverse() * (res.H_free.transpose().inverse()*subvec_w_ind(grad_clamped, res.v_free)) - subvec_w_ind(x, res.v_free);
+    search = -res.H_free.inverse() * 
+             (res.H_free.transpose().inverse()*subvec_w_ind(grad_clamped, res.v_free)) 
+             - subvec_w_ind(x, res.v_free);
   }
   else {
-    VectorXd search_update_vals = (-res.H_free.inverse() * (res.H_free.transpose().inverse()*subvec_w_ind(grad_clamped, res.v_free)) - subvec_w_ind(x, res.v_free));
+    VectorXd search_update_vals = -res.H_free.inverse() 
+                                  * (res.H_free.transpose().inverse()*subvec_w_ind(grad_clamped, res.v_free)) 
+                                  - subvec_w_ind(x, res.v_free);
 
     // TODO find cleaner way
     int update_idx = 0;