ocp_solver.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_OCP_SOLVER_HPP_
#define ROBOTOC_OCP_SOLVER_HPP_
 
#include <vector>
#include <memory>
#include <iostream>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/robot_properties.hpp"
#include "robotoc/utils/aligned_vector.hpp"
#include "robotoc/planner/contact_sequence.hpp"
#include "robotoc/cost/cost_function.hpp"
#include "robotoc/constraints/constraints.hpp"
#include "robotoc/ocp/ocp.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/ocp/direct_multiple_shooting.hpp"
#include "robotoc/riccati/riccati_recursion.hpp"
#include "robotoc/riccati/riccati_factorization.hpp"
#include "robotoc/line_search/line_search.hpp"
#include "robotoc/line_search/line_search_settings.hpp"
#include "robotoc/sto/switching_time_optimization.hpp"
#include "robotoc/sto/sto_cost_function.hpp"
#include "robotoc/sto/sto_constraints.hpp"
#include "robotoc/solver/solution_interpolator.hpp"
#include "robotoc/solver/solver_options.hpp"
#include "robotoc/solver/solver_statistics.hpp"
#include "robotoc/utils/timer.hpp"
 
 
namespace robotoc {
 
class OCPSolver {
public:
  OCPSolver(const OCP& ocp, 
            const SolverOptions& solver_options=SolverOptions());
 
  OCPSolver();
 
  ~OCPSolver() = default;
 
  OCPSolver(const OCPSolver&) = default;
 
  OCPSolver& operator=(const OCPSolver&) = default;
 
  OCPSolver(OCPSolver&&) noexcept = default;
 
  OCPSolver& operator=(OCPSolver&&) 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 Solution& getSolution() const;
 
  const SplitSolution& getSolution(const int stage) const;
 
  std::vector<Eigen::VectorXd> getSolution(const std::string& name,
                                           const std::string& option="") const;
 
  const aligned_vector<LQRPolicy>& getLQRPolicy() const;
 
  const RiccatiFactorization& getRiccatiFactorization() const;
 
  void setSolution(const Solution& s);
 
  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 TimeDiscretization& getTimeDiscretization() const;
 
  void setRobotProperties(const RobotProperties& properties);
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, 
                                  const OCPSolver& ocp_solver);
 
private:
  aligned_vector<Robot> robots_;
  std::shared_ptr<ContactSequence> contact_sequence_;
  TimeDiscretization time_discretization_;
  DirectMultipleShooting dms_;
  SwitchingTimeOptimization sto_;
  RiccatiRecursion riccati_recursion_;
  LineSearch line_search_;
  OCP ocp_;
  KKTMatrix kkt_matrix_;
  KKTResidual kkt_residual_;
  Solution s_;
  Direction d_;
  RiccatiFactorization riccati_factorization_;
  SolutionInterpolator solution_interpolator_;
  SolverOptions solver_options_;
  SolverStatistics solver_statistics_;
  Timer timer_;
 
  void updateSolution(const double t, const Eigen::VectorXd& q, 
                      const Eigen::VectorXd& v);
 
  void resizeData();
 
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_OCP_SOLVER_HPP_