GraspSequenceValidator.h

Go to the documentation of this file.

/*
 * GraspSequenceValidator.h
 *
 *  Created on: Oct 31, 2012
 *      Author: coky
 */

#ifndef GRASPSEQUENCEVALIDATOR_H_
#define GRASPSEQUENCEVALIDATOR_H_


#include "object_manipulator/grasp_execution/approach_lift_grasp.h"
#include <arm_kinematics_constraint_aware/arm_kinematics_solver_constraint_aware.h>
#include <pluginlib/class_loader.h>
#include <object_manipulator/grasp_execution/grasp_tester_fast.h>


class GraspSequenceValidator : public object_manipulator::GraspTester
{
public:
        struct GraspSequenceDetails: public object_manipulator::GraspExecutionInfo
        {
        public:

                trajectory_msgs::JointTrajectory twist_trajectory_; // should be executed after the approach trajectory
        };

protected:

  virtual std::vector<arm_navigation_msgs::LinkPadding>
    linkPaddingForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal);

  bool getInterpolatedIK(const std::string& arm_name,
                         const tf::Transform& first_pose,
                         const tf::Vector3& direction,
                         const double& distance,
                         const std::vector<double>& ik_solution,
                         const bool& reverse,
                         const bool& premultiply,
                         trajectory_msgs::JointTrajectory& traj);

  bool getInterpolatedIK(const std::string& arm_name,
          const tf::Transform& initialTransform,
          const tf::Quaternion &rot,
          const std::vector<double>& ikSolutionAtStart,
          std::vector<double>& ikSolutionAtEnd,
          trajectory_msgs::JointTrajectory& traj);

  std::map<std::string, arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware*> ik_solver_map_;
  double consistent_angle_;
  unsigned int num_points_;
  unsigned int redundancy_;

  ros::Publisher vis_marker_array_publisher_;
  ros::Publisher vis_marker_publisher_;

  planning_environment::CollisionModels* getCollisionModels();
  planning_models::KinematicState* getPlanningSceneState();

  planning_environment::CollisionModels* cm_;
  planning_models::KinematicState* state_;

 public:

  pluginlib::ClassLoader<kinematics::KinematicsBase> kinematics_loader_;

  GraspSequenceValidator(planning_environment::CollisionModels* cm = NULL,
                  const std::string& plugin_name="");

  ~GraspSequenceValidator();

  void setPlanningSceneState(planning_models::KinematicState* state)
  {
    state_ = state;
  }

  void getGroupJoints(const std::string& group_name,
                      std::vector<std::string>& group_links);

  void getGroupLinks(const std::string& group_name,
                     std::vector<std::string>& group_links);

  virtual void testGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
          const object_manipulation_msgs::Grasp &grasp,
          object_manipulator::GraspExecutionInfo &execution_info)
  {


  }

  virtual void testGrasps(const object_manipulation_msgs::PickupGoal &pickup_goal,
                          const std::vector<object_manipulation_msgs::Grasp> &grasps,
                          std::vector<object_manipulator::GraspExecutionInfo> &execution_info,
                          bool return_on_first_hit);

  virtual void testGrasps(const object_manipulation_msgs::PickupGoal &pickup_goal,
                          const std::vector<object_manipulation_msgs::Grasp> &grasps,double twistAngle,
                          std::vector<GraspSequenceDetails> &execution_info,
                          bool return_on_first_hit);
};


#endif /* GRASPSEQUENCEVALIDATOR_H_ */