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


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22