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


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22