chainiksolvervel_wdls.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_wdls.hpp"
00023 #include "utilities/svd_eigen_HH.hpp"
00024 
00025 namespace KDL
00026 {
00027     
00028     ChainIkSolverVel_wdls::ChainIkSolverVel_wdls(const Chain& _chain,double _eps,int _maxiter):
00029         chain(_chain),
00030         jnt2jac(chain),
00031         jac(chain.getNrOfJoints()),
00032         U(MatrixXd::Zero(6,chain.getNrOfJoints())),
00033         S(VectorXd::Zero(chain.getNrOfJoints())),
00034         V(MatrixXd::Zero(chain.getNrOfJoints(),chain.getNrOfJoints())),
00035         eps(_eps),
00036         maxiter(_maxiter),
00037         tmp(VectorXd::Zero(chain.getNrOfJoints())),
00038         tmp_jac(MatrixXd::Zero(6,chain.getNrOfJoints())),
00039         tmp_jac_weight1(MatrixXd::Zero(6,chain.getNrOfJoints())),
00040         tmp_jac_weight2(MatrixXd::Zero(6,chain.getNrOfJoints())),
00041         tmp_ts(MatrixXd::Zero(6,6)),
00042         tmp_js(MatrixXd::Zero(chain.getNrOfJoints(),chain.getNrOfJoints())),
00043         weight_ts(MatrixXd::Identity(6,6)),
00044         weight_js(MatrixXd::Identity(chain.getNrOfJoints(),chain.getNrOfJoints())),
00045         lambda(0.0),
00046         lambda_scaled(0.0),
00047         nrZeroSigmas(0),
00048         svdResult(0),
00049         sigmaMin(0)
00050     {
00051     }
00052     
00053     ChainIkSolverVel_wdls::~ChainIkSolverVel_wdls()
00054     {
00055     }
00056     
00057     void ChainIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq){
00058         weight_js = Mq;
00059     }
00060     
00061     void ChainIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx){
00062         weight_ts = Mx;
00063     }
00064 
00065     void ChainIkSolverVel_wdls::setLambda(const double lambda_in)
00066     {
00067         lambda=lambda_in;
00068     }
00069 
00070     void ChainIkSolverVel_wdls::setEps(const double eps_in)
00071     {
00072         eps=eps_in;
00073     }
00074 
00075     void ChainIkSolverVel_wdls::setMaxIter(const int maxiter_in)
00076     {
00077         maxiter=maxiter_in;
00078     }
00079 
00080     int ChainIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00081     {
00082         jnt2jac.JntToJac(q_in,jac);
00083         
00084         double sum;
00085         unsigned int i,j;
00086         
00087         // Initialize (internal) return values
00088         nrZeroSigmas = 0 ;
00089         sigmaMin = 0.;
00090         lambda_scaled = 0.;
00091 
00092         /*
00093         for (i=0;i<jac.rows();i++) {
00094             for (j=0;j<jac.columns();j++)
00095                 tmp_jac(i,j) = jac(i,j);
00096         }
00097         */
00098         
00099         // Create the Weighted jacobian
00100         tmp_jac_weight1 = jac.data.lazyProduct(weight_js);
00101         tmp_jac_weight2 = weight_ts.lazyProduct(tmp_jac_weight1);
00102    
00103         // Compute the SVD of the weighted jacobian
00104         svdResult = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
00105         if (0 != svdResult)
00106         {
00107             qdot_out.data.setZero() ;
00108             return (error = E_SVD_FAILED);
00109         }
00110 
00111         //Pre-multiply U and V by the task space and joint space weighting matrix respectively
00112         tmp_ts = weight_ts.lazyProduct(U.topLeftCorner(6,6));
00113         tmp_js = weight_js.lazyProduct(V);
00114 
00115         // Minimum of six largest singular values of J is S(5) if number of joints >=6 and 0 for <6
00116         if ( jac.columns() >= 6 ) {
00117             sigmaMin = S(5);
00118         }
00119         else {
00120             sigmaMin = 0.;
00121         }
00122 
00123         // tmp = (Si*U'*Ly*y),
00124         for (i=0;i<jac.columns();i++) {
00125             sum = 0.0;
00126             for (j=0;j<jac.rows();j++) {
00127                 if(i<6)
00128                     sum+= tmp_ts(j,i)*v_in(j);
00129                 else
00130                     sum+=0.0;
00131             }
00132             // If sigmaMin > eps, then wdls is not active and lambda_scaled = 0 (default value)
00133             // If sigmaMin < eps, then wdls is active and lambda_scaled is scaled from 0 to lambda
00134             // Note:  singular values are always positive so sigmaMin >=0
00135             if ( sigmaMin < eps )
00136             {
00137                             lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ;
00138             }
00139                         if(fabs(S(i))<eps) {
00140                                 if (i<6) {
00141                                         // Scale lambda to size of singular value sigmaMin
00142                                         tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda_scaled*lambda_scaled)));
00143                                 }
00144                                 else {
00145                                         tmp(i)=0.0;  // S(i)=0 for i>=6 due to cols>rows
00146                                 }
00147                                 //  Count number of singular values near zero
00148                                 ++nrZeroSigmas ;
00149                         }
00150                         else {
00151                 tmp(i) = sum/S(i);
00152                         }
00153         }
00154 
00155         /*
00156         // x = Lx^-1*V*tmp + x
00157         for (i=0;i<jac.columns();i++) {
00158             sum = 0.0;
00159             for (j=0;j<jac.columns();j++) {
00160                 sum+=tmp_js(i,j)*tmp(j);
00161             }
00162             qdot_out(i)=sum;
00163         }
00164         */
00165         qdot_out.data=tmp_js.lazyProduct(tmp);
00166 
00167         // If number of near zero singular values is greater than the full rank
00168         // of jac, then wdls is active
00169         if ( nrZeroSigmas > (jac.columns()-jac.rows()) )        {
00170             return (error = E_CONVERGE_PINV_SINGULAR);  // converged but pinv singular
00171         } else {
00172             return (error = E_NOERROR);                 // have converged
00173         }
00174     }
00175 
00176     const char* ChainIkSolverVel_wdls::strError(const int error) const
00177     {
00178         if (E_SVD_FAILED == error) return "SVD failed";
00179         else return SolverI::strError(error);
00180     }
00181 
00182 }


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