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 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();
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
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 };
