sto_cost_function.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_STO_COST_FUNCTION_HPP_
#define ROBOTOC_STO_COST_FUNCTION_HPP_
 
#include <vector>
#include <memory>
#include <unordered_map>
 
#include "Eigen/Core"
 
#include "robotoc/ocp/time_discretization.hpp"
#include "robotoc/sto/sto_cost_function_component_base.hpp"
#include "robotoc/core/kkt_residual.hpp"
#include "robotoc/core/kkt_matrix.hpp"
 
 
namespace robotoc {
 
class STOCostFunction {
public:
  using STOCostFunctionComponentBasePtr 
      = std::shared_ptr<STOCostFunctionComponentBase>;
 
  STOCostFunction();
 
  ~STOCostFunction() = default;
 
  STOCostFunction(const STOCostFunction&) = default;
 
  STOCostFunction& operator=(const STOCostFunction&) = default;
 
  STOCostFunction(STOCostFunction&&) noexcept = default;
 
  STOCostFunction& operator=(STOCostFunction&&) noexcept = default;
 
  bool exist(const std::string& name) const;
 
  void add(const std::string& name, const STOCostFunctionComponentBasePtr& cost);
 
  void erase(const std::string& name);
 
  STOCostFunctionComponentBasePtr get(const std::string& name) const;
 
  void clear();
 
  double evalCost(const TimeDiscretization& time_discretization) const;
 
  double linearizeCost(const TimeDiscretization& time_discretization,
                       Eigen::VectorXd& lt) const;
 
  double quadratizeCost(const TimeDiscretization& time_discretization,
                        Eigen::VectorXd& lt, Eigen::MatrixXd& Qtt) const;
 
private:
  std::vector<STOCostFunctionComponentBasePtr> costs_;
  std::unordered_map<std::string, size_t> cost_names_;
 
};
 
} // namespace robotoc
 
#endif // ROBOTOC_STO_COST_FUNCTION_HPP_