chainiksolvervel_pinv_givens.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_givens.hpp"
00023 #include "utilities/svd_eigen_Macie.hpp"
00024 
00025 namespace KDL
00026 {
00027     ChainIkSolverVel_pinv_givens::ChainIkSolverVel_pinv_givens(const Chain& _chain):
00028         chain(_chain),
00029         jnt2jac(chain),
00030         jac(chain.getNrOfJoints()),
00031         transpose(chain.getNrOfJoints()>6),toggle(true),
00032         m(max(6,chain.getNrOfJoints())),
00033         n(min(6,chain.getNrOfJoints())),
00034         jac_eigen(m,n),
00035         U(MatrixXd::Identity(m,m)),
00036         V(MatrixXd::Identity(n,n)),
00037         B(m,n),
00038         S(n),
00039         tempi(m),
00040         tempj(m),
00041         UY(VectorXd::Zero(6)),
00042         SUY(VectorXd::Zero(chain.getNrOfJoints())),
00043         qdot_eigen(chain.getNrOfJoints()),
00044         v_in_eigen(6)
00045     {
00046     }
00047 
00048     ChainIkSolverVel_pinv_givens::~ChainIkSolverVel_pinv_givens()
00049     {
00050     }
00051 
00052 
00053     int ChainIkSolverVel_pinv_givens::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00054     {
00055         toggle=!toggle;
00056 
00057         jnt2jac.JntToJac(q_in,jac);
00058 
00059         for(unsigned int i=0;i<6;i++)
00060             v_in_eigen(i)=v_in(i);
00061 
00062         for(unsigned int i=0;i<m;i++){
00063             for(unsigned int j=0;j<n;j++)
00064                 if(transpose)
00065                     jac_eigen(i,j)=jac(j,i);
00066                 else
00067                     jac_eigen(i,j)=jac(i,j);
00068         }
00069         int ret = svd_eigen_Macie(jac_eigen,U,S,V,B,tempi,1e-15,toggle);
00070         //std::cout<<"# sweeps: "<<ret<<std::endl;
00071 
00072         if(transpose)
00073             UY.noalias() = V.transpose() * v_in_eigen;
00074         else
00075             UY.noalias() = U.transpose() * v_in_eigen;
00076 
00077         for (unsigned int i = 0; i < n; i++){
00078             double wi = UY(i);
00079             double alpha = S(i);
00080             
00081             if (alpha != 0)
00082                 alpha = 1.0 / alpha;
00083             else
00084                 alpha = 0.0;
00085             SUY(i)= alpha * wi;
00086         }
00087         if(transpose)
00088             qdot_eigen.noalias() = U * SUY;
00089         else
00090             qdot_eigen.noalias() = V * SUY;
00091 
00092         for (unsigned int j=0;j<chain.getNrOfJoints();j++)
00093             qdot_out(j)=qdot_eigen(j);
00094 
00095         return ret;
00096 
00097     }
00098 
00099 }


orocos_kdl
Author(s):
autogenerated on Mon Oct 6 2014 03:11:16