chainidsolver_vereshchagin.cpp
Go to the documentation of this file.
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 (error = E_SIZE_MISMATCH);
00051     if (alfa.columns() != nc || beta.rows() != nc)
00052         return (error = E_SIZE_MISMATCH);
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 (error = E_NOERROR);
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


orocos_kdl
Author(s):
autogenerated on Sat Oct 7 2017 03:04:28