pr2_arm_kinematics_constraint_aware: pr2_arm_kinematics_constraint_aware: pr2_arm_kinematics_constraint

00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: Sachin Chitta
00036  *********************************************************************/
00037 
00038 #ifndef PR2_ARM_IK_CONSTRAINT_AWARE_H
00039 #define PR2_ARM_IK_CONSTRAINT_AWARE_H
00040 
00041 #include <angles/angles.h>
00042 #include <pr2_arm_kinematics/pr2_arm_kinematics.h>
00043 
00044 //Pose command for the ik node
00045 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00046 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00047 #include <kinematics_msgs/GetPositionFK.h>
00048 
00049 #include <boost/shared_ptr.hpp>
00050 
00051 #include <planning_environment/models/collision_models_interface.h>
00052 
00053 #include <arm_navigation_msgs/DisplayTrajectory.h>
00054 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00055 
00056 #include <urdf/model.h>
00057 
00058 namespace pr2_arm_kinematics
00059 {
00060 class PR2ArmIKConstraintAware : public pr2_arm_kinematics::PR2ArmKinematics
00061 {
00062 public:
00063 
00074   PR2ArmIKConstraintAware();
00075 
00076   virtual ~PR2ArmIKConstraintAware()
00077   {
00078     if (collision_models_interface_)
00079       delete collision_models_interface_;
00080   };
00081 
00092   int CartToJntSearch(const KDL::JntArray& q_in, 
00093                       const KDL::Frame& p_in, 
00094                       KDL::JntArray &q_out, 
00095                       const double &timeout, 
00096                       arm_navigation_msgs::ArmNavigationErrorCodes& error_code);
00097 
00108   bool getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request, 
00109                                     kinematics_msgs::GetConstraintAwarePositionIK::Response &response);
00110 
00111   virtual bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request, 
00112                              kinematics_msgs::GetPositionIK::Response &response);
00113   
00114  protected:
00115 
00116     virtual bool transformPose(const std::string& des_frame,
00117                                const geometry_msgs::PoseStamped& pose_in,
00118                                geometry_msgs::PoseStamped& pose_out);
00119 
00120 
00121 private:
00122 
00123   ros::ServiceServer ik_collision_service_;
00124   planning_environment::CollisionModelsInterface *collision_models_interface_;
00125   std::string group_;
00126   bool use_collision_map_;
00127   ros::Publisher vis_marker_publisher_;
00128   ros::Publisher vis_marker_array_publisher_;
00129   void contactFound(collision_space::EnvironmentModel::Contact &contact);
00130   std::vector<std::string> end_effector_collision_links_;
00131   std::vector<std::string> arm_links_;
00132   void initialPoseCheck(const KDL::JntArray &jnt_array, 
00133                         const KDL::Frame &p_in,
00134                         arm_navigation_msgs::ArmNavigationErrorCodes &error_code);
00135   void collisionCheck(const KDL::JntArray &jnt_array, 
00136                       const KDL::Frame &p_in,
00137                       arm_navigation_msgs::ArmNavigationErrorCodes &error_code);
00138   void printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector);
00139   ros::Publisher display_trajectory_publisher_;
00140   bool visualize_solution_;
00141   kinematics_msgs::PositionIKRequest ik_request_;
00142   ros::Time last_planning_scene_drop_;
00143   arm_navigation_msgs::Constraints constraints_;
00144   void advertiseIK();
00145 
00146   bool isReady(arm_navigation_msgs::ArmNavigationErrorCodes &error_code);
00147   void sendEndEffectorPose(const planning_models::KinematicState* state, bool valid);
00148 };
00149 }
00150 #endif
pr2_arm_kinematics_constraint_aware.h
