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.

Delete this entry (admin only).