stomp_planner.h

Go to the documentation of this file.

#ifndef STOMP_MOVEIT_STOMP_PLANNER_H_
#define STOMP_MOVEIT_STOMP_PLANNER_H_

#include <moveit/planning_interface/planning_interface.h>
#include <stomp_core/stomp.h>
#include <stomp_moveit/stomp_optimization_task.h>
#include <stomp_moveit/utils/kinematics.h>
#include <boost/thread.hpp>
#include <ros/ros.h>

namespace stomp_moveit
{

using StompOptimizationTaskPtr = std::shared_ptr<StompOptimizationTask>;

class StompPlanner: public planning_interface::PlanningContext
{
public:
  StompPlanner(const std::string& group,const XmlRpc::XmlRpcValue& config,const moveit::core::RobotModelConstPtr& model);
  virtual ~StompPlanner();

  virtual bool solve(planning_interface::MotionPlanResponse &res) override;

  virtual bool solve(planning_interface::MotionPlanDetailedResponse &res) override;

  virtual bool terminate() override;

  virtual void clear() override;

  static bool getConfigData(ros::NodeHandle &nh, std::map<std::string, XmlRpc::XmlRpcValue> &config, std::string param = std::string("stomp"));

  bool canServiceRequest(const moveit_msgs::MotionPlanRequest &req)  const;

  static moveit_msgs::TrajectoryConstraints encodeSeedTrajectory(const trajectory_msgs::JointTrajectory& seed);

protected:

  void setup();

  bool getStartAndGoal(Eigen::VectorXd& start, Eigen::VectorXd& goal);

  bool getSeedParameters(Eigen::MatrixXd& parameters) const;

  bool parametersToJointTrajectory(const Eigen::MatrixXd& parameters, trajectory_msgs::JointTrajectory& traj);

  bool jointTrajectorytoParameters(const trajectory_msgs::JointTrajectory& traj, Eigen::MatrixXd& parameters) const;

  bool extractSeedTrajectory(const moveit_msgs::MotionPlanRequest& req, trajectory_msgs::JointTrajectory& seed) const;

protected:

  // stomp optimization
  std::shared_ptr< stomp_core::Stomp> stomp_;
  StompOptimizationTaskPtr task_;
  XmlRpc::XmlRpcValue config_;
  stomp_core::StompConfiguration stomp_config_;

  // robot model
  moveit::core::RobotModelConstPtr robot_model_;
  utils::kinematics::IKSolverPtr ik_solver_;

  // ros tasks
  ros::NodeHandlePtr ph_;
};


} /* namespace stomp_moveit */
#endif /* STOMP_MOVEIT_STOMP_PLANNER_H_ */