# Demo entry 6749379

Iterative LQR

Submitted by anonymous on Jun 12, 2018 at 19:35
Language: C++. Code size: 4.0 kB.

```Trajectory ILQRPlanner::plan(const Scenario& scenario, double dt) {
using namespace Eigen;

// Get an initial trajectory.
Trajectory traj;
std::vector<Control> controls;
std::tie(traj, controls) = generateInitialTrajectory(scenario, dt);

// Reserve space for derivatives.
StdVectorOfMatrices<6, 1> lx;
StdVectorOfMatrices<2, 1> lu;
StdVectorOfMatrices<6, 6> lxx;
StdVectorOfMatrices<2, 2> luu;
StdVectorOfMatrices<2, 6> lux;
StdVectorOfMatrices<6, 6> fx;
StdVectorOfMatrices<6, 2> fu;

// Regularization coefficients for Step 2
double delta0 = 2;
double delta  = delta0;
double mu_min = 1e-6;
double mu = mu_min;

// Run iLQR.
bool trajectory_has_changed;
do {
trajectory_has_changed = false;

// Step 1: Compute derivatives
for (int i = 0; i <= traj.states.size()-1; ++i) {
const auto& x = traj.states[i];
const auto& u = controls[i];
lx.push_back(loss_x(x, u, i * dt));
lu.push_back(loss_u(x, u, i * dt));
lxx.push_back(loss_xx(x, u, i * dt));
luu.push_back(loss_uu(x, u, i * dt));
lux.push_back(loss_ux(x, u, i * dt));
fx.push_back(dynamics_x(x, u, dt));
fu.push_back(dynamics_u(x, u, dt));
}

// Step 2: Backward Pass
StdVectorOfMatrices<2, 1> kVec;
StdVectorOfMatrices<2, 6> KVec;
bool restart_step_two;
do {
Matrix<double, 6, 1> Vx;    // TODO: Initialize this.
Matrix<double, 6, 6> Vxx;   // TODO: Initialize this.
Matrix<double, 6, 6> In = Matrix<double, 6, 6>::Identity();

kVec.clear();
KVec.clear();

restart_step_two = false;
for (int i = traj.states.size() - 1; i >= 0; i--) {
// Eq. 5
Matrix<double, 6, 1> Qx = lx[i] + fx[i].transpose()*Vx;
Matrix<double, 2, 1> Qu = lu[i] + fu[i].transpose()*Vx;
Matrix<double, 6, 6> Qxx = lxx[i] + fx[i].transpose()*Vxx*fx[i];
Matrix<double, 2, 2> Quu = luu[i] + fu[i].transpose()*Vxx*fu[i];
Matrix<double, 2, 6> Qux = lux[i] + fu[i].transpose()*Vxx*fx[i];

// Eq. 10
Quu = luu[i] + fu[i].transpose()*(Vxx + mu*In)*fu[i];
Qux = lux[i] + fu[i].transpose()*(Vxx + mu*In)*fx[i];
Matrix<double, 2, 1> k = -Quu.inverse()*Qu;
Matrix<double, 2, 6> K = -Quu.inverse()*Qux;

kVec.push_back(k);
KVec.push_back(K);

// If Quu is not positive definite
Matrix<double, 2, 1> eigenvals = Quu.eigenvalues().real();
if(eigenvals(0) <= 0 || eigenvals(1) <= 0) {
// Increase mu and restart step 2.
delta = std::max(delta0, delta*delta0);
mu = std::max(mu_min, mu*delta);
restart_step_two = true;
break;
}

// Eq. 11
Vx = Qx + K.transpose()*Quu*k + K.transpose()*Qu + Qux.transpose()*k;
Vxx = Qxx + K.transpose()*Quu*K + K.transpose()*Qux + Qux.transpose()*K;
}
} while (restart_step_two);

// Decrease mu for the next ilqr iteration.
delta = std::min(1.0/delta0, delta/delta0);
mu = mu*delta;
if(mu < mu_min) {
mu = 0;
}

// Step 3: Forward Pass
bool restart_step_three;
double alpha;
do {
alpha = 1.0;
restart_step_three = false;
for (int i = 0; i < traj.states.size() - 1; i++) {
// Eq. 12
//controls[i] += alpha*kVec[i] + KVec[i]*(traj.states[i] - traj.states[i]);
// Eq. 8c
//traj.states[i] = dynamics(traj.states[i], controls[i], dt);

// if(dynamics(xhat(i), uhat(i)) > eps) {
//  x(i+1) = dynamics(xhat(i), uhat(i))
//  trajectory_has_changed = true;
//}
// if( integration diverged or condition 13 not met ) {
//  alpha /= 2.0;
//  restart_step_three = true;
//}
}
} while (restart_step_three);
} while (trajectory_has_changed);
return traj;
}
```

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.