kdl_wrapper.cpp
Go to the documentation of this file.
00001 /*
00002  * kdl_wrapper.cpp
00003  *
00004  *  Created on: Nov 8, 2013
00005  *      Author: Francisco Vina
00006  */
00007 
00008 /* Copyright (c) 2013, Francisco Vina, CVAP, KTH
00009    All rights reserved.
00010 
00011    Redistribution and use in source and binary forms, with or without
00012    modification, are permitted provided that the following conditions are met:
00013       * Redistributions of source code must retain the above copyright
00014         notice, this list of conditions and the following disclaimer.
00015       * Redistributions in binary form must reproduce the above copyright
00016         notice, this list of conditions and the following disclaimer in the
00017         documentation and/or other materials provided with the distribution.
00018       * Neither the name of KTH nor the
00019         names of its contributors may be used to endorse or promote products
00020         derived from this software without specific prior written permission.
00021 
00022    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00023    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00024    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00025    DISCLAIMED. IN NO EVENT SHALL KTH BE LIABLE FOR ANY
00026    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00027    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00029    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00030    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00031    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00032 */
00033 
00034 #include <kdl_wrapper/kdl_wrapper.h>
00035 #include <urdf/model.h>
00036 #include <kdl_parser/kdl_parser.hpp>
00037 
00038 KDLWrapper::KDLWrapper()
00039 {
00040     n = ros::NodeHandle("~");
00041 
00042     ik_solver_vel = NULL;
00043     jnt_jac_solver = NULL;
00044     fk_solver_pos = NULL;
00045     fk_solver_vel = NULL;
00046     fk_solver_acc = NULL;
00047 
00048     m_initialized = false;
00049 }
00050 
00051 KDLWrapper::~KDLWrapper()
00052 {
00053     m_initialized = false;
00054     delete ik_solver_vel;
00055     delete jnt_jac_solver;
00056     delete fk_solver_pos;
00057     delete fk_solver_vel;
00058     delete fk_solver_acc;
00059 }
00060 
00061 bool KDLWrapper::init(const std::string &chain_root, const std::string &chain_tip)
00062 {
00063     if(m_initialized)
00064     {
00065         ROS_ERROR("Already initialized");
00066         return false;
00067     }
00068 
00069     KDL::Tree tree;
00070     if(!getTreeFromURDF(tree))
00071     {
00072         return false;
00073     }
00074 
00075     if(!tree.getChain(chain_root, chain_tip, m_chain))
00076     {
00077         ROS_ERROR("Error getting KDL chain");
00078         return false;
00079     }
00080     ROS_DEBUG("Number of segments: %d", m_chain.getNrOfSegments());
00081     ROS_DEBUG("Number of joints in chain: %d", m_chain.getNrOfJoints());
00082 
00083     //  m_InvVelSolver.reset( new KDL::ChainIkSolverVel_pinv(m_ArmChain, 0.0001, 300));
00084     ik_solver_vel = new KDL::ChainIkSolverVel_wdls(m_chain, 0.01);
00085     ik_solver_vel->setLambda(0.3);
00086 
00087     jnt_jac_solver = new KDL::ChainJntToJacSolver(m_chain);
00088     fk_solver_pos = new KDL::ChainFkSolverPos_recursive(m_chain);
00089     fk_solver_vel = new KDL::ChainFkSolverVel_recursive(m_chain);
00090     fk_solver_acc = new KDL::ChainFkSolverAcc_recursive(m_chain);
00091 
00092     ROS_DEBUG("Successfully initialized KDL chain");
00093 
00094     m_initialized = true;
00095     return true;
00096 }
00097 
00098 bool KDLWrapper::isInitialized()
00099 {
00100     return m_initialized;
00101 }
00102 
00103 KDL::Chain KDLWrapper::getKDLChain()
00104 {
00105     return m_chain;
00106 }
00107 
00108 bool KDLWrapper::getTreeFromURDF(KDL::Tree &tree)
00109 {
00110 
00112     std::string param_name = "robot_description";
00113     std::string full_param_name;
00114     std::string xml_string;
00115 
00116     n.searchParam(param_name, full_param_name);
00117     if (n.hasParam(full_param_name))
00118     {
00119         n.getParam(full_param_name.c_str(), xml_string);
00120     }
00121 
00122     else
00123     {
00124         ROS_ERROR("Parameter %s not set, shutting down node...", full_param_name.c_str());
00125         n.shutdown();
00126         return false;
00127     }
00128 
00129     if (xml_string.size() == 0)
00130     {
00131         ROS_ERROR("Unable to load robot model from parameter %s",full_param_name.c_str());
00132         n.shutdown();
00133         return false;
00134     }
00135     ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
00136 
00138     urdf::Model model;
00139     if (!model.initString(xml_string))
00140     {
00141         ROS_ERROR("Failed to parse urdf file");
00142         n.shutdown();
00143         return false;
00144     }
00145     ROS_DEBUG("Successfully parsed urdf file");
00146 
00147     if (!kdl_parser::treeFromUrdfModel(model, tree)){
00148         ROS_ERROR("Failed to construct kdl tree");
00149         n.shutdown();
00150         return false;
00151     }
00152 
00153     return true;
00154 
00155 }


kdl_wrapper
Author(s): Francisco Vina
autogenerated on Mon Oct 6 2014 01:24:24