unconstr_ocp_solver.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_UNCONSTR_OCP_SOLVER_HPP_
#define ROBOTOC_UNCONSTR_OCP_SOLVER_HPP_
 
#include <vector>
#include <memory>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/utils/aligned_vector.hpp"
#include "robotoc/cost/cost_function.hpp"
#include "robotoc/constraints/constraints.hpp"
#include "robotoc/unconstr/unconstr_direct_multiple_shooting.hpp"
#include "robotoc/core/solution.hpp"
#include "robotoc/core/direction.hpp"
#include "robotoc/core/kkt_matrix.hpp"
#include "robotoc/core/kkt_residual.hpp"
#include "robotoc/riccati/unconstr_riccati_recursion.hpp"
#include "robotoc/line_search/unconstr_line_search.hpp"
#include "robotoc/ocp/ocp.hpp"
#include "robotoc/ocp/grid_info.hpp"
#include "robotoc/solver/solver_options.hpp"
#include "robotoc/solver/solver_statistics.hpp"
#include "robotoc/utils/timer.hpp"
 
 
namespace robotoc {
 
class UnconstrOCPSolver {
public:
  UnconstrOCPSolver(const OCP& ocp, 
                    const SolverOptions& solver_options=SolverOptions());
 
  UnconstrOCPSolver();
 
  ~UnconstrOCPSolver() = default;
 
  UnconstrOCPSolver(const UnconstrOCPSolver&) = default;
 
  UnconstrOCPSolver& operator=(const UnconstrOCPSolver&) = default;
 
  UnconstrOCPSolver(UnconstrOCPSolver&&) noexcept = default;
 
  UnconstrOCPSolver& operator=(UnconstrOCPSolver&&) noexcept = default;
 
  void setSolverOptions(const SolverOptions& solver_options);
 
  void discretize(const double t);
 
  void initConstraints();
 
  void solve(const double t, const Eigen::VectorXd& q, const Eigen::VectorXd& v,
             const bool init_solver=true);
  const SolverStatistics& getSolverStatistics() const;
 
  const SplitSolution& getSolution(const int stage) const;
 
  std::vector<Eigen::VectorXd> getSolution(const std::string& name) const;
 
  const std::vector<LQRPolicy>& getLQRPolicy() const;
 
  void setSolution(const std::string& name, const Eigen::VectorXd& value);
 
  double KKTError(const double t, const Eigen::VectorXd& q, 
                  const Eigen::VectorXd& v);
 
  double KKTError() const;
 
  const std::vector<GridInfo>& getTimeDiscretization() const;
 
  void setRobotProperties(const RobotProperties& properties);
 
private:
  aligned_vector<Robot> robots_;
  std::vector<GridInfo> time_discretization_;
  UnconstrDirectMultipleShooting dms_;
  UnconstrRiccatiRecursion riccati_recursion_;
  UnconstrLineSearch line_search_;
  OCP ocp_;
  KKTMatrix kkt_matrix_;
  KKTResidual kkt_residual_;
  Solution s_;
  Direction d_;
  UnconstrRiccatiFactorization riccati_factorization_;
  SolverOptions solver_options_;
  SolverStatistics solver_statistics_;
  Timer timer_;
 
  void updateSolution(const double t, const Eigen::VectorXd& q, 
                      const Eigen::VectorXd& v);
 
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_UNCONSTR_OCP_SOLVER_HPP_