cost_function.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_COST_FUNCTION_HPP_
#define ROBOTOC_COST_FUNCTION_HPP_
 
#include <memory>
#include <cmath>
#include <vector>
#include <unordered_map>
#include <iostream>
 
#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_component_base.hpp"
#include "robotoc/cost/cost_function_data.hpp"
 
 
namespace robotoc {
 
class CostFunction {
public:
  using CostFunctionComponentBasePtr 
      = std::shared_ptr<CostFunctionComponentBase>;
 
  CostFunction(const double discount_factor, const double discount_time_step);
 
  CostFunction();
 
  ~CostFunction() = default;
 
  CostFunction(const CostFunction&) = default;
 
  CostFunction& operator=(const CostFunction&) = default;
 
  CostFunction(CostFunction&&) noexcept = default;
 
  CostFunction& operator=(CostFunction&&) noexcept = default;
 
  void setDiscountFactor(const double discount_factor, 
                         const double discount_time_step);
 
  double discountFactor() const;
 
  double discountTimeStep() const;
 
  bool exist(const std::string& name) const;
 
  void add(const std::string& name, const CostFunctionComponentBasePtr& cost);
 
  void erase(const std::string& name);
 
  CostFunctionComponentBasePtr get(const std::string& name) const;
 
  void clear();
 
  CostFunctionData createCostFunctionData(const Robot& robot) const;
 
  double evalStageCost(Robot& robot, const ContactStatus& contact_status,
                       CostFunctionData& data, const GridInfo& grid_info, 
                       const SplitSolution& s) const;
 
  double linearizeStageCost(Robot& robot, const ContactStatus& contact_status, 
                            CostFunctionData& data, const GridInfo& grid_info, 
                            const SplitSolution& s, 
                            SplitKKTResidual& kkt_residual) const;
 
  double quadratizeStageCost(Robot& robot, const ContactStatus& contact_status, 
                             CostFunctionData& data, const GridInfo& grid_info, 
                             const SplitSolution& s, 
                             SplitKKTResidual& kkt_residual,
                             SplitKKTMatrix& kkt_matrix) const;
 
  double evalTerminalCost(Robot& robot, CostFunctionData& data, 
                          const GridInfo& grid_info, 
                          const SplitSolution& s) const;
 
  double linearizeTerminalCost(Robot& robot, CostFunctionData& data, 
                               const GridInfo& grid_info, const SplitSolution& s, 
                               SplitKKTResidual& kkt_residual) const;
 
  double quadratizeTerminalCost(Robot& robot, CostFunctionData& data, 
                                const GridInfo& grid_info, const SplitSolution& s, 
                                SplitKKTResidual& kkt_residual,
                                SplitKKTMatrix& kkt_matrix) const;
 
  double evalImpactCost(Robot& robot, const ImpactStatus& impact_status, 
                         CostFunctionData& data, const GridInfo& grid_info,
                         const SplitSolution& s) const;
 
  double linearizeImpactCost(Robot& robot, const ImpactStatus& impact_status, 
                              CostFunctionData& data, const GridInfo& grid_info,
                              const SplitSolution& s, 
                              SplitKKTResidual& kkt_residual) const;
 
  double quadratizeImpactCost(Robot& robot, const ImpactStatus& impact_status, 
                               CostFunctionData& data, const GridInfo& grid_info,
                               const SplitSolution& s, 
                               SplitKKTResidual& kkt_residual,
                               SplitKKTMatrix& kkt_matrix) const;
 
  std::vector<std::string> getCostComponentList() const;
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, 
                                  const CostFunction& cost_function);
 
  friend std::ostream& operator<<(std::ostream& os, 
                                  const std::shared_ptr<CostFunction>& cost_function);
 
private:
  std::vector<CostFunctionComponentBasePtr> costs_;
  std::unordered_map<std::string, size_t> cost_names_;
 
  double discount(const double t0, const double t) const {
    assert(t >= t0);
    return std::pow(discount_factor_, ((t-t0)/discount_time_step_));
  }
 
  double discount_factor_, discount_time_step_;
  bool discounted_cost_;
};
 
} // namespace robotoc
 
#endif // ROBOTOC_COST_FUNCTION_HPP_