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: 00044 static const int E_CONVERGE_PINV_SINGULAR = +100; 00045 00057 explicit ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150); 00058 ~ChainIkSolverVel_pinv(); 00059 00075 virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); 00080 virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;}; 00081 00087 unsigned int getNrZeroSigmas()const {return nrZeroSigmas;}; 00088 00093 int getSVDResult()const {return svdResult;}; 00094 00096 virtual const char* strError(const int error) const; 00097 00099 virtual void updateInternalDataStructures(); 00100 private: 00101 const Chain& chain; 00102 ChainJntToJacSolver jnt2jac; 00103 unsigned int nj; 00104 Jacobian jac; 00105 SVD_HH svd; 00106 std::vector<JntArray> U; 00107 JntArray S; 00108 std::vector<JntArray> V; 00109 JntArray tmp; 00110 double eps; 00111 int maxiter; 00112 unsigned int nrZeroSigmas; 00113 int svdResult; 00114 00115 }; 00116 } 00117 #endif 00118