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         lastNrOfIter(0),
00058         lastSV(_chain.getNrOfJoints()),
00059         jac(6, _chain.getNrOfJoints()),
00060         grad(_chain.getNrOfJoints()),
00061         display_information(false),
00062         maxiter(_maxiter),
00063         eps(_eps),
00064         eps_joints(_eps_joints),
00065         L(_L.cast<ScalarType>()),
00066         chain(_chain),
00067         T_base_jointroot(_chain.getNrOfJoints()),
00068         T_base_jointtip(_chain.getNrOfJoints()),
00069         q(_chain.getNrOfJoints()),
00070         A(_chain.getNrOfJoints(), _chain.getNrOfJoints()),
00071         tmp(_chain.getNrOfJoints()),
00072         ldlt(_chain.getNrOfJoints()),
00073         svd(6, _chain.getNrOfJoints(),Eigen::ComputeThinU | Eigen::ComputeThinV),
00074         diffq(_chain.getNrOfJoints()),
00075         q_new(_chain.getNrOfJoints()),
00076         original_Aii(_chain.getNrOfJoints())
00077 {}
00078 
00079 ChainIkSolverPos_LMA::ChainIkSolverPos_LMA(
00080                 const KDL::Chain& _chain,
00081                 double _eps,
00082                 int _maxiter,
00083                 double _eps_joints
00084 ) :
00085         lastNrOfIter(0),
00086         lastSV(_chain.getNrOfJoints()>6?6:_chain.getNrOfJoints()),
00087         jac(6, _chain.getNrOfJoints()),
00088         grad(_chain.getNrOfJoints()),
00089         display_information(false),
00090         maxiter(_maxiter),
00091         eps(_eps),
00092         eps_joints(_eps_joints),
00093         chain(_chain),
00094         T_base_jointroot(_chain.getNrOfJoints()),
00095         T_base_jointtip(_chain.getNrOfJoints()),
00096         q(_chain.getNrOfJoints()),
00097         A(_chain.getNrOfJoints(), _chain.getNrOfJoints()),
00098         ldlt(_chain.getNrOfJoints()),
00099         svd(6, _chain.getNrOfJoints(),Eigen::ComputeThinU | Eigen::ComputeThinV),
00100         diffq(_chain.getNrOfJoints()),
00101         q_new(_chain.getNrOfJoints()),
00102         original_Aii(_chain.getNrOfJoints())
00103 {
00104         L(0)=1;
00105         L(1)=1;
00106         L(2)=1;
00107         L(3)=0.01;
00108         L(4)=0.01;
00109         L(5)=0.01;
00110 }
00111 
00112 ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA() {}
00113 
00114 void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) {
00115         using namespace KDL;
00116         unsigned int jointndx=0;
00117         T_base_head = Frame::Identity(); // frame w.r.t. base of head
00118         for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
00119                 const Segment& segment = chain.getSegment(i);
00120                 if (segment.getJoint().getType()!=Joint::None) {
00121                         T_base_jointroot[jointndx] = T_base_head;
00122                         T_base_head = T_base_head * segment.pose(q(jointndx));
00123                         T_base_jointtip[jointndx] = T_base_head;
00124                         jointndx++;
00125                 } else {
00126                         T_base_head = T_base_head * segment.pose(0.0);
00127                 }
00128         }
00129 }
00130 
00131 void ChainIkSolverPos_LMA::compute_jacobian(const VectorXq& q) {
00132         using namespace KDL;
00133         unsigned int jointndx=0;
00134         for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
00135                 const Segment& segment = chain.getSegment(i);
00136                 if (segment.getJoint().getType()!=Joint::None) {
00137                         // compute twist of the end effector motion caused by joint [jointndx]; expressed in base frame, with vel. ref. point equal to the end effector
00138                         KDL::Twist t = ( T_base_jointroot[jointndx].M * segment.twist(q(jointndx),1.0) ).RefPoint( T_base_head.p - T_base_jointtip[jointndx].p);
00139                         jac(0,jointndx)=t[0];
00140                         jac(1,jointndx)=t[1];
00141                         jac(2,jointndx)=t[2];
00142                         jac(3,jointndx)=t[3];
00143                         jac(4,jointndx)=t[4];
00144                         jac(5,jointndx)=t[5];
00145                         jointndx++;
00146                 }
00147         }
00148 }
00149 
00150 void ChainIkSolverPos_LMA::display_jac(const KDL::JntArray& jval) {
00151         VectorXq q;
00152         q = jval.data.cast<ScalarType>();
00153         compute_fwdpos(q);
00154         compute_jacobian(q);
00155         svd.compute(jac);
00156         std::cout << "Singular values : " << svd.singularValues().transpose()<<"\n";
00157 }
00158 
00159 
00160 int ChainIkSolverPos_LMA::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out) {
00161         using namespace KDL;
00162         double v      = 2;
00163         double tau    = 10;
00164         double rho;
00165         double lambda;
00166         Twist t;
00167         double delta_pos_norm;
00168         Eigen::Matrix<ScalarType,6,1> delta_pos;
00169         Eigen::Matrix<ScalarType,6,1> delta_pos_new;
00170 
00171 
00172         q=q_init.data.cast<ScalarType>();
00173         compute_fwdpos(q);
00174         Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00175         delta_pos=L.asDiagonal()*delta_pos;
00176         delta_pos_norm = delta_pos.norm();
00177         if (delta_pos_norm<eps) {
00178                 lastNrOfIter    =0 ;
00179                 Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00180                 lastDifference  = delta_pos.norm();
00181                 lastTransDiff   = delta_pos.topRows(3).norm();
00182                 lastRotDiff     = delta_pos.bottomRows(3).norm();
00183                 svd.compute(jac);
00184                 original_Aii    = svd.singularValues();
00185                 lastSV          = svd.singularValues();
00186                 q_out.data      = q.cast<double>();
00187                 return 0;
00188         }
00189         compute_jacobian(q);
00190         jac = L.asDiagonal()*jac;
00191 
00192         lambda = tau;
00193         double dnorm = 1;
00194         for (unsigned int i=0;i<maxiter;++i) {
00195 
00196                 svd.compute(jac);
00197                 original_Aii = svd.singularValues();
00198                 for (unsigned int j=0;j<original_Aii.rows();++j) {
00199                         original_Aii(j) = original_Aii(j)/( original_Aii(j)*original_Aii(j)+lambda);
00200 
00201                 }
00202                 tmp = svd.matrixU().transpose()*delta_pos;
00203                 tmp = original_Aii.cwiseProduct(tmp);
00204                 diffq = svd.matrixV()*tmp;
00205                 grad = jac.transpose()*delta_pos;
00206                 if (display_information) {
00207                         std::cout << "------- iteration " << i << " ----------------\n"
00208                                           << "  q              = " << q.transpose() << "\n"
00209                                           << "  weighted jac   = \n" << jac << "\n"
00210                                           << "  lambda         = " << lambda << "\n"
00211                                           << "  eigenvalues    = " << svd.singularValues().transpose() << "\n"
00212                                           << "  difference     = "   << delta_pos.transpose() << "\n"
00213                                           << "  difference norm= "   << delta_pos_norm << "\n"
00214                                           << "  proj. on grad. = "   << grad << "\n";
00215                         std::cout << std::endl;
00216                 }
00217                 dnorm = diffq.lpNorm<Eigen::Infinity>();
00218                 if (dnorm < eps_joints) {
00219                                 lastDifference = delta_pos_norm;
00220                                 lastNrOfIter   = i;
00221                                 lastSV         = svd.singularValues();
00222                                 q_out.data     = q.cast<double>();
00223                                 compute_fwdpos(q);
00224                                 Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00225                                 lastTransDiff  = delta_pos.topRows(3).norm();
00226                                 lastRotDiff    = delta_pos.bottomRows(3).norm();
00227                                 return -2;
00228                 }
00229 
00230 
00231                 if (grad.transpose()*grad < eps_joints*eps_joints ) {
00232                         compute_fwdpos(q);
00233                         Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00234                         lastDifference = delta_pos_norm;
00235                         lastTransDiff = delta_pos.topRows(3).norm();
00236                         lastRotDiff   = delta_pos.bottomRows(3).norm();
00237                         lastSV        = svd.singularValues();
00238                         lastNrOfIter  = i;
00239                         q_out.data    = q.cast<double>();
00240                         return -1;
00241                 }
00242 
00243                 q_new = q+diffq;
00244                 compute_fwdpos(q_new);
00245                 Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos_new );
00246                 delta_pos_new             = L.asDiagonal()*delta_pos_new;
00247                 double delta_pos_new_norm = delta_pos_new.norm();
00248                 rho                       = delta_pos_norm*delta_pos_norm - delta_pos_new_norm*delta_pos_new_norm;
00249                 rho                      /= diffq.transpose()*(lambda*diffq + grad);
00250                 if (rho > 0) {
00251                         q               = q_new;
00252                         delta_pos       = delta_pos_new;
00253                         delta_pos_norm  = delta_pos_new_norm;
00254                         if (delta_pos_norm<eps) {
00255                                 Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
00256                                 lastDifference = delta_pos_norm;
00257                                 lastTransDiff  = delta_pos.topRows(3).norm();
00258                                 lastRotDiff    = delta_pos.bottomRows(3).norm();
00259                                 lastSV         = svd.singularValues();
00260                                 lastNrOfIter   = i;
00261                                 q_out.data     = q.cast<double>();
00262                                 return 0;
00263                         }
00264                         compute_jacobian(q_new);
00265                         jac = L.asDiagonal()*jac;
00266                         double tmp=2*rho-1;
00267                         lambda = lambda*max(1/3.0, 1-tmp*tmp*tmp);
00268                         v = 2;
00269                 } else {
00270                         lambda = lambda*v;
00271                         v      = 2*v;
00272                 }
00273         }
00274         lastDifference = delta_pos_norm;
00275         lastTransDiff  = delta_pos.topRows(3).norm();
00276         lastRotDiff    = delta_pos.bottomRows(3).norm();
00277         lastSV         = svd.singularValues();
00278         lastNrOfIter   = maxiter;
00279         q_out.data     = q.cast<double>();
00280         return -3;
00281 
00282 }
00283 
00284 
00285 
00286 };//namespace KDL


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