task_space_6d_cost.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_TASK_SPACE_6D_COST_HPP_
#define ROBOTOC_TASK_SPACE_6D_COST_HPP_
 
#include <string>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/contact_status.hpp"
#include "robotoc/robot/impact_status.hpp"
#include "robotoc/robot/se3.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/task_space_6d_ref_base.hpp"
 
 
namespace robotoc {
 
class TaskSpace6DCost final : public CostFunctionComponentBase {
public:
  using Vector6d = Eigen::Matrix<double, 6, 1>;
 
  TaskSpace6DCost(const Robot& robot, const int frame_id);
 
  TaskSpace6DCost(const Robot& robot, const std::string& frame_name);
  TaskSpace6DCost(const Robot& robot, const int frame_id,
                  const std::shared_ptr<TaskSpace6DRefBase>& ref);
 
  TaskSpace6DCost(const Robot& robot, const int frame_id,
                  const SE3& const_ref);
 
  TaskSpace6DCost(const Robot& robot, const int frame_id,
                  const Eigen::Vector3d& const_position_ref,
                  const Eigen::Matrix3d& const_rotation_ref);
 
  TaskSpace6DCost(const Robot& robot, const std::string& frame_name,
                  const std::shared_ptr<TaskSpace6DRefBase>& ref);
 
  TaskSpace6DCost(const Robot& robot, const std::string& frame_name,
                  const SE3& const_ref);
 
  TaskSpace6DCost(const Robot& robot, const std::string& frame_name,
                  const Eigen::Vector3d& const_position_ref,
                  const Eigen::Matrix3d& const_rotation_ref);
 
  TaskSpace6DCost();
 
  ~TaskSpace6DCost();
 
  TaskSpace6DCost(const TaskSpace6DCost&) = default;
 
  TaskSpace6DCost& operator=(const TaskSpace6DCost&) = default;
 
  TaskSpace6DCost(TaskSpace6DCost&&) noexcept = default;
 
  TaskSpace6DCost& operator=(TaskSpace6DCost&&) noexcept = default;
 
  void set_ref(const std::shared_ptr<TaskSpace6DRefBase>& ref);
 
  void set_const_ref(const SE3& const_ref);
 
  void set_const_ref(const Eigen::Vector3d& const_position_ref,
                     const Eigen::Matrix3d& const_rotation_ref);
 
  void set_weight(const Eigen::Vector3d& weight_position,
                  const Eigen::Vector3d& weight_rotation);
 
  void set_weight_terminal(const Eigen::Vector3d& weight_position_terminal,
                           const Eigen::Vector3d& weight_rotation_terminal);
 
  void set_weight_impact(const Eigen::Vector3d& weight_position_impact,
                          const Eigen::Vector3d& weight_rotation_impact);
 
  bool isCostActive(const GridInfo& grid_info) const {
    if (use_nonconst_ref_) {
      return ref_->isActive(grid_info);
    }
    else {
      return true;
    }
  }
 
  void evalDiff(const Robot& robot, CostFunctionData& data, 
                const GridInfo& grid_info) const {
    if (use_nonconst_ref_) {
      if (ref_->isActive(grid_info)) {
        ref_->updateRef(grid_info, data.x6d_ref);
        data.x6d_ref_inv = data.x6d_ref.inverse();
        data.diff_x6d = data.x6d_ref_inv * robot.framePlacement(frame_id_);
        data.diff_6d = Log6Map(data.diff_x6d);
      }
    }
    else {
      data.diff_x6d = const_ref_inv_ * robot.framePlacement(frame_id_);
      data.diff_6d = Log6Map(data.diff_x6d);
    }
  }
 
  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 frame_id_;
  SE3 const_ref_, const_ref_inv_;
  Eigen::VectorXd weight_, weight_terminal_, weight_impact_;
  std::shared_ptr<TaskSpace6DRefBase> ref_;
  bool use_nonconst_ref_, enable_cost_, enable_cost_terminal_, enable_cost_impact_;
};
 
} // namespace robotoc
 
 
#endif // ROBOTOC_TASK_SPACE_6D_COST_HPP_