00001
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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();
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
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 };