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


orocos_kdl
Author(s):
autogenerated on Sat Oct 7 2017 03:04:28