configuration_space_cost.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_CONFIGURATION_SPACE_COST_HPP_
#define ROBOTOC_CONFIGURATION_SPACE_COST_HPP_
 
#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"
#include "robotoc/cost/configuration_space_ref_base.hpp"
 
 
namespace robotoc {
 
class ConfigurationSpaceCost final : public CostFunctionComponentBase {
public:
  using Vector6d = Eigen::Matrix<double, 6, 1>;
 
  ConfigurationSpaceCost(const Robot& robot);
 
  ConfigurationSpaceCost(const Robot& robot, 
                         const std::shared_ptr<ConfigurationSpaceRefBase>& ref);
 
  ConfigurationSpaceCost();
 
  ~ConfigurationSpaceCost();
 
  ConfigurationSpaceCost(const ConfigurationSpaceCost&) = default;
 
  ConfigurationSpaceCost& operator=(const ConfigurationSpaceCost&) = default;
 
  ConfigurationSpaceCost(ConfigurationSpaceCost&&) noexcept = default;
 
  ConfigurationSpaceCost& operator=(ConfigurationSpaceCost&&) noexcept = default;
 
  void set_ref(const std::shared_ptr<ConfigurationSpaceRefBase>& ref);
 
  void set_q_ref(const Eigen::VectorXd& q_ref);
 
  void set_v_ref(const Eigen::VectorXd& v_ref);
 
  void set_u_ref(const Eigen::VectorXd& u_ref);
 
  void set_q_weight(const Eigen::VectorXd& q_weight);
 
  void set_v_weight(const Eigen::VectorXd& v_weight);
 
  void set_a_weight(const Eigen::VectorXd& a_weight);
 
  void set_u_weight(const Eigen::VectorXd& u_weight);
 
  void set_q_weight_terminal(const Eigen::VectorXd& q_weight_terminal);
 
  void set_v_weight_terminal(const Eigen::VectorXd& v_weight_terminal);
 
  void set_q_weight_impact(const Eigen::VectorXd& q_weight_impact);
 
  void set_v_weight_impact(const Eigen::VectorXd& v_weight_impact);
 
  void set_dv_weight_impact(const Eigen::VectorXd& dv_weight_impact);
 
  bool isCostConfigActive(const GridInfo& grid_info) const {
    if (use_nonconst_ref_) {
      return ref_->isActive(grid_info);
    }
    else {
      return true;
    }
  }
 
  void evalConfigDiff(const Robot& robot, CostFunctionData& data, 
                      const GridInfo& grid_info, 
                      const Eigen::VectorXd& q) const {
    if (use_nonconst_ref_) {
      if (ref_->isActive(grid_info)) {
        ref_->updateRef(robot, grid_info, data.q_ref);
        robot.subtractConfiguration(q, data.q_ref, data.qdiff);
      }
    }
    else {
      robot.subtractConfiguration(q, q_ref_, data.qdiff);
    }
  }
 
  void evalConfigDiffJac(const Robot& robot, CostFunctionData& data, 
                         const GridInfo& grid_info, 
                         const Eigen::VectorXd& q) const {
    if (use_nonconst_ref_) {
      if (ref_->isActive(grid_info)) {
        robot.dSubtractConfiguration_dqf(q, data.q_ref, data.J_qdiff);
      }
    }
    else {
      robot.dSubtractConfiguration_dqf(q, q_ref_, data.J_qdiff);
    }
  }
 
  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;
 
private:
  int dimq_, dimv_, dimu_;
  Eigen::VectorXd q_ref_, v_ref_, u_ref_, 
                  q_weight_, v_weight_, a_weight_, u_weight_,
                  q_weight_terminal_, v_weight_terminal_, 
                  q_weight_impact_, v_weight_impact_, dv_weight_impact_;
  std::shared_ptr<ConfigurationSpaceRefBase> ref_;
  bool use_nonconst_ref_, 
       enable_q_cost_, enable_v_cost_, enable_a_cost_, enable_u_cost_,
       enable_q_cost_terminal_, enable_v_cost_terminal_,
       enable_q_cost_impact_, enable_v_cost_impact_, enable_dv_cost_impact_;
};
 
} // namespace robotoc
 
 
#endif // ROBOTOC_CONFIGURATION_SPACE_COST_HPP_