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         nrZeroSigmas(0),
00038         svdResult(0)
00039     {
00040     }
00041 
00042     ChainIkSolverVel_pinv::~ChainIkSolverVel_pinv()
00043     {
00044     }
00045 
00046 
00047     int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00048     {
00049         //Let the ChainJntToJacSolver calculate the jacobian "jac" for
00050         //the current joint positions "q_in" 
00051         jnt2jac.JntToJac(q_in,jac);
00052 
00053         double sum;
00054         unsigned int i,j;
00055 
00056         // Initialize near zero singular value counter
00057         nrZeroSigmas = 0 ;
00058 
00059         //Do a singular value decomposition of "jac" with maximum
00060         //iterations "maxiter", put the results in "U", "S" and "V"
00061         //jac = U*S*Vt
00062         svdResult = svd.calculate(jac,U,S,V,maxiter);
00063         if (0 != svdResult)
00064         {
00065             qdot_out.data.setZero();
00066             return (error = E_SVD_FAILED);
00067         }
00068 
00069         // We have to calculate qdot_out = jac_pinv*v_in
00070         // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
00071         // qdot_out = V*S_pinv*Ut*v_in
00072 
00073         //first we calculate Ut*v_in
00074         for (i=0;i<jac.columns();i++) {
00075             sum = 0.0;
00076             for (j=0;j<jac.rows();j++) {
00077                 sum+= U[j](i)*v_in(j);
00078             }
00079             //If the singular value is too small (<eps), don't invert it but
00080             //set the inverted singular value to zero (truncated svd)
00081             if ( fabs(S(i))<eps ) {
00082                 tmp(i) = 0.0 ;
00083                 //  Count number of singular values near zero
00084                 ++nrZeroSigmas ;
00085             }
00086             else {
00087                 tmp(i) = sum/S(i) ;
00088             }
00089         }
00090         //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
00091         //it with V to get qdot_out
00092         for (i=0;i<jac.columns();i++) {
00093             sum = 0.0;
00094             for (j=0;j<jac.columns();j++) {
00095                 sum+=V[i](j)*tmp(j);
00096             }
00097             //Put the result in qdot_out
00098             qdot_out(i)=sum;
00099         }
00100 
00101         // Note if the solution is singular, i.e. if number of near zero
00102         // singular values is greater than the full rank of jac
00103         if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
00104             return (error = E_CONVERGE_PINV_SINGULAR);   // converged but pinv singular
00105         } else {
00106             return (error = E_NOERROR);                 // have converged
00107         }
00108     }
00109 
00110     const char* ChainIkSolverVel_pinv::strError(const int error) const
00111     {
00112         if (E_SVD_FAILED == error) return "SVD failed";
00113         else return SolverI::strError(error);
00114     }
00115 }


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14