chainiksolverpos_lma.cpp
Go to the documentation of this file.
00001 
00006 /**************************************************************************
00007     begin                : May 2012
00008     copyright            : (C) 2012 Erwin Aertbelien
00009     email                : firstname.lastname@mech.kuleuven.ac.be
00010 
00011  History (only major changes)( AUTHOR-Description ) :
00012 
00013  ***************************************************************************
00014  *   This library is free software; you can redistribute it and/or         *
00015  *   modify it under the terms of the GNU Lesser General Public            *
00016  *   License as published by the Free Software Foundation; either          *
00017  *   version 2.1 of the License, or (at your option) any later version.    *
00018  *                                                                         *
00019  *   This library is distributed in the hope that it will be useful,       *
00020  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00021  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00022  *   Lesser General Public License for more details.                       *
00023  *                                                                         *
00024  *   You should have received a copy of the GNU Lesser General Public      *
00025  *   License along with this library; if not, write to the Free Software   *
00026  *   Foundation, Inc., 59 Temple Place,                                    *
00027  *   Suite 330, Boston, MA  02111-1307  USA                                *
00028  *                                                                         *
00029  ***************************************************************************/
00030 
00031 #include "chainiksolverpos_lma.hpp"
00032 #include <iostream>
00033 
00034 namespace KDL {
00035 
00036 
00037 
00038 
00039 template <typename Derived>
00040 inline void Twist_to_Eigen(const KDL::Twist& t,Eigen::MatrixBase<Derived>& e) {
00041         e(0)=t.vel.data[0];
00042         e(1)=t.vel.data[1];
00043         e(2)=t.vel.data[2];
00044         e(3)=t.rot.data[0];
00045         e(4)=t.rot.data[1];
00046         e(5)=t.rot.data[2];
00047 }
00048 
00049 
00050 ChainIkSolverPos_LMA::ChainIkSolverPos_LMA(
00051                 const KDL::Chain& _chain,
00052                 const Eigen::Matrix<double,6,1>& _L,
00053                 double _eps,
00054                 int _maxiter,
00055                 double _eps_joints
00056 ) :
00057     chain(_chain),
00058         nj(chain.getNrOfJoints()),
00059         ns(chain.getNrOfSegments()),
00060         lastNrOfIter(0),
00061         lastDifference(0),
00062         lastTransDiff(0),
00063         lastRotDiff(0),
00064         lastSV(nj),
00065         jac(6, nj),
00066         grad(nj),
00067         display_information(false),
00068         maxiter(_maxiter),
00069         eps(_eps),
00070         eps_joints(_eps_joints),
00071         L(_L.cast<ScalarType>()),
00072         T_base_jointroot(nj),
00073         T_base_jointtip(nj),
00074         q(nj),
00075         A(nj, nj),
00076         tmp(nj),
00077         ldlt(nj),
00078         svd(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV),
00079         diffq(nj),
00080         q_new(nj),
00081         original_Aii(nj)
00082 {}
00083 
00084 ChainIkSolverPos_LMA::ChainIkSolverPos_LMA(
00085                 const KDL::Chain& _chain,
00086                 double _eps,
00087                 int _maxiter,
00088                 double _eps_joints
00089 ) :
00090     chain(_chain),
00091     nj(chain.getNrOfJoints()),
00092     ns(chain.getNrOfSegments()),
00093         lastNrOfIter(0),
00094         lastDifference(0),
00095     lastTransDiff(0),
00096         lastRotDiff(0),
00097         lastSV(nj>6?6:nj),
00098         jac(6, nj),
00099         grad(nj),
00100         display_information(false),
00101         maxiter(_maxiter),
00102         eps(_eps),
00103         eps_joints(_eps_joints),
00104         T_base_jointroot(nj),
00105         T_base_jointtip(nj),
00106         q(nj),
00107         A(nj, nj),
00108         ldlt(nj),
00109         svd(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV),
00110         diffq(nj),
00111         q_new(nj),
00112         original_Aii(nj)
00113 {
00114         L(0)=1;
00115         L(1)=1;
00116         L(2)=1;
00117         L(3)=0.01;
00118         L(4)=0.01;
00119         L(5)=0.01;
00120 }
00121 
00122 void ChainIkSolverPos_LMA::updateInternalDataStructures() {
00123     nj = chain.getNrOfJoints();
00124     ns = chain.getNrOfSegments();
00125     lastSV.conservativeResize(nj>6?6:nj);
00126     jac.conservativeResize(Eigen::NoChange, nj);
00127     grad.conservativeResize(nj);
00128     T_base_jointroot.resize(nj);
00129     T_base_jointtip.resize(nj);
00130     q.conservativeResize(nj);
00131     A.conservativeResize(nj, nj);
00132     ldlt = Eigen::LDLT<MatrixXq>(nj);
00133     svd = Eigen::JacobiSVD<MatrixXq>(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV);
00134     diffq.conservativeResize(nj);
00135     q_new.conservativeResize(nj);
00136     original_Aii.conservativeResize(nj);
00137 }
00138 
00139 ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA() {}
00140 
00141 void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) {
00142         using namespace KDL;
00143         unsigned int jointndx=0;
00144         T_base_head = Frame::Identity(); // frame w.r.t. base of head
00145         for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
00146                 const Segment& segment = chain.getSegment(i);
00147                 if (segment.getJoint().getType()!=Joint::None) {
00148                         T_base_jointroot[jointndx] = T_base_head;
00149                         T_base_head = T_base_head * segment.pose(q(jointndx));
00150                         T_base_jointtip[jointndx] = T_base_head;
00151                         jointndx++;
00152                 } else {
00153                         T_base_head = T_base_head * segment.pose(0.0);
00154                 }
00155         }
00156 }
00157 
00158 void ChainIkSolverPos_LMA::compute_jacobian(const VectorXq& q) {
00159         using namespace KDL;
00160         unsigned int jointndx=0;
00161         for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
00162                 const Segment& segment = chain.getSegment(i);
00163                 if (segment.getJoint().getType()!=Joint::None) {
00164                         // compute twist of the end effector motion caused by joint [jointndx]; expressed in base frame, with vel. ref. point equal to the end effector
00165                         KDL::Twist t = ( T_base_jointroot[jointndx].M * segment.twist(q(jointndx),1.0) ).RefPoint( T_base_head.p - T_base_jointtip[jointndx].p);
00166                         jac(0,jointndx)=t[0];
00167                         jac(1,jointndx)=t[1];
00168                         jac(2,jointndx)=t[2];
00169                         jac(3,jointndx)=t[3];
00170                         jac(4,jointndx)=t[4];
00171                         jac(5,jointndx)=t[5];
00172                         jointndx++;
00173                 }
00174         }
00175 }
00176 
00177 void ChainIkSolverPos_LMA::display_jac(const KDL::JntArray& jval) {
00178         VectorXq q;
00179         q = jval.data.cast<ScalarType>();
00180         compute_fwdpos(q);
00181         compute_jacobian(q);
00182         svd.compute(jac);
00183         std::cout << "Singular values : " << svd.singularValues().transpose()<<"\n";
00184 }
00185 
00186 
00187 int ChainIkSolverPos_LMA::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out) {
00188   if (nj != chain.getNrOfJoints())
00189     return (error = E_NOT_UP_TO_DATE);
00190   
00191   if (nj != q_init.rows() || nj != q_out.rows())
00192     return (error = E_SIZE_MISMATCH);
00193 
00194         using namespace KDL;
00195         double v      = 2;
00196         double tau    = 10;
00197         double rho;
00198         double lambda;
00199         Twist t;
00200         double delta_pos_norm;
00201         Eigen::Matrix<ScalarType,6,1> delta_pos;
00202         Eigen::Matrix<ScalarType,6,1> delta_pos_new;
00203 
00204 
00205         q=q_init.data.cast<ScalarType>();
00206         compute_fwdpos(q);
00207         Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00208         delta_pos=L.asDiagonal()*delta_pos;
00209         delta_pos_norm = delta_pos.norm();
00210         if (delta_pos_norm<eps) {
00211                 lastNrOfIter    =0 ;
00212                 Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00213                 lastDifference  = delta_pos.norm();
00214                 lastTransDiff   = delta_pos.topRows(3).norm();
00215                 lastRotDiff     = delta_pos.bottomRows(3).norm();
00216                 svd.compute(jac);
00217                 original_Aii    = svd.singularValues();
00218                 lastSV          = svd.singularValues();
00219                 q_out.data      = q.cast<double>();
00220                 return (error = E_NOERROR);
00221         }
00222         compute_jacobian(q);
00223         jac = L.asDiagonal()*jac;
00224 
00225         lambda = tau;
00226         double dnorm = 1;
00227         for (unsigned int i=0;i<maxiter;++i) {
00228 
00229                 svd.compute(jac);
00230                 original_Aii = svd.singularValues();
00231                 for (unsigned int j=0;j<original_Aii.rows();++j) {
00232                         original_Aii(j) = original_Aii(j)/( original_Aii(j)*original_Aii(j)+lambda);
00233 
00234                 }
00235                 tmp = svd.matrixU().transpose()*delta_pos;
00236                 tmp = original_Aii.cwiseProduct(tmp);
00237                 diffq = svd.matrixV()*tmp;
00238                 grad = jac.transpose()*delta_pos;
00239                 if (display_information) {
00240                         std::cout << "------- iteration " << i << " ----------------\n"
00241                                           << "  q              = " << q.transpose() << "\n"
00242                                           << "  weighted jac   = \n" << jac << "\n"
00243                                           << "  lambda         = " << lambda << "\n"
00244                                           << "  eigenvalues    = " << svd.singularValues().transpose() << "\n"
00245                                           << "  difference     = "   << delta_pos.transpose() << "\n"
00246                                           << "  difference norm= "   << delta_pos_norm << "\n"
00247                                           << "  proj. on grad. = "   << grad << "\n";
00248                         std::cout << std::endl;
00249                 }
00250                 dnorm = diffq.lpNorm<Eigen::Infinity>();
00251                 if (dnorm < eps_joints) {
00252                                 lastDifference = delta_pos_norm;
00253                                 lastNrOfIter   = i;
00254                                 lastSV         = svd.singularValues();
00255                                 q_out.data     = q.cast<double>();
00256                                 compute_fwdpos(q);
00257                                 Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00258                                 lastTransDiff  = delta_pos.topRows(3).norm();
00259                                 lastRotDiff    = delta_pos.bottomRows(3).norm();
00260                                 return (error = E_INCREMENT_JOINTS_TOO_SMALL);
00261                 }
00262 
00263 
00264                 if (grad.transpose()*grad < eps_joints*eps_joints ) {
00265                         compute_fwdpos(q);
00266                         Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00267                         lastDifference = delta_pos_norm;
00268                         lastTransDiff = delta_pos.topRows(3).norm();
00269                         lastRotDiff   = delta_pos.bottomRows(3).norm();
00270                         lastSV        = svd.singularValues();
00271                         lastNrOfIter  = i;
00272                         q_out.data    = q.cast<double>();
00273                         return (error = E_GRADIENT_JOINTS_TOO_SMALL);
00274                 }
00275 
00276                 q_new = q+diffq;
00277                 compute_fwdpos(q_new);
00278                 Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos_new );
00279                 delta_pos_new             = L.asDiagonal()*delta_pos_new;
00280                 double delta_pos_new_norm = delta_pos_new.norm();
00281                 rho                       = delta_pos_norm*delta_pos_norm - delta_pos_new_norm*delta_pos_new_norm;
00282                 rho                      /= diffq.transpose()*(lambda*diffq + grad);
00283                 if (rho > 0) {
00284                         q               = q_new;
00285                         delta_pos       = delta_pos_new;
00286                         delta_pos_norm  = delta_pos_new_norm;
00287                         if (delta_pos_norm<eps) {
00288                                 Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00289                                 lastDifference = delta_pos_norm;
00290                                 lastTransDiff  = delta_pos.topRows(3).norm();
00291                                 lastRotDiff    = delta_pos.bottomRows(3).norm();
00292                                 lastSV         = svd.singularValues();
00293                                 lastNrOfIter   = i;
00294                                 q_out.data     = q.cast<double>();
00295                                 return (error = E_NOERROR);
00296                         }
00297                         compute_jacobian(q_new);
00298                         jac = L.asDiagonal()*jac;
00299                         double tmp=2*rho-1;
00300                         lambda = lambda*max(1/3.0, 1-tmp*tmp*tmp);
00301                         v = 2;
00302                 } else {
00303                         lambda = lambda*v;
00304                         v      = 2*v;
00305                 }
00306         }
00307         lastDifference = delta_pos_norm;
00308         lastTransDiff  = delta_pos.topRows(3).norm();
00309         lastRotDiff    = delta_pos.bottomRows(3).norm();
00310         lastSV         = svd.singularValues();
00311         lastNrOfIter   = maxiter;
00312         q_out.data     = q.cast<double>();
00313         return (error = E_MAX_ITERATIONS_EXCEEDED);
00314 
00315 }
00316 
00317 const char* ChainIkSolverPos_LMA::strError(const int error) const
00318     {
00319         if (E_GRADIENT_JOINTS_TOO_SMALL == error) return "The gradient of E towards the joints is to small";
00320         else if (E_INCREMENT_JOINTS_TOO_SMALL == error) return "The joint position increments are to small";
00321         else return SolverI::strError(error);
00322     }
00323 
00324 };//namespace KDL


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