tree_kinematics.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (GNU Lesser General Public License)
00003  *
00004  * Copyright (c) 2011, PAL Robotics, S.L.
00005  * All rights reserved.
00006  *
00007  * This library is free software; you can redistribute it and/or
00008  * modify it under the terms of the GNU Lesser General Public
00009  * License as published by the Free Software Foundation; either
00010  * version 2.1 of the License, or (at your option) any later version.
00011  *
00012  * This library is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015  * Lesser General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU Lesser General Public
00018  * License along with this library; if not, write to the Free Software
00019  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00020  */
00029 #include <vector>
00030 #include <string>
00031 #include <boost/scoped_ptr.hpp>
00032 
00033 #include <ros/ros.h>
00034 #include <tf/transform_listener.h>
00035 #include <geometry_msgs/PoseStamped.h>
00036 #include <kdl_parser/kdl_parser.hpp>
00037 
00038 #include <kdl/tree.hpp>
00039 #include <kdl/frames.hpp>
00040 #include <kdl/jntarray.hpp>
00041 
00042 #include <kdl/treefksolver.hpp>
00043 #include <kdl/treeiksolver.hpp>
00044 #include <kdl/treefksolverpos_recursive.hpp>
00045 #include <kdl/treeiksolvervel_wdls.hpp>
00046 #include <tree_kinematics/treeiksolverpos_online.hpp>
00047 
00048 // for self-collision checking
00049 #include <boost/scoped_ptr.hpp>
00050 
00051 #include <kinematics_msgs/KinematicSolverInfo.h>
00052 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00053 #include <tree_kinematics/get_tree_position_ik.h>
00054 #include <kinematics_msgs/GetPositionFK.h>
00055 
00056 
00057 namespace tree_kinematics
00058 {
00059 
00068 class TreeKinematics
00069 {
00070   public:
00071     TreeKinematics(): nh_private_("~")
00072     {
00073       loop_count_ = 1;
00074       ik_srv_duration_ = 0.0;
00075       ik_srv_duration_median_ = 0.0;
00076       ik_duration_ = 0.0;
00077       ik_duration_median_ = 0.0;
00078     };
00079 
00080     ~TreeKinematics(){};
00081 
00093     bool init();
00094 
00109     bool getPositionFk(kinematics_msgs::GetPositionFK::Request& request,
00110                        kinematics_msgs::GetPositionFK::Response& response);
00111 
00125     bool getPositionIk(tree_kinematics::get_tree_position_ik::Request &request,
00126                        tree_kinematics::get_tree_position_ik::Response &response);
00127 
00128   private:
00129     ros::NodeHandle nh_, nh_private_;
00130     KDL::Tree kdl_tree_;                // KDL representation of a tree
00131     std::string tree_root_name_;        // The name of the tree's root
00132     unsigned int nr_of_jnts_;           // number of joints in the KDL::Tree
00133     int srv_call_frequency_;            // how often the service is called per second
00134     KDL::MatrixXd     js_w_matr_;       // matrix of joint weights for the IK velocity solver
00135     KDL::MatrixXd     ts_w_matr_;       // matrix for task space weights for the IK velocity solver
00136     double            lambda_;          // damping factor for the IK velocity solver
00137 
00138     boost::scoped_ptr<KDL::TreeFkSolverPos>         fk_solver_;
00139     boost::scoped_ptr<KDL::TreeIkSolverVel_wdls>    ik_vel_solver_;
00140     boost::scoped_ptr<KDL::TreeIkSolverPos_Online>  ik_pos_solver_;
00141 
00142     ros::ServiceServer fk_service_, ik_service_;
00143     tf::TransformListener tf_listener_;
00144     kinematics_msgs::KinematicSolverInfo info_;
00145 
00162     bool loadModel(const std::string xml,
00163                    KDL::Tree& kdl_tree,
00164                    std::string& tree_root_name,
00165                    unsigned int& nr_of_jnts,
00166                    KDL::JntArray& joint_min,
00167                    KDL::JntArray& joint_max,
00168                    KDL::JntArray& joint_vel_max);
00169 
00187     bool readJoints(urdf::Model& robot_model,
00188                     KDL::Tree& kdl_tree,
00189                     std::string& tree_root_name,
00190                     unsigned int& nr_of_jnts,
00191                     KDL::JntArray& joint_min,
00192                     KDL::JntArray& joint_max,
00193                     KDL::JntArray& joint_vel_max);
00194 
00204     int getJointIndex(const std::string& name);
00205 
00206     unsigned int loop_count_;
00207     double ik_srv_duration_, ik_srv_duration_median_, ik_duration_, ik_duration_median_;
00208 };
00209 
00210 } // namespace
00211 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


tree_kinematics
Author(s): Marcus Liebhardt
autogenerated on Thu Jun 27 2013 16:12:20