chainiksolvervel_pinv_nso.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_nso.hpp"
00023 
00024 namespace KDL
00025 {
00026     ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, JntArray _opt_pos, JntArray _weights, double _eps, int _maxiter, int _alpha):
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         tmp2(chain.getNrOfJoints()-6),
00036         eps(_eps),
00037         maxiter(_maxiter),
00038         alpha(_alpha),
00039         opt_pos(_opt_pos),
00040         weights(_weights)
00041     {
00042     }
00043 
00044         ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, int _alpha):
00045         chain(_chain),
00046         jnt2jac(chain),
00047         jac(chain.getNrOfJoints()),
00048         svd(jac),
00049         U(6,JntArray(chain.getNrOfJoints())),
00050         S(chain.getNrOfJoints()),
00051         V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())),
00052         tmp(chain.getNrOfJoints()),
00053         tmp2(chain.getNrOfJoints()-6),
00054         eps(_eps),
00055         maxiter(_maxiter),
00056         alpha(_alpha)
00057     {
00058     }
00059 
00060     ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso()
00061     {
00062     }
00063 
00064 
00065     int ChainIkSolverVel_pinv_nso::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00066     {
00067         //Let the ChainJntToJacSolver calculate the jacobian "jac" for
00068         //the current joint positions "q_in" 
00069         jnt2jac.JntToJac(q_in,jac);
00070 
00071         //Do a singular value decomposition of "jac" with maximum
00072         //iterations "maxiter", put the results in "U", "S" and "V"
00073         //jac = U*S*Vt
00074         int ret = svd.calculate(jac,U,S,V,maxiter);
00075 
00076         double sum;
00077         unsigned int i,j;
00078 
00079         // We have to calculate qdot_out = jac_pinv*v_in
00080         // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
00081         // qdot_out = V*S_pinv*Ut*v_in
00082 
00083         //first we calculate Ut*v_in
00084         for (i=0;i<jac.columns();i++) {
00085             sum = 0.0;
00086             for (j=0;j<jac.rows();j++) {
00087                 sum+= U[j](i)*v_in(j);
00088             }
00089             //If the singular value is too small (<eps), don't invert it but
00090             //set the inverted singular value to zero (truncated svd)
00091             tmp(i) = sum*(fabs(S(i))<eps?0.0:1.0/S(i));
00092         }
00093         //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
00094         //it with V to get qdot_out
00095         for (i=0;i<jac.columns();i++) {
00096             sum = 0.0;
00097             for (j=0;j<jac.columns();j++) {
00098                 sum+=V[i](j)*tmp(j);
00099             }
00100             //Put the result in qdot_out
00101             qdot_out(i)=sum;
00102         }
00103         
00104         //Now onto NULL space
00105         
00106         for(i = 0; i < jac.columns(); i++)
00107             tmp(i) = weights(i)*(opt_pos(i) - q_in(i));
00108         
00109         //Vtn*tmp
00110         for (i=jac.rows()+1;i<jac.columns();i++) {
00111             tmp2(i-(jac.rows()+1)) = 0.0;
00112             for (j=0;j<jac.columns();j++) {
00113                 tmp2(i-(jac.rows()+1)) +=V[j](i)*tmp(j);
00114             }
00115         }
00116         
00117         for (i=0;i<jac.columns();i++) {
00118             sum = 0.0;
00119             for (j=jac.rows()+1;j<jac.columns();j++) {
00120                 sum +=V[i](j)*tmp2(j);
00121             }
00122             qdot_out(i) += alpha*sum;
00123         }
00124         
00125         //return the return value of the svd decomposition
00126         return ret;
00127     }
00128     
00129         int ChainIkSolverVel_pinv_nso::setWeights(const JntArray & _weights)
00130         {
00131                 weights = _weights;
00132                 return 0;
00133         }
00134         int ChainIkSolverVel_pinv_nso::setOptPos(const JntArray & _opt_pos)
00135         {
00136                 opt_pos = _opt_pos;
00137                 return 0;
00138         }
00139         int ChainIkSolverVel_pinv_nso::setAlpha(const int _alpha)
00140         {
00141                 alpha = _alpha;
00142                 return 0;
00143         }
00144 
00145 
00146 }


orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:25