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 #include "utilities/svd_eigen_HH.hpp"
00024 
00025 namespace KDL
00026 {
00027     ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, JntArray _opt_pos, JntArray _weights, double _eps, int _maxiter, double _alpha):
00028         chain(_chain),
00029         jnt2jac(chain),
00030         nj(chain.getNrOfJoints()),
00031         jac(nj),
00032         U(MatrixXd::Zero(6,nj)),
00033         S(VectorXd::Zero(nj)),
00034         Sinv(VectorXd::Zero(nj)),
00035         V(MatrixXd::Zero(nj,nj)),
00036         tmp(VectorXd::Zero(nj)),
00037         tmp2(VectorXd::Zero(nj)),
00038         eps(_eps),
00039         maxiter(_maxiter),
00040         alpha(_alpha),
00041         weights(_weights),
00042         opt_pos(_opt_pos)
00043     {
00044     }
00045 
00046     ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, double _alpha):
00047         chain(_chain),
00048         jnt2jac(chain),
00049         nj(chain.getNrOfJoints()),
00050         jac(nj),
00051         U(MatrixXd::Zero(6,nj)),
00052         S(VectorXd::Zero(nj)),
00053         Sinv(VectorXd::Zero(nj)),
00054         V(MatrixXd::Zero(nj,nj)),
00055         tmp(VectorXd::Zero(nj)),
00056         tmp2(VectorXd::Zero(nj)),
00057         eps(_eps),
00058         maxiter(_maxiter),
00059         alpha(_alpha)
00060     {
00061     }
00062 
00063     ChainIkSolverVel_pinv_nso::~ChainIkSolverVel_pinv_nso()
00064     {
00065     }
00066 
00067 
00068     int ChainIkSolverVel_pinv_nso::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00069     {
00070         //Let the ChainJntToJacSolver calculate the jacobian "jac" for
00071         //the current joint positions "q_in" 
00072         jnt2jac.JntToJac(q_in,jac);
00073 
00074         //Do a singular value decomposition of "jac" with maximum
00075         //iterations "maxiter", put the results in "U", "S" and "V"
00076         //jac = U*S*Vt
00077         int svdResult = svd_eigen_HH(jac.data,U,S,V,tmp,maxiter);
00078         if (0 != svdResult)
00079         {
00080             qdot_out.data.setZero() ;
00081             return svdResult;
00082         }
00083 
00084         unsigned int i;
00085 
00086         // We have to calculate qdot_out = jac_pinv*v_in
00087         // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
00088         // qdot_out = V*S_pinv*Ut*v_in
00089 
00090         // S^-1
00091         for (i = 0; i < nj; ++i) {
00092             Sinv(i) = fabs(S(i))<eps ? 0.0 : 1.0/S(i);
00093         }
00094         for (i = 0; i < 6; ++i) {
00095             tmp(i) = v_in(i);
00096         }
00097 
00098         qdot_out.data = V * Sinv.asDiagonal() * U.transpose() * tmp.head(6);
00099 
00100         // Now onto NULL space
00101         // Given the cost function g, and the current joints q, desired joints qd, and weights w:
00102         // t = g(q) = 1/2 * Sum( w_i * (q_i - qd_i)^2 )
00103         //
00104         // The jacobian Jc is:
00105         //  t_dot = Jc(q) * q_dot
00106         //  Jc = dt/dq = w_j * (q_i - qd_i) [1 x nj vector]
00107         //
00108         // The pseudo inverse (^-1) is
00109         // Jc^-1 = w_j * (q_i - qd_i) / A [nj x 1 vector]
00110         // A = Sum( w_i^2 * (q_i - qd_i)^2 )
00111         //
00112         // We can set the change as the step needed to remove the error times a value alpha:
00113         // t_dot = -2 * alpha * t
00114         //
00115         // When we put it together and project into the nullspace, the final result is
00116         // q'_out += (I_n - J^-1 * J) * Jc^-1 * (-2 * alpha * g(q))
00117 
00118         double g = 0; // g(q)
00119         double A = 0; // normalizing term
00120         for (i = 0; i < nj; ++i) {
00121             double qd = q_in(i) - opt_pos(i);
00122             g += 0.5 * qd*qd * weights(i);
00123             A += qd*qd * weights(i)*weights(i);
00124         }
00125 
00126         if (A > 1e-9) {
00127           // Calculate inverse Jacobian Jc^-1
00128           for (i = 0; i < nj; ++i) {
00129               tmp(i) = weights(i)*(q_in(i) - opt_pos(i)) / A;
00130           }
00131 
00132           // Calcualte J^-1 * J * Jc^-1 = V*S^-1*U' * U*S*V' * tmp
00133           tmp2 = V * Sinv.asDiagonal() * U.transpose() * U * S.asDiagonal() * V.transpose() * tmp;
00134 
00135           for (i = 0; i < nj; ++i) {
00136               //std::cerr << i <<": "<< qdot_out(i) <<", "<< -2*alpha*g * (tmp(i) - tmp2(i)) << std::endl;
00137               qdot_out(i) += -2*alpha*g * (tmp(i) - tmp2(i));
00138           }
00139         }
00140         //return the return value of the svd decomposition
00141         return svdResult;
00142     }
00143 
00144     int ChainIkSolverVel_pinv_nso::setWeights(const JntArray & _weights)
00145     {
00146       weights = _weights;
00147       return 0;
00148     }
00149 
00150     int ChainIkSolverVel_pinv_nso::setOptPos(const JntArray & _opt_pos)
00151     {
00152       opt_pos = _opt_pos;
00153       return 0;
00154     }
00155 
00156     int ChainIkSolverVel_pinv_nso::setAlpha(const double _alpha)
00157     {
00158       alpha = _alpha;
00159       return 0;
00160     }
00161 
00162 
00163 }


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