unconstr_parnmpc_solver.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_UNCONSTR_PARNMPC_SOLVER_HPP_
#define ROBOTOC_UNCONSTR_PARNMPC_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/core/solution.hpp"
#include "robotoc/core/direction.hpp"
#include "robotoc/core/kkt_matrix.hpp"
#include "robotoc/core/kkt_residual.hpp"
#include "robotoc/parnmpc/unconstr_backward_correction.hpp"
#include "robotoc/line_search/unconstr_line_search.hpp"
#include "robotoc/ocp/ocp.hpp"
#include "robotoc/ocp/time_discretization.hpp"
#include "robotoc/solver/solver_options.hpp"
#include "robotoc/solver/solver_statistics.hpp"
#include "robotoc/utils/timer.hpp"
 
 
namespace robotoc {
 
class UnconstrParNMPCSolver {
public:
  UnconstrParNMPCSolver(const OCP& ocp, 
                        const SolverOptions& solver_options=SolverOptions());
 
  UnconstrParNMPCSolver();
 
  ~UnconstrParNMPCSolver() = default;
 
  UnconstrParNMPCSolver(const UnconstrParNMPCSolver&) = default;
 
  UnconstrParNMPCSolver& operator=(const UnconstrParNMPCSolver&) = default;
 
  UnconstrParNMPCSolver(UnconstrParNMPCSolver&&) noexcept = default;
 
  UnconstrParNMPCSolver& operator=(UnconstrParNMPCSolver&&) noexcept = default;
 
  void setSolverOptions(const SolverOptions& solver_options);
 
  void discretize(const double t);
 
  void initConstraints();
 
  void initBackwardCorrection();
 
  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;
 
  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_;
  UnconstrBackwardCorrection backward_correction_;
  UnconstrLineSearch line_search_;
  OCP ocp_;
  KKTMatrix kkt_matrix_;
  KKTResidual kkt_residual_;
  Solution s_;
  Direction d_;
  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_PARNMPC_SOLVER_HPP_