00001 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00002 00003 // Version: 1.0 00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00006 // URL: http://www.orocos.org/kdl 00007 00008 // This library is free software; you can redistribute it and/or 00009 // modify it under the terms of the GNU Lesser General Public 00010 // License as published by the Free Software Foundation; either 00011 // version 2.1 of the License, or (at your option) any later version. 00012 00013 // This library is distributed in the hope that it will be useful, 00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00016 // Lesser General Public License for more details. 00017 00018 // You should have received a copy of the GNU Lesser General Public 00019 // License along with this library; if not, write to the Free Software 00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 00021 00022 #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_HPP 00023 #define KDL_CHAIN_IKSOLVERVEL_PINV_HPP 00024 00025 #include "chainiksolver.hpp" 00026 #include "chainjnttojacsolver.hpp" 00027 #include "utilities/svd_HH.hpp" 00028 00029 namespace KDL 00030 { 00040 class ChainIkSolverVel_pinv : public ChainIkSolverVel 00041 { 00042 public: 00043 static const int E_SVD_FAILED = -100; 00044 00045 static const int E_CONVERGE_PINV_SINGULAR = +100; 00046 00058 explicit ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150); 00059 ~ChainIkSolverVel_pinv(); 00060 00076 virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); 00081 virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;}; 00082 00088 unsigned int getNrZeroSigmas()const {return nrZeroSigmas;}; 00089 00094 int getSVDResult()const {return svdResult;}; 00095 00097 virtual const char* strError(const int error) const; 00098 00099 private: 00100 const Chain chain; 00101 ChainJntToJacSolver jnt2jac; 00102 Jacobian jac; 00103 SVD_HH svd; 00104 std::vector<JntArray> U; 00105 JntArray S; 00106 std::vector<JntArray> V; 00107 JntArray tmp; 00108 double eps; 00109 int maxiter; 00110 unsigned int nrZeroSigmas; 00111 int svdResult; 00112 00113 }; 00114 } 00115 #endif 00116