local_contact_force_cost.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_LOCAL_CONTACT_FORCE_COST_HPP_
#define ROBOTOC_LOCAL_CONTACT_FORCE_COST_HPP_
 
#include <vector>
 
#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/cost/cost_function_component_base.hpp"
#include "robotoc/cost/cost_function_data.hpp"
 
 
namespace robotoc {
 
class LocalContactForceCost final : public CostFunctionComponentBase {
public:
  LocalContactForceCost(const Robot& robot);
 
  LocalContactForceCost();
 
  ~LocalContactForceCost();
 
  LocalContactForceCost(const LocalContactForceCost&) = default;
 
  LocalContactForceCost& operator=(const LocalContactForceCost&) = default;
 
  LocalContactForceCost(LocalContactForceCost&&) noexcept = default;
 
  LocalContactForceCost& operator=(LocalContactForceCost&&) noexcept = default;
 
  void set_f_ref(const std::vector<Eigen::Vector3d>& f_ref);
 
  void set_f_weight(const std::vector<Eigen::Vector3d>& f_weight);
 
  void set_fi_ref(const std::vector<Eigen::Vector3d>& fi_ref);
 
  void set_fi_weight(const std::vector<Eigen::Vector3d>& fi_weight);
 
  double evalStageCost(Robot& robot, const ContactStatus& contact_status, 
                       CostFunctionData& data, const GridInfo& grid_info, 
                       const SplitSolution& s) const override;
 
  void evalStageCostDerivatives(Robot& robot, const ContactStatus& contact_status, 
                                CostFunctionData& data, const GridInfo& grid_info,
                                const SplitSolution& s, 
                                SplitKKTResidual& kkt_residual) const override;
 
  void evalStageCostHessian(Robot& robot, const ContactStatus& contact_status, 
                            CostFunctionData& data, const GridInfo& grid_info,  
                            const SplitSolution& s, 
                            SplitKKTMatrix& kkt_matrix) const override;
 
  double evalTerminalCost(Robot& robot, CostFunctionData& data, 
                          const GridInfo& grid_info, 
                          const SplitSolution& s) const override;
 
  void evalTerminalCostDerivatives(Robot& robot, CostFunctionData& data, 
                                   const GridInfo& grid_info, 
                                   const SplitSolution& s, 
                                   SplitKKTResidual& kkt_residual) const override;
 
  void evalTerminalCostHessian(Robot& robot, CostFunctionData& data, 
                               const GridInfo& grid_info, 
                               const SplitSolution& s, 
                               SplitKKTMatrix& kkt_matrix) const override;
 
  double evalImpactCost(Robot& robot, const ImpactStatus& impact_status, 
                         CostFunctionData& data, const GridInfo& grid_info, 
                         const SplitSolution& s) const override;
 
  void evalImpactCostDerivatives(Robot& robot, const ImpactStatus& impact_status, 
                                  CostFunctionData& data, const GridInfo& grid_info,
                                  const SplitSolution& s, 
                                  SplitKKTResidual& kkt_residual) const override;
 
  void evalImpactCostHessian(Robot& robot, const ImpactStatus& impact_status, 
                              CostFunctionData& data, const GridInfo& grid_info,
                              const SplitSolution& s, 
                              SplitKKTMatrix& kkt_matrix) const override;
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
private:
  int max_num_contacts_, max_dimf_;
  std::vector<ContactType> contact_types_;
  std::vector<Eigen::Vector3d> f_ref_, f_weight_, fi_ref_, fi_weight_;
 
};
 
} // namespace robotoc
 
 
#endif // ROBOTOC_LOCAL_CONTACT_FORCE_COST_HPP_