asif.hpp

// Copyright (C) 2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <cassert>
#include <utility>
 
#include "asif_func.hpp"
#include "qp_solver.hpp"
#include "time.hpp"
 
namespace smooth::feedback {
 
template<Manifold U>
struct ASIFilterParams
{
  double T{1};
  std::size_t nh{1};
  Eigen::Matrix<double, Dof<U>, 1> u_weight{Eigen::Matrix<double, Dof<U>, 1>::Ones()};
  ManifoldBounds<U> ulim{};
  ASIFtoQPParams asif{};
  QPSolverParams qp{};
};
 
template<LieGroup G, Manifold U, typename Dyn, diff::Type DT = diff::Type::Default>
class ASIFilter
{
public:
  ASIFilter(const Dyn & f, const ASIFilterParams<U> & prm = ASIFilterParams<U>{})
      : ASIFilter(Dyn(f), ASIFilterParams<U>(prm))
  {}
 
  ASIFilter(Dyn && f, ASIFilterParams<U> && prm = ASIFilterParams<U>{}) : f_(std::move(f)), prm_(std::move(prm))
  {
    const int nu_ineq = prm_.ulim.A.rows();
    asif_to_qp_allocate<G, U>(qp_, prm_.asif.K, nu_ineq, prm_.nh);
  }
 
  std::pair<U, QPSolutionStatus> operator()(const G & g, const U & u_des, auto && h, auto && bu)
  {
    using std::chrono::duration, std::chrono::duration_cast, std::chrono::nanoseconds;
 
    assert((prm_.nh == std::invoke_result_t<decltype(h), Scalar<G>, G>::RowsAtCompileTime));
 
    ASIFProblem<G, U> pbm{
      .T     = prm_.T,
      .x0    = g,
      .u_des = u_des,
      .W_u   = prm_.u_weight,
      .ulim  = prm_.ulim,
    };
 
    asif_to_qp_update<G, U, DT>(qp_, pbm, prm_.asif, f_, std::forward<decltype(h)>(h), std::forward<decltype(bu)>(bu));
    auto sol = feedback::solve_qp(qp_, prm_.qp, warmstart_);
 
    if (sol.code == QPSolutionStatus::Optimal) { warmstart_ = sol; }
 
    return {rplus(u_des, sol.primal.template head<Dof<U>>()), sol.code};
  }
 
private:
  Dyn f_;
 
  QuadraticProgram<-1, -1, double> qp_;
  ASIFilterParams<U> prm_;
  std::optional<QPSolution<-1, -1, double>> warmstart_;
};
 
}  // namespace smooth::feedback