unconstr_direct_multiple_shooting.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_UNCONSTR_DIRECT_MULTIPLE_SHOOTING_HPP_
#define ROBOTOC_UNCONSTR_DIRECT_MULTIPLE_SHOOTING_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/ocp/ocp.hpp"
#include "robotoc/ocp/grid_info.hpp"
#include "robotoc/unconstr/unconstr_ocp_data.hpp"
#include "robotoc/unconstr/unconstr_intermediate_stage.hpp"
#include "robotoc/unconstr/unconstr_terminal_stage.hpp"
 
 
namespace robotoc {
 
class UnconstrDirectMultipleShooting {
public:
  UnconstrDirectMultipleShooting(const OCP& ocp, const int nthreads=1);
 
  UnconstrDirectMultipleShooting();
 
  ~UnconstrDirectMultipleShooting() = default;
 
  UnconstrDirectMultipleShooting(
      const UnconstrDirectMultipleShooting&) = default;
 
  UnconstrDirectMultipleShooting& operator=(
      const UnconstrDirectMultipleShooting&) = default;
 
  UnconstrDirectMultipleShooting(
      UnconstrDirectMultipleShooting&&) noexcept = default;
 
  UnconstrDirectMultipleShooting& operator=(
      UnconstrDirectMultipleShooting&&) noexcept = default;
 
  void setNumThreads(const int nthreads);
 
  void initConstraints(aligned_vector<Robot>& robots,
                       const std::vector<GridInfo>& time_discretization, 
                       const Solution& s);
 
  bool isFeasible(aligned_vector<Robot>& robots, 
                  const std::vector<GridInfo>& time_discretization, 
                  const Solution& s);
 
  void evalOCP(aligned_vector<Robot>& robots, 
               const std::vector<GridInfo>& time_discretization, 
               const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
               const Solution& s, KKTResidual& kkt_residual);
 
  void evalKKT(aligned_vector<Robot>& robots, 
               const std::vector<GridInfo>& time_discretization, 
               const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
               const Solution& s, KKTMatrix& kkt_matrix, 
               KKTResidual& kkt_residual);
 
  static void computeInitialStateDirection(const Eigen::VectorXd& q, 
                                           const Eigen::VectorXd& v, 
                                           const Solution& s, Direction& d);
 
  const PerformanceIndex& getEval() const;
 
  void computeStepSizes(const std::vector<GridInfo>& time_discretization,
                        KKTMatrix& kkt_matrix, KKTResidual& kkt_residual,
                        Direction& d);
 
  double maxPrimalStepSize() const;
 
  double maxDualStepSize() const;
 
  void integrateSolution(const aligned_vector<Robot>& robots,
                         const std::vector<GridInfo>& time_discretization, 
                         const double primal_step_size,
                         const double dual_step_size, Direction& d, Solution& s);
 
  void integratePrimalSolution(const aligned_vector<Robot>& robots,
                               const std::vector<GridInfo>& time_discretization, 
                               const double primal_step_size,
                               const Direction& d, Solution& s);
 
private:
  int nthreads_;
  UnconstrIntermediateStage intermediate_stage_;
  UnconstrTerminalStage terminal_stage_;
  aligned_vector<UnconstrOCPData> data_;
  PerformanceIndex performance_index_; 
  Eigen::VectorXd max_primal_step_sizes_, max_dual_step_sizes_;
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_UNCONSTR_DIRECT_MULTIPLE_SHOOTING_HPP_