obstacle_distance_gradient.h

Go to the documentation of this file.

#ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_COST_FUNCTIONS_OBSTACLE_DISTANCE_GRADIENT_H_
#define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_COST_FUNCTIONS_OBSTACLE_DISTANCE_GRADIENT_H_

#include <stomp_moveit/cost_functions/stomp_cost_function.h>
#include <array>

namespace stomp_moveit
{
namespace cost_functions
{
class ObstacleDistanceGradient : public StompCostFunction
{
public:
  ObstacleDistanceGradient();
  virtual ~ObstacleDistanceGradient();

  virtual bool initialize(moveit::core::RobotModelConstPtr robot_model_ptr, const std::string& group_name,
                          XmlRpc::XmlRpcValue& config) override;

  virtual bool configure(const XmlRpc::XmlRpcValue& config) override;


  virtual bool setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                    const moveit_msgs::MotionPlanRequest &req,
                                    const stomp_core::StompConfiguration &config,
                                    moveit_msgs::MoveItErrorCodes& error_code) override;

  virtual bool computeCosts(const Eigen::MatrixXd& parameters, std::size_t start_timestep, std::size_t num_timesteps,
                            int iteration_number, int rollout_number, Eigen::VectorXd& costs, bool& validity) override;

  virtual std::string getGroupName() const override
  {
    return group_name_;
  }

  virtual std::string getName() const override
  {
    return name_ + "/" + group_name_  ;
  }

  virtual void done(bool success,int total_iterations,double final_cost,const Eigen::MatrixXd& parameters) override;


protected:

  bool checkIntermediateCollisions(const Eigen::VectorXd& start, const Eigen::VectorXd& end,double longest_valid_joint_move);


  std::string name_;

  // robot details
  std::string group_name_;
  moveit::core::RobotModelConstPtr robot_model_ptr_;
  moveit::core::RobotStatePtr robot_state_;

  // intermediate collision check support
  std::array<moveit::core::RobotStatePtr,3 > intermediate_coll_states_;   
  // planning context information
  planning_scene::PlanningSceneConstPtr planning_scene_;
  moveit_msgs::MotionPlanRequest plan_request_;

  // distance and collision check
  collision_detection::CollisionRequest collision_request_;
  collision_detection::CollisionResult collision_result_;

  // parameters
  double max_distance_;               
  double longest_valid_joint_move_;   
};

} /* namespace cost_functions */
} /* namespace stomp_moveit */

#endif /* INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_COST_FUNCTIONS_OBSTACLE_DISTANCE_GRADIENT_H_ */