parnmpc_terminal_stage.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_UNCONSTR_PARNMPC_STAGE_HPP_
#define ROBOTOC_UNCONSTR_PARNMPC_STAGE_HPP_
 
#include <memory>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/contact_status.hpp"
#include "robotoc/core/split_solution.hpp"
#include "robotoc/core/split_direction.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
#include "robotoc/cost/cost_function.hpp"
#include "robotoc/cost/cost_function_data.hpp"
#include "robotoc/constraints/constraints.hpp"
#include "robotoc/constraints/constraints_data.hpp"
#include "robotoc/unconstr/unconstr_ocp_data.hpp"
#include "robotoc/ocp/grid_info.hpp"
 
 
namespace robotoc {
 
class ParNMPCTerminalStage {
public:
  ParNMPCTerminalStage(const Robot& robot, 
                       const std::shared_ptr<CostFunction>& cost,
                       const std::shared_ptr<Constraints>& constraints);
 
  ParNMPCTerminalStage();
 
  ~ParNMPCTerminalStage() = default;
 
  ParNMPCTerminalStage(const ParNMPCTerminalStage&) = default;
 
  ParNMPCTerminalStage& operator=(const ParNMPCTerminalStage&) = default;
 
  ParNMPCTerminalStage(ParNMPCTerminalStage&&) noexcept = default;
 
  ParNMPCTerminalStage& operator=(ParNMPCTerminalStage&&) noexcept = default;
 
  UnconstrOCPData createData(const Robot& robot) const;
 
  bool isFeasible(Robot& robot, const GridInfo& grid_info,
                  const SplitSolution& s, UnconstrOCPData& data) const;
 
  void initConstraints(Robot& robot, const GridInfo& grid_info, 
                       const SplitSolution& s, UnconstrOCPData& data) const;
 
  void evalOCP(Robot& robot, const GridInfo& grid_info, 
               const Eigen::VectorXd& q_prev, const Eigen::VectorXd& v_prev, 
               const SplitSolution& s, UnconstrOCPData& data, 
               SplitKKTResidual& kkt_residual) const;
 
  void evalKKT(Robot& robot, const GridInfo& grid_info, 
               const Eigen::VectorXd& q_prev, const Eigen::VectorXd& v_prev, 
               const SplitSolution& s, UnconstrOCPData& data,
               SplitKKTMatrix& kkt_matrix, SplitKKTResidual& kkt_residual) const;
 
  void expandPrimalAndDual(const double dt, const SplitKKTMatrix& kkt_matrix, 
                           const SplitKKTResidual& kkt_residual, 
                           UnconstrOCPData& data, SplitDirection& d) const; 
 
  double maxPrimalStepSize(const UnconstrOCPData& data) const;
 
  double maxDualStepSize(const UnconstrOCPData& data) const;
 
  void updatePrimal(const Robot& robot, const double primal_step_size, 
                    const SplitDirection& d, SplitSolution& s, 
                    UnconstrOCPData& data) const;
 
  void updateDual(const double dual_step_size, UnconstrOCPData& data) const;
 
  void evalTerminalCostHessian(Robot& robot, const GridInfo& grid_info, 
                               const SplitSolution& s, 
                               UnconstrOCPData& data,
                               SplitKKTMatrix& kkt_matrix,
                               SplitKKTResidual& kkt_residual) const;
 
private:
  std::shared_ptr<CostFunction> cost_;
  std::shared_ptr<Constraints> constraints_;
  ContactStatus contact_status_;
};
 
} // namespace robotoc
 
#endif // ROBOTOC_UNCONSTR_PARNMPC_STAGE_HPP_