katana_kinematics_constraint_aware: katana_kinematics_constraint_aware: katana_openrave

00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2011  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  *
00020  * katana_kinematics_plugin.cpp
00021  * based on the pr2_arm_kinematics_plugin.h by Sachin Chitta
00022  *
00023  *  Created on: 30.05.2010
00024  *  Author: Henning Deeken  // hdeeken@uos.de
00025  */
00026 #ifndef KATANA_OPENRAVE_KINEMATICS_H
00027 #define KATANA_OPENRAVE_KINEMATICS_H
00028 
00029 #include <algorithm>
00030 #include <numeric>
00031 #include <boost/shared_ptr.hpp>
00032 #include <ros/ros.h>
00033 #include <tf/tf.h>
00034 #include <tf/transform_listener.h>
00035 #include <angles/angles.h>
00036 #include <geometry_msgs/PoseStamped.h>
00037 #include <kinematics_msgs/GetPositionFK.h>
00038 #include <kinematics_msgs/GetPositionIK.h>
00039 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00040 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00041 #include <orrosplanning/IK.h>
00042 #include <urdf/model.h>
00043 #include <kinematics_base/kinematics_base.h>
00044 #include <arm_kinematics_constraint_aware/arm_kinematics_constraint_aware_utils.h>
00045 #include <ompl/util/RandomNumbers.h>
00046 
00047 namespace katana_kinematics_constraint_aware
00048 {
00049 class KatanaKinematicsPlugin : public kinematics::KinematicsBase
00050 {
00051 public:
00052 
00056   KatanaKinematicsPlugin();
00057 
00062   bool isActive();
00063 
00071   bool getPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_seed_state,
00072                      std::vector<double> &solution, int &error_code);
00073 
00083   bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_seed_state,
00084                         const double &timeout, std::vector<double> &solution, int &error_code);
00093   bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_seed_state,
00094                         const double &timeout, std::vector<double> &solution, const boost::function<
00095                             void(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_solution,
00096                                  int &error_code)> &desired_pose_callback, const boost::function<
00097                             void(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_solution,
00098                                  int &error_code)> &solution_callback, int &error_code);
00099 
00106   bool getPositionFK(const std::vector<std::string> &link_names, const std::vector<double> &joint_angles, std::vector<
00107       geometry_msgs::Pose> &poses);
00108 
00113   bool initialize(std::string name);
00114 
00119   std::string getBaseFrame();
00120 
00124   std::string getToolFrame();
00125 
00129   std::vector<std::string> getJointNames();
00130 
00134   std::vector<std::string> getLinkNames();
00135 
00136 protected:
00137 
00138   bool active_;
00139   urdf::Model robot_model_;
00140 
00141   ros::NodeHandle node_handle_, root_handle_;
00142 
00143   ros::ServiceClient ik_service_, fk_service_, ik_solver_info_service_, fk_solver_info_service_;
00144   tf::TransformListener tf_;
00145   std::string root_name_;
00146   int dimension_;
00147 
00148   kinematics_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_;
00149 
00150   boost::function<void(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_solution, int &error_code)>
00151                                                                                                                      desiredPoseCallback_;
00152   boost::function<void(const geometry_msgs::Pose &ik_pose, const std::vector<double> &ik_solution, int &error_code)>
00153                                                                                                                      solutionCallback_;
00154   void desiredPoseCallback(const std::vector<double>& joint_angles, const geometry_msgs::Pose& ik_pose,
00155                            arm_navigation_msgs::ArmNavigationErrorCodes& error_code);
00156 
00157   void jointSolutionCallback(const std::vector<double>& joint_angles,
00158                                                      const geometry_msgs::Pose& ik_pose,
00159                                                      arm_navigation_msgs::ArmNavigationErrorCodes& error_code);
00160 
00161 };
00162 }
00163 
00164 #endif
katana_openrave_kinematics.h
