direct_multiple_shooting.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_DIRECT_MULTIPLE_SHOOTING_HPP_ 
#define ROBOTOC_DIRECT_MULTIPLE_SHOOTING_HPP_
 
#include <vector>
#include <memory>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/utils/aligned_vector.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/planner/contact_sequence.hpp"
#include "robotoc/ocp/ocp.hpp"
#include "robotoc/ocp/grid_info.hpp"
#include "robotoc/ocp/time_discretization.hpp"
#include "robotoc/ocp/intermediate_stage.hpp"
#include "robotoc/ocp/impact_stage.hpp"
#include "robotoc/ocp/terminal_stage.hpp"
 
 
namespace robotoc {
 
class DirectMultipleShooting {
public:
  DirectMultipleShooting(const OCP& ocp, const int nthreads);
 
  DirectMultipleShooting();
 
  ~DirectMultipleShooting() = default;
 
  DirectMultipleShooting(const DirectMultipleShooting&) = default;
 
  DirectMultipleShooting& operator=(const DirectMultipleShooting&) = default;
 
  DirectMultipleShooting(DirectMultipleShooting&&) noexcept = default;
 
  DirectMultipleShooting& operator=(DirectMultipleShooting&&) noexcept = default;
 
  void setNumThreads(const int nthreads);
 
  void initConstraints(aligned_vector<Robot>& robots,
                       const TimeDiscretization& time_discretization, 
                       const Solution& s);
 
  bool isFeasible(aligned_vector<Robot>& robots, 
                  const TimeDiscretization& time_discretization, 
                  const Solution& s);
 
  void evalOCP(aligned_vector<Robot>& robots, 
               const TimeDiscretization& time_discretization, 
               const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
               const Solution& s, KKTResidual& kkt_residual);
 
  void evalKKT(aligned_vector<Robot>& robots, 
               const TimeDiscretization& time_discretization, 
               const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
               const Solution& s, KKTMatrix& kkt_matrix, 
               KKTResidual& kkt_residual);
 
  void computeInitialStateDirection(const Robot& robot,  
                                    const Eigen::VectorXd& q, 
                                    const Eigen::VectorXd& v, 
                                    const Solution& s, Direction& d) const;
 
  const PerformanceIndex& getEval() const;
 
  void computeStepSizes(const TimeDiscretization& time_discretization,
                        Direction& d);
 
  double maxPrimalStepSize() const;
 
  double maxDualStepSize() const;
 
  void integrateSolution(const aligned_vector<Robot>& robots,
                         const TimeDiscretization& time_discretization, 
                         const double primal_step_size,
                         const double dual_step_size, Direction& d, Solution& s);
 
  void integratePrimalSolution(const aligned_vector<Robot>& robots,
                               const TimeDiscretization& time_discretization, 
                               const double primal_step_size,
                               const Direction& d, Solution& s);
 
  void resizeData(const TimeDiscretization& time_discretization);
 
private:
  int nthreads_;
  aligned_vector<OCPData> ocp_data_;
  IntermediateStage intermediate_stage_;
  ImpactStage impact_stage_;
  TerminalStage terminal_stage_;
  PerformanceIndex performance_index_; 
  Eigen::VectorXd max_primal_step_sizes_, max_dual_step_sizes_;
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_DIRECT_MULTIPLE_SHOOTING_HPP_