chainiksolvervel_pinv.cpp
Go to the documentation of this file.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 #include "chainiksolvervel_pinv.hpp" 00023 00024 namespace KDL 00025 { 00026 ChainIkSolverVel_pinv::ChainIkSolverVel_pinv(const Chain& _chain,double _eps,int _maxiter): 00027 chain(_chain), 00028 jnt2jac(chain), 00029 jac(chain.getNrOfJoints()), 00030 svd(jac), 00031 U(6,JntArray(chain.getNrOfJoints())), 00032 S(chain.getNrOfJoints()), 00033 V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())), 00034 tmp(chain.getNrOfJoints()), 00035 eps(_eps), 00036 maxiter(_maxiter) 00037 { 00038 } 00039 00040 ChainIkSolverVel_pinv::~ChainIkSolverVel_pinv() 00041 { 00042 } 00043 00044 00045 int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) 00046 { 00047 //Let the ChainJntToJacSolver calculate the jacobian "jac" for 00048 //the current joint positions "q_in" 00049 jnt2jac.JntToJac(q_in,jac); 00050 00051 //Do a singular value decomposition of "jac" with maximum 00052 //iterations "maxiter", put the results in "U", "S" and "V" 00053 //jac = U*S*Vt 00054 int ret = svd.calculate(jac,U,S,V,maxiter); 00055 00056 double sum; 00057 unsigned int i,j; 00058 00059 // We have to calculate qdot_out = jac_pinv*v_in 00060 // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut): 00061 // qdot_out = V*S_pinv*Ut*v_in 00062 00063 //first we calculate Ut*v_in 00064 for (i=0;i<jac.columns();i++) { 00065 sum = 0.0; 00066 for (j=0;j<jac.rows();j++) { 00067 sum+= U[j](i)*v_in(j); 00068 } 00069 //If the singular value is too small (<eps), don't invert it but 00070 //set the inverted singular value to zero (truncated svd) 00071 tmp(i) = sum*(fabs(S(i))<eps?0.0:1.0/S(i)); 00072 } 00073 //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply 00074 //it with V to get qdot_out 00075 for (i=0;i<jac.columns();i++) { 00076 sum = 0.0; 00077 for (j=0;j<jac.columns();j++) { 00078 sum+=V[i](j)*tmp(j); 00079 } 00080 //Put the result in qdot_out 00081 qdot_out(i)=sum; 00082 } 00083 //return the return value of the svd decomposition 00084 return ret; 00085 } 00086 00087 }