tool_goal_pose.h

Go to the documentation of this file.

#ifndef INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_COST_FUNCTIONS_TOOL_GOAL_POSE_H_
#define INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_COST_FUNCTIONS_TOOL_GOAL_POSE_H_

#include <Eigen/Sparse>
#include <moveit/robot_model/robot_model.h>
#include <moveit/collision_detection_fcl/collision_world_fcl.h>
#include <moveit/collision_detection_fcl/collision_robot_fcl.h>
#include "stomp_moveit/cost_functions/stomp_cost_function.h"

namespace stomp_moveit
{
namespace cost_functions
{

class ToolGoalPose: public StompCostFunction
{
public:
  ToolGoalPose();
  virtual ~ToolGoalPose();

  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:

  std::string name_;

  // robot details
  std::string group_name_;
  std::string tool_link_;
  moveit::core::RobotModelConstPtr robot_model_;
  moveit::core::RobotStatePtr state_;

  // planning context information
  planning_scene::PlanningSceneConstPtr planning_scene_;
  moveit_msgs::MotionPlanRequest plan_request_;

  // goal pose
  Eigen::Affine3d tool_goal_pose_;                    
  // ros parameters
  Eigen::ArrayXi dof_nullity_;                        
  std::pair<double,double> position_error_range_;     
  std::pair<double,double> orientation_error_range_;  
  double position_cost_weight_;                       
  double orientation_cost_weight_;                    
  // support variables
  Eigen::VectorXd last_joint_pose_;
  Eigen::Affine3d last_tool_pose_;
  Eigen::VectorXd tool_twist_error_;


};

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

#endif /* INDUSTRIAL_MOVEIT_STOMP_MOVEIT_INCLUDE_STOMP_MOVEIT_COST_FUNCTIONS_TOOL_GOAL_POSE_H_ */