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 * KNIKinematics.h 00020 * 00021 * Created on: 20.01.2011 00022 * Author: Martin Günther 00023 */ 00024 00025 #ifndef KNIKINEMATICS_H_ 00026 #define KNIKINEMATICS_H_ 00027 00028 #include <ros/ros.h> 00029 #include <ros/package.h> 00030 #include <tf/transform_listener.h> 00031 #include <geometry_msgs/PoseStamped.h> 00032 #include <kinematics_msgs/GetKinematicSolverInfo.h> 00033 #include <kinematics_msgs/GetPositionFK.h> 00034 #include <kinematics_msgs/GetPositionIK.h> 00035 #include <urdf/model.h> 00036 00037 #include <KNI_InvKin/ikBase.h> 00038 #include <KNI/kmlFactories.h> 00039 #include <KNI_InvKin/KatanaKinematics.h> 00040 00041 #include <katana/KNIConverter.h> 00042 #include <katana/EulerTransformationMatrices.hh> 00043 00044 #include "tf/LinearMath/Transform.h" 00045 00046 namespace katana 00047 { 00048 00049 class KNIKinematics 00050 { 00051 public: 00052 KNIKinematics(); 00053 virtual ~KNIKinematics(); 00054 00055 private: 00056 ros::NodeHandle nh_; 00057 ros::ServiceServer get_kinematic_solver_info_server_; 00058 ros::ServiceServer get_fk_server_; 00059 ros::ServiceServer get_ik_server_; 00060 00061 std::vector<std::string> joint_names_; 00062 std::vector<arm_navigation_msgs::JointLimits> joint_limits_; 00063 00064 CikBase ikBase_; 00065 KNIConverter* converter_; 00066 tf::TransformListener tf_listener_; 00067 00068 bool get_kinematic_solver_info(kinematics_msgs::GetKinematicSolverInfo::Request &req, 00069 kinematics_msgs::GetKinematicSolverInfo::Response &res); 00070 00071 bool get_position_fk(kinematics_msgs::GetPositionFK::Request &req, kinematics_msgs::GetPositionFK::Response &res); 00072 bool get_position_ik(kinematics_msgs::GetPositionIK::Request &req, kinematics_msgs::GetPositionIK::Response &res); 00073 00074 std::vector<double> getCoordinates(); 00075 std::vector<double> getCoordinates(std::vector<double> jointAngles); 00076 std::vector<int> makeJointsLookup(std::vector<std::string> &joint_names); 00077 00078 }; 00079 00080 } 00081 00082 #endif /* KNIKINEMATICS_H_ */