grasp_tester_fast.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 // Author(s): E. Gil Jones
00035 
00036 #ifndef _GRASP_TESTER_FAST_
00037 #define _GRASP_TESTER_FAST_
00038 
00039 #include "object_manipulator/grasp_execution/approach_lift_grasp.h"
00040 #include <arm_kinematics_constraint_aware/arm_kinematics_solver_constraint_aware.h>
00041 #include <pluginlib/class_loader.h>
00042 
00043 //#include <pr2_arm_kinematics_constraint_aware/pr2_arm_ik_solver_constraint_aware.h>
00044 
00045 namespace object_manipulator {
00046 
00048 
00066 inline geometry_msgs::Vector3 doNegate(const geometry_msgs::Vector3& vec) {
00067   geometry_msgs::Vector3 v;
00068   v.x = - vec.x;
00069   v.y = - vec.y;
00070   v.z = - vec.z;
00071   return v;
00072 }
00073 
00074 class GraspTesterFast : public GraspTester
00075 {
00076 protected:
00077   virtual void testGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00078                          const manipulation_msgs::Grasp &grasp,
00079                          GraspExecutionInfo &execution_info);  
00080   
00082   virtual std::vector<arm_navigation_msgs::LinkPadding> 
00083     linkPaddingForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal);
00084 
00085   bool getInterpolatedIK(const std::string& arm_name,
00086                          const tf::Transform& first_pose,
00087                          const tf::Vector3& direction,
00088                          const double& distance,
00089                          const std::vector<double>& ik_solution,
00090                          const bool& reverse, 
00091                          const bool& premultiply,
00092                          trajectory_msgs::JointTrajectory& traj);
00093 
00094   //arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware* right_arm_solver_;
00095 
00096   std::map<std::string, arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware*> ik_solver_map_;
00097   //std::map<std::string, pr2_arm_kinematics::PR2ArmIKSolverConstraintAware*> ik_solver_map_;
00098   
00099   double consistent_angle_;
00100   unsigned int num_points_;
00101   unsigned int redundancy_;
00102   
00103   ros::Publisher vis_marker_array_publisher_;
00104   ros::Publisher vis_marker_publisher_;
00105 
00106   planning_environment::CollisionModels* getCollisionModels();
00107   planning_models::KinematicState* getPlanningSceneState();
00108 
00109   planning_environment::CollisionModels* cm_;
00110   planning_models::KinematicState* state_;
00111 
00112  public:
00113 
00114   pluginlib::ClassLoader<kinematics::KinematicsBase> kinematics_loader_;
00115 
00117   GraspTesterFast(planning_environment::CollisionModels* cm = NULL,
00118                   const std::string& plugin_name="pr2_arm_kinematics/PR2ArmKinematicsPlugin");
00119 
00120   ~GraspTesterFast();
00121 
00122   void setPlanningSceneState(planning_models::KinematicState* state) {
00123     state_ = state;
00124   }
00125 
00126   void getGroupJoints(const std::string& group_name,
00127                       std::vector<std::string>& group_links);
00128   
00129   void getGroupLinks(const std::string& group_name,
00130                      std::vector<std::string>& group_links);
00131 
00132   virtual void testGrasps(const object_manipulation_msgs::PickupGoal &pickup_goal,
00133                           const std::vector<manipulation_msgs::Grasp> &grasps,
00134                           std::vector<GraspExecutionInfo> &execution_info,
00135                           bool return_on_first_hit);
00136 };
00137 
00138 } //namespace object_manipulator
00139 
00140 #endif


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Mon Oct 6 2014 02:59:50