cost_function_component_base.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_COST_FUNCTION_COMPONENT_BASE_HPP_
#define ROBOTOC_COST_FUNCTION_COMPONENT_BASE_HPP_
 
#include <memory>
#include <stdexcept>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/contact_status.hpp"
#include "robotoc/robot/impact_status.hpp"
#include "robotoc/core/split_solution.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
#include "robotoc/ocp/grid_info.hpp"
#include "robotoc/cost/cost_function_data.hpp"
 
 
namespace robotoc {
 
class CostFunctionComponentBase : public std::enable_shared_from_this<CostFunctionComponentBase> {
public:
  CostFunctionComponentBase() {}
 
  virtual ~CostFunctionComponentBase() {}
 
  CostFunctionComponentBase(const CostFunctionComponentBase&) = default;
 
  CostFunctionComponentBase& operator=(const CostFunctionComponentBase&) 
      = default;
 
  CostFunctionComponentBase(CostFunctionComponentBase&&) noexcept = default;
 
  CostFunctionComponentBase& operator=(CostFunctionComponentBase&&) noexcept 
      = default;
 
  virtual double evalStageCost(Robot& robot, const ContactStatus& contact_status,
                               CostFunctionData& data, const GridInfo& grid_info,
                               const SplitSolution& s) const = 0;
 
  virtual void evalStageCostDerivatives(Robot& robot, 
                                        const ContactStatus& contact_status,
                                        CostFunctionData& data, 
                                        const GridInfo& grid_info, 
                                        const SplitSolution& s, 
                                        SplitKKTResidual& kkt_residual) const = 0;
 
  virtual void evalStageCostHessian(Robot& robot, 
                                    const ContactStatus& contact_status,
                                    CostFunctionData& data, 
                                    const GridInfo& grid_info, 
                                    const SplitSolution& s, 
                                    SplitKKTMatrix& kkt_matrix) const = 0;
 
  virtual double evalTerminalCost(Robot& robot, CostFunctionData& data, 
                                  const GridInfo& grid_info, 
                                  const SplitSolution& s) const = 0;
 
  virtual void evalTerminalCostDerivatives(Robot& robot, 
                                           CostFunctionData& data,  
                                           const GridInfo& grid_info, 
                                           const SplitSolution& s, 
                                           SplitKKTResidual& kkt_residual) const = 0;
 
  virtual void evalTerminalCostHessian(Robot& robot, 
                                       CostFunctionData& data, 
                                       const GridInfo& grid_info, 
                                       const SplitSolution& s, 
                                       SplitKKTMatrix& kkt_matrix) const = 0;
 
  virtual double evalImpactCost(Robot& robot, 
                                 const ImpactStatus& impact_status,
                                 CostFunctionData& data, 
                                 const GridInfo& grid_info, 
                                 const SplitSolution& s) const = 0;
 
  virtual void evalImpactCostDerivatives(Robot& robot, 
                                          const ImpactStatus& impact_status, 
                                          CostFunctionData& data,  
                                          const GridInfo& grid_info, 
                                          const SplitSolution& s, 
                                          SplitKKTResidual& kkt_residual) const = 0;
 
  virtual void evalImpactCostHessian(Robot& robot, 
                                      const ImpactStatus& impact_status, 
                                      CostFunctionData& data,  
                                      const GridInfo& grid_info, 
                                      const SplitSolution& s, 
                                      SplitKKTMatrix& kkt_matrix) const = 0; 
 
  template <typename Derived>
  std::shared_ptr<Derived> as_shared_ptr() {
    auto ptr = shared_from_this();
    auto derived_ptr = std::dynamic_pointer_cast<Derived>(ptr);
    if (derived_ptr == nullptr) {
      throw std::runtime_error("[CostFunctionComponentBase] runtime error: failed in down-casting!");
    }
    return derived_ptr;
  }
 
};
 
} // namespace robotoc
 
#endif // ROBOTOC_COST_FUNCTION_COMPONENT_BASE_HPP_