00001 // Copyright (C) 2009, 2011 00002 00003 // Version: 1.0 00004 // Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov 00005 // Maintainer: Ruben Smits, Azamat Shakhimardanov 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 "chainidsolver_vereshchagin.hpp" 00023 #include "frames_io.hpp" 00024 #include "utilities/svd_eigen_HH.hpp" 00025 00026 00027 namespace KDL 00028 { 00029 using namespace Eigen; 00030 00031 ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin(const Chain& chain_, Twist root_acc, unsigned int _nc) : 00032 chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(_nc), 00033 results(ns + 1, segment_info(nc)) 00034 { 00035 acc_root = root_acc; 00036 00037 //Provide the necessary memory for computing the inverse of M0 00038 nu_sum.resize(nc); 00039 M_0_inverse.resize(nc, nc); 00040 Um = MatrixXd::Identity(nc, nc); 00041 Vm = MatrixXd::Identity(nc, nc); 00042 Sm = VectorXd::Ones(nc); 00043 tmpm = VectorXd::Ones(nc); 00044 } 00045 00046 int ChainIdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques) 00047 { 00048 //Check sizes always 00049 if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || f_ext.size() != ns) 00050 return -1; 00051 if (alfa.columns() != nc || beta.rows() != nc) 00052 return -2; 00053 //do an upward recursion for position and velocities 00054 this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext); 00055 //do an inward recursion for inertia, forces and constraints 00056 this->downwards_sweep(alfa, torques); 00057 //Solve for the constraint forces 00058 this->constraint_calculation(beta); 00059 //do an upward recursion to propagate the result 00060 this->final_upwards_sweep(q_dotdot, torques); 00061 return 0; 00062 } 00063 00064 void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext) 00065 { 00066 //if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns) 00067 // return -1; 00068 00069 unsigned int j = 0; 00070 F_total = Frame::Identity(); 00071 for (unsigned int i = 0; i < ns; i++) 00072 { 00073 //Express everything in the segments reference frame (body coordinates) 00074 //which is at the segments tip, i.e. where the next joint is attached. 00075 00076 //Calculate segment properties: X,S,vj,cj 00077 const Segment& segment = chain.getSegment(i); 00078 segment_info& s = results[i + 1]; 00079 //The pose between the joint root and the segment tip (tip expressed in joint root coordinates) 00080 s.F = segment.pose(q(j)); //X pose of each link in link coord system 00081 00082 F_total = F_total * s.F; //X pose of the each link in root coord system 00083 s.F_base = F_total; //X pose of the each link in root coord system for getter functions 00084 00085 //The velocity due to the joint motion of the segment expressed in the segments reference frame (tip) 00086 Twist vj = s.F.M.Inverse(segment.twist(q(j), qdot(j))); //XDot of each link 00087 //Twist aj = s.F.M.Inverse(segment.twist(q(j), qdotdot(j))); //XDotDot of each link 00088 00089 //The unit velocity due to the joint motion of the segment expressed in the segments reference frame (tip) 00090 s.Z = s.F.M.Inverse(segment.twist(q(j), 1.0)); 00091 //Put Z in the joint root reference frame: 00092 s.Z = s.F * s.Z; 00093 00094 //The total velocity of the segment expressed in the the segments reference frame (tip) 00095 if (i != 0) 00096 { 00097 s.v = s.F.Inverse(results[i].v) + vj; // recursive velocity of each link in segment frame 00098 //s.A=s.F.Inverse(results[i].A)+aj; 00099 s.A = s.F.M.Inverse(results[i].A); 00100 } 00101 else 00102 { 00103 s.v = vj; 00104 s.A = s.F.M.Inverse(acc_root); 00105 } 00106 //c[i] = cj + v[i]xvj (remark: cj=0, since our S is not time dependent in local coordinates) 00107 //The velocity product acceleration 00108 //std::cout << i << " Initial upward" << s.v << std::endl; 00109 s.C = s.v*vj; //This is a cross product: cartesian space BIAS acceleration in local link coord. 00110 //Put C in the joint root reference frame 00111 s.C = s.F * s.C; //+F_total.M.Inverse(acc_root)); 00112 //The rigid body inertia of the segment, expressed in the segments reference frame (tip) 00113 s.H = segment.getInertia(); 00114 00115 //wrench of the rigid body bias forces and the external forces on the segment (in body coordinates, tip) 00116 //external forces are taken into account through s.U. 00117 Wrench FextLocal = F_total.M.Inverse() * f_ext[i]; 00118 s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i]; 00119 if (segment.getJoint().getType() != Joint::None) 00120 j++; 00121 } 00122 00123 } 00124 00125 void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const JntArray &torques) 00126 { 00127 int j = nj - 1; 00128 for (int i = ns; i >= 0; i--) 00129 { 00130 //Get a handle for the segment we are working on. 00131 segment_info& s = results[i]; 00132 //For segment N, 00133 //tilde is in the segment refframe (tip, not joint root) 00134 //without tilde is at the joint root (the childs tip!!!) 00135 //P_tilde is the articulated body inertia 00136 //R_tilde is the sum of external and coriolis/centrifugal forces 00137 //M is the (unit) acceleration energy already generated at link i 00138 //G is the (unit) magnitude of the constraint forces at link i 00139 //E are the (unit) constraint forces due to the constraints 00140 if (i == (int)ns) 00141 { 00142 s.P_tilde = s.H; 00143 s.R_tilde = s.U; 00144 s.M.setZero(); 00145 s.G.setZero(); 00146 //changeBase(alfa_N,F_total.M.Inverse(),alfa_N2); 00147 for (unsigned int r = 0; r < 3; r++) 00148 for (unsigned int c = 0; c < nc; c++) 00149 { 00150 //copy alfa constrain force matrix in E~ 00151 s.E_tilde(r, c) = alfa(r + 3, c); 00152 s.E_tilde(r + 3, c) = alfa(r, c); 00153 } 00154 //Change the reference frame of alfa to the segmentN tip frame 00155 //F_Total holds end effector frame, if done per segment bases then constraints could be extended to all segments 00156 Rotation base_to_end = F_total.M.Inverse(); 00157 for (unsigned int c = 0; c < nc; c++) 00158 { 00159 Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), 00160 Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); 00161 col = base_to_end*col; 00162 s.E_tilde.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); 00163 } 00164 } 00165 else 00166 { 00167 //For all others: 00168 //Everything should expressed in the body coordinates of segment i 00169 segment_info& child = results[i + 1]; 00170 //Copy PZ into a vector so we can do matrix manipulations, put torques above forces 00171 Vector6d vPZ; 00172 vPZ << Vector3d::Map(child.PZ.torque.data), Vector3d::Map(child.PZ.force.data); 00173 Matrix6d PZDPZt; 00174 PZDPZt.noalias() = vPZ * vPZ.transpose(); 00175 PZDPZt /= child.D; 00176 00177 //equation a) (see Vereshchagin89) PZDPZt=[I,H;H',M] 00178 //Azamat:articulated body inertia as in Featherstone (7.19) 00179 s.P_tilde = s.H + child.P - ArticulatedBodyInertia(PZDPZt.bottomRightCorner<3,3>(), PZDPZt.topRightCorner<3,3>(), PZDPZt.topLeftCorner<3,3>()); 00180 //equation b) (see Vereshchagin89) 00181 //Azamat: bias force as in Featherstone (7.20) 00182 s.R_tilde = s.U + child.R + child.PC + (child.PZ / child.D) * child.u; 00183 //equation c) (see Vereshchagin89) 00184 s.E_tilde = child.E; 00185 00186 //Azamat: equation (c) right side term 00187 s.E_tilde.noalias() -= (vPZ * child.EZ.transpose()) / child.D; 00188 00189 //equation d) (see Vereshchagin89) 00190 s.M = child.M; 00191 //Azamat: equation (d) right side term 00192 s.M.noalias() -= (child.EZ * child.EZ.transpose()) / child.D; 00193 00194 //equation e) (see Vereshchagin89) 00195 s.G = child.G; 00196 Twist CiZDu = child.C + (child.Z / child.D) * child.u; 00197 Vector6d vCiZDu; 00198 vCiZDu << Vector3d::Map(CiZDu.rot.data), Vector3d::Map(CiZDu.vel.data); 00199 s.G.noalias() += child.E.transpose() * vCiZDu; 00200 } 00201 if (i != 0) 00202 { 00203 //Transform all results to joint root coordinates of segment i (== body coordinates segment i-1) 00204 //equation a) 00205 s.P = s.F * s.P_tilde; 00206 //equation b) 00207 s.R = s.F * s.R_tilde; 00208 //equation c), in matrix: torques above forces, so switch and switch back 00209 for (unsigned int c = 0; c < nc; c++) 00210 { 00211 Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), 00212 Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); 00213 col = s.F*col; 00214 s.E.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); 00215 } 00216 00217 //needed for next recursion 00218 s.PZ = s.P * s.Z; 00219 s.D = dot(s.Z, s.PZ); 00220 s.PC = s.P * s.C; 00221 00222 //u=(Q-Z(R+PC)=sum of external forces along the joint axes, 00223 //R are the forces comming from the children, 00224 //Q is taken zero (do we need to take the previous calculated torques? 00225 00226 //projection of coriolis and centrepital forces into joint subspace (0 0 Z) 00227 s.totalBias = -dot(s.Z, s.R + s.PC); 00228 s.u = torques(j) + s.totalBias; 00229 00230 //Matrix form of Z, put rotations above translations 00231 Vector6d vZ; 00232 vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data); 00233 s.EZ.noalias() = s.E.transpose() * vZ; 00234 00235 if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) 00236 j--; 00237 } 00238 } 00239 } 00240 00241 void ChainIdSolver_Vereshchagin::constraint_calculation(const JntArray& beta) 00242 { 00243 //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0) 00244 //M_0_inverse, always nc*nc symmetric matrix 00245 //std::cout<<"M0: "<<results[0].M<<std::endl; 00246 //results[0].M-=MatrixXd::Identity(nc,nc); 00247 //std::cout<<"augmented M0: "<<results[0].M<<std::endl; 00248 00249 00250 //ToDo: Need to check ill conditions 00251 00252 //M_0_inverse=results[0].M.inverse(); 00253 svd_eigen_HH(results[0].M, Um, Sm, Vm, tmpm); 00254 //truncated svd, what would sdls, dls physically mean? 00255 for (unsigned int i = 0; i < nc; i++) 00256 if (Sm(i) < 1e-14) 00257 Sm(i) = 0.0; 00258 else 00259 Sm(i) = 1 / Sm(i); 00260 00261 results[0].M.noalias() = Vm * Sm.asDiagonal(); 00262 M_0_inverse.noalias() = results[0].M * Um.transpose(); 00263 //results[0].M.ldlt().solve(MatrixXd::Identity(nc,nc),&M_0_inverse); 00264 //results[0].M.computeInverse(&M_0_inverse); 00265 Vector6d acc; 00266 acc << Vector3d::Map(acc_root.rot.data), Vector3d::Map(acc_root.vel.data); 00267 nu_sum.noalias() = -(results[0].E_tilde.transpose() * acc); 00268 //nu_sum.setZero(); 00269 nu_sum += beta.data; 00270 nu_sum -= results[0].G; 00271 00272 //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0) 00273 nu.noalias() = M_0_inverse * nu_sum; 00274 } 00275 00276 void ChainIdSolver_Vereshchagin::final_upwards_sweep(JntArray &q_dotdot, JntArray &torques) 00277 { 00278 unsigned int j = 0; 00279 00280 for (unsigned int i = 1; i <= ns; i++) 00281 { 00282 segment_info& s = results[i]; 00283 //Calculation of joint and segment accelerations 00284 //equation g) qdotdot[i] = D^-1*(Q - Z'(R + P(C + acc[i-1]) + E*nu)) 00285 // = D^-1(u - Z'(P*acc[i-1] + E*nu) 00286 Twist a_g; 00287 Twist a_p; 00288 if (i == 1) 00289 { 00290 a_p = acc_root; 00291 } 00292 else 00293 { 00294 a_p = results[i - 1].acc; 00295 } 00296 00297 //The contribution of the constraint forces at segment i 00298 Vector6d tmp = s.E*nu; 00299 Wrench constraint_force = Wrench(Vector(tmp(3), tmp(4), tmp(5)), 00300 Vector(tmp(0), tmp(1), tmp(2))); 00301 00302 //acceleration components are also computed 00303 //Contribution of the acceleration of the parent (i-1) 00304 Wrench parent_force = s.P*a_p; 00305 double parent_forceProjection = -dot(s.Z, parent_force); 00306 double parentAccComp = parent_forceProjection / s.D; 00307 00308 //The constraint force and acceleration force projected on the joint axes -> axis torque/force 00309 double constraint_torque = -dot(s.Z, constraint_force); 00310 //The result should be the torque at this joint 00311 00312 torques(j) = constraint_torque; 00313 //s.constAccComp = torques(j) / s.D; 00314 s.constAccComp = constraint_torque / s.D; 00315 s.nullspaceAccComp = s.u / s.D; 00316 //total joint space acceleration resulting from accelerations of parent joints, constraint forces and 00317 // nullspace forces. 00318 q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp); 00319 s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed 00320 if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) 00321 j++; 00322 } 00323 } 00324 00325 /* 00326 void ChainIdSolver_Vereshchagin::getLinkCartesianPose(Frames& x_base) 00327 { 00328 for (int i = 0; i < ns; i++) 00329 { 00330 x_base[i] = results[i + 1].F_base; 00331 } 00332 return; 00333 } 00334 00335 void ChainIdSolver_Vereshchagin::getLinkCartesianVelocity(Twists& xDot_base) 00336 { 00337 00338 for (int i = 0; i < ns; i++) 00339 { 00340 xDot_base[i] = results[i + 1].F_base.M * results[i + 1].v; 00341 } 00342 return; 00343 } 00344 00345 void ChainIdSolver_Vereshchagin::getLinkCartesianAcceleration(Twists& xDotDot_base) 00346 { 00347 00348 for (int i = 0; i < ns; i++) 00349 { 00350 xDotDot_base[i] = results[i + 1].F_base.M * results[i + 1].acc; 00351 //std::cout << "XDotDot_base[i] " << xDotDot_base[i] << std::endl; 00352 } 00353 return; 00354 } 00355 00356 void ChainIdSolver_Vereshchagin::getLinkPose(Frames& x_local) 00357 { 00358 for (int i = 0; i < ns; i++) 00359 { 00360 x_local[i] = results[i + 1].F; 00361 } 00362 return; 00363 } 00364 00365 void ChainIdSolver_Vereshchagin::getLinkVelocity(Twists& xDot_local) 00366 { 00367 for (int i = 0; i < ns; i++) 00368 { 00369 xDot_local[i] = results[i + 1].v; 00370 } 00371 return; 00372 00373 } 00374 00375 void ChainIdSolver_Vereshchagin::getLinkAcceleration(Twists& xDotdot_local) 00376 { 00377 for (int i = 0; i < ns; i++) 00378 { 00379 xDotdot_local[i] = results[i + 1].acc; 00380 } 00381 return; 00382 00383 } 00384 00385 void ChainIdSolver_Vereshchagin::getJointBiasAcceleration(JntArray& bias_q_dotdot) 00386 { 00387 for (int i = 0; i < ns; i++) 00388 { 00389 //this is only force 00390 double tmp = results[i + 1].totalBias; 00391 //this is accelleration 00392 bias_q_dotdot(i) = tmp / results[i + 1].D; 00393 00394 //s.totalBias = - dot(s.Z, s.R + s.PC); 00395 //std::cout << "totalBiasAccComponent" << i << ": " << bias_q_dotdot(i) << std::endl; 00396 //bias_q_dotdot(i) = s.totalBias/s.D 00397 00398 } 00399 return; 00400 00401 } 00402 00403 void ChainIdSolver_Vereshchagin::getJointConstraintAcceleration(JntArray& constraint_q_dotdot) 00404 { 00405 for (int i = 0; i < ns; i++) 00406 { 00407 constraint_q_dotdot(i) = results[i + 1].constAccComp; 00408 //double tmp = results[i + 1].u; 00409 //s.u = torques(j) + s.totalBias; 00410 // std::cout << "s.constraintAccComp" << i << ": " << results[i+1].constAccComp << std::endl; 00411 //nullspace_q_dotdot(i) = s.u/s.D 00412 00413 } 00414 return; 00415 00416 00417 } 00418 00419 //Check the name it does not seem to be appropriate 00420 00421 void ChainIdSolver_Vereshchagin::getJointNullSpaceAcceleration(JntArray& nullspace_q_dotdot) 00422 { 00423 for (int i = 0; i < ns; i++) 00424 { 00425 nullspace_q_dotdot(i) = results[i + 1].nullspaceAccComp; 00426 //double tmp = results[i + 1].u; 00427 //s.u = torques(j) + s.totalBias; 00428 //std::cout << "s.nullSpaceAccComp" << i << ": " << results[i+1].nullspaceAccComp << std::endl; 00429 //nullspace_q_dotdot(i) = s.u/s.D 00430 00431 } 00432 return; 00433 00434 00435 } 00436 00437 //This is not only a bias force energy but also includes generalized forces 00438 //change type of parameter G 00439 //this method should retur array of G's 00440 00441 void ChainIdSolver_Vereshchagin::getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G) 00442 { 00443 for (int i = 0; i < ns; i++) 00444 { 00445 G = results[i + 1].G; 00446 //double tmp = results[i + 1].u; 00447 //s.u = torques(j) + s.totalBias; 00448 //std::cout << "s.G " << i << ": " << results[i+1].G << std::endl; 00449 //nullspace_q_dotdot(i) = s.u/s.D 00450 00451 } 00452 return; 00453 00454 } 00455 00456 //this method should retur array of R's 00457 00458 void ChainIdSolver_Vereshchagin::getLinkBiasForceMatrix(Wrenches& R_tilde) 00459 { 00460 for (int i = 0; i < ns; i++) 00461 { 00462 R_tilde[i] = results[i + 1].R_tilde; 00463 //Azamat: bias force as in Featherstone (7.20) 00464 //s.R_tilde = s.U + child.R + child.PC + child.PZ / child.D * child.u; 00465 std::cout << "s.R_tilde " << i << ": " << results[i + 1].R_tilde << std::endl; 00466 } 00467 return; 00468 } 00469 00470 */ 00471 00472 }//namespace