Homepage Demos Overview Downloads Tutorials Reference
Credits

kinemat.cpp

Go to the documentation of this file.
00001 /*
00002 ROBOOP -- A robotics object oriented package in C++
00003 Copyright (C) 1996-2004  Richard Gourdeau
00004 
00005 This library is free software; you can redistribute it and/or modify
00006 it under the terms of the GNU Lesser General Public License as
00007 published by the Free Software Foundation; either version 2.1 of the
00008 License, or (at your option) any later version.
00009 
00010 This library is distributed in the hope that it will be useful,
00011 but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 GNU Lesser General Public License for more details.
00014 
00015 You should have received a copy of the GNU Lesser General Public
00016 License along with this library; if not, write to the Free Software
00017 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018 
00019 
00020 Report problems and direct all questions to:
00021 
00022 Richard Gourdeau
00023 Professeur Agrege
00024 Departement de genie electrique
00025 Ecole Polytechnique de Montreal
00026 C.P. 6079, Succ. Centre-Ville
00027 Montreal, Quebec, H3C 3A7
00028 
00029 email: richard.gourdeau@polymtl.ca
00030 -------------------------------------------------------------------------------
00031 Revision_history:
00032 
00033 2003/02/03: Etienne Lachance
00034    -Rewrite mRobot::jacobian() since previous version was incorrect.
00035    -Added functions kine_pd().
00036    -Make sur that joint angles (qout) are in [-pi, pi] in inv_kin functions.
00037 
00038 2003/05/18: Etienne Lachance
00039    -Added functions Robot_basic::jacobian_DLS_inv and 
00040     Robot/mRobot/mRobot_min_para::jacobian_dot
00041 
00042 2003/08/22: Etienne Lachance
00043    -Added parameter converge in inv_kin prototype function. It indicates if the
00044     inverse kinematics solution converge.
00045 
00046 2003/11/26: Etienne Lachance
00047    -Use angle conditions only if it converge in inv_kin.
00048 
00049 2004/01/23: Etienne Lachance
00050    -Added const in non reference argument for all functions.
00051 
00052 2004/03/12: Etienne Lachance
00053    -Added logic to set q in inv_kin. 
00054 
00055 2004/04/24: Etienne Lachance
00056    -Moved inv_kin to invkine.cpp.
00057 
00058 2004/05/14: Etienne Lachance
00059    -Replaced vec_x_prod by CrossProduct.
00060 
00061 2004/05/21: Etienne Lachance
00062    -Added Doxygen comments.
00063 
00064 2004/07/01: Ethan Tira-Thompson
00065     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00066 
00067 2004/07/16: Ethan Tira-Thompson
00068     -Supports Link::immobile flag so jacobians and deltas are 0 for immobile joints
00069     -Jacobians will only contain entries for mobile joints - otherwise NaNs result
00070      in later processing
00071     -Added parameters to jacobian functions to generate for frames other than 
00072      the end effector
00073 
00074 2004/07/21: Ethan Tira-Thompson
00075     -Fixed a few bugs in previous modifications
00076     -Added dTdqi(...,endlink) variants
00077     -Renamed dp parameter of dTdqi to dpos so it doesn't shadow member variable
00078 
00079 2004/08/09: Ethan Tira-Thompson
00080     -kine(0) will return the identity matrix instead of just an error
00081 -------------------------------------------------------------------------------
00082 */
00083 
00084 /*!
00085   @file kinemat.cpp
00086   @brief Kinematics functions.
00087 */
00088 
00089 #include "robot.h"
00090 
00091 #ifdef use_namespace
00092 namespace ROBOOP {
00093   using namespace NEWMAT;
00094 #endif
00095 //! @brief RCS/CVS version.
00096 static const char rcsid[] __UNUSED__ = "$Id: kinemat.cpp,v 1.17 2005/07/26 03:22:09 ejt Exp $";
00097 
00098 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos)const
00099 /*!
00100   @brief Direct kinematics at end effector.
00101   @param Rot: End effector orientation.
00102   @param pos: Enf effector position.
00103 */
00104 {
00105    kine(Rot,pos,dof+fix);
00106 }
00107 
00108 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos, const int j)const
00109 /*!
00110   @brief Direct kinematics at end effector.
00111   @param Rot: Frame j orientation.
00112   @param pos: Frame j position.
00113   @param j: Selected frame.
00114 */
00115 {
00116    if(j < 0 || j > dof+fix) error("j must be 0 <= j <= dof+fix");
00117    if(j == 0) {
00118       Rot = R[0];
00119       pos = p[0];
00120       return;
00121    }
00122 
00123    Rot = links[1].R;
00124    pos = links[1].p;
00125    for (int i = 2; i <= j; i++) {
00126       pos = pos + Rot*links[i].p;
00127       Rot = Rot*links[i].R;
00128    }
00129 }
00130 
00131 ReturnMatrix Robot_basic::kine(void)const
00132 //! @brief Return the end effector direct kinematics transform matrix.
00133 {
00134    Matrix thomo;
00135 
00136    thomo = kine(dof+fix);
00137    thomo.Release(); return thomo;
00138 }
00139 
00140 ReturnMatrix Robot_basic::kine(const int j)const
00141 //! @brief Return the frame j direct kinematics transform matrix.
00142 {
00143    Matrix Rot, thomo(4,4);
00144    ColumnVector pos;
00145 
00146    kine(Rot,pos,j);
00147    thomo << fourbyfourident;
00148    thomo.SubMatrix(1,3,1,3) = Rot;
00149    thomo.SubMatrix(1,3,4,4) = pos;
00150    thomo.Release(); return thomo;
00151 }
00152 
00153 ReturnMatrix Robot_basic::kine_pd(const int j)const
00154 /*!
00155   @brief Direct kinematics with velocity.
00156 
00157   Return a \f$3\times 5\f$ matrix. The first three columns
00158   are the frame j to the base rotation, the fourth column is
00159   the frame j w.r.t to the base postion vector and the last 
00160   column is the frame j w.r.t to the base translational 
00161   velocity vector. Print an error on the console if j is
00162   out of range.
00163 */
00164 {
00165    Matrix temp(3,5), Rot;
00166    ColumnVector pos, pos_dot;
00167 
00168    if(j < 1 || j > dof)
00169      error("j must be 1 <= j <= dof");
00170 
00171    kine_pd(Rot, pos, pos_dot, j);
00172 
00173    temp.SubMatrix(1,3,1,3) = Rot;
00174    temp.SubMatrix(1,3,4,4) = pos;
00175    temp.SubMatrix(1,3,5,5) = pos_dot;
00176    temp.Release();
00177    return temp;
00178 }
00179 
00180 ReturnMatrix Robot_basic::jacobian_DLS_inv(const double eps, const double lambda_max,
00181       const int ref)const
00182 /*!
00183   @brief Inverse Jacobian based on damped least squares inverse.
00184 
00185   @param eps: Range of singular region.
00186   @param lambda_max: Value to obtain a good solution in singular region.
00187   @param ref: Selected frame (ex: joint 4).
00188 
00189   The Jacobian inverse, based on damped least squares, is 
00190   \f[
00191     J^{-1}(q) = \big(J^T(q)J(q) + \lambda^2I \big)^{-1}J^T(q)
00192   \f]
00193   where \f$\lambda\f$ and \f$I\f$ is a damping factor and the identity matrix 
00194   respectively. Based on SVD (Singular Value Decomposition) the Jacobian is
00195   \f$ J = \sum_{i=1}^{m}\sigma_i u_i v_i^T\f$, where \f$u_i\f$, \f$v_i\f$ and
00196   \f$\sigma_i\f$ are respectively the input vector, the ouput vector and the
00197   singular values (\f$\sigma_1 \geq \sigma_2 \cdots \geq \sigma_r \geq 0\f$, \f$r\f$
00198   is the rank of J). Using the previous equations we obtain
00199   \f[
00200     J^{-1}(q) = \sum_{i=1}^{m} \frac{\sigma_i}{\sigma_i^2 + \lambda_i^2}v_iu_i
00201   \f]
00202   A singular region, based on the smallest singular value, can be defined by
00203   \f[
00204    \lambda^2 = \Bigg\{
00205     \begin{array}{cc}
00206       0 &     \textrm{si $\sigma_6 \geq \epsilon$} \\
00207       \Big(1 - (\frac{\sigma_6}{\epsilon})^2 \Big)\lambda^2_{max} & \textrm{sinon}
00208     \end{array}
00209   \f] 
00210 */
00211 {
00212    Matrix jacob_inv_DLS, U, V;
00213    DiagonalMatrix Q;
00214    SVD(jacobian(ref), Q, U, V);
00215 
00216    if(Q(6,6) >= eps)
00217       jacob_inv_DLS = V*Q.i()*U.t();
00218    else
00219    {
00220       Q(6,6) += (1 - pow(Q(6,6)/eps,2))*lambda_max*lambda_max;
00221       jacob_inv_DLS = V*Q.i()*U.t();
00222    }
00223 
00224    jacob_inv_DLS.Release();
00225    return(jacob_inv_DLS);
00226 }
00227 
00228 /*! @brief converts one or more column-wise vectors from frame @a cur to frame @a dest
00229  *  @param[out] R on return contains rotation matrix between @a cur and @a dest
00230  *  @param[out] p on return contains vector from @a cur to @a dest
00231  *  @param[in] cur is the current frame
00232  *  @param[in] dest is the target frame */
00233 void Robot_basic::convertFrame(Matrix& R, ColumnVector& p, int cur, int dest) const {
00234    if(cur>dest && cur>0) {
00235      if(dest<=0)
00236         dest=1; //skip identity matrix
00237      R=links[dest].R;
00238      p=links[dest].p;
00239      for(dest++;dest<cur;dest++) {
00240        p=p+R*links[dest].p;
00241        R=R*links[dest].R;
00242      }
00243    } else if(cur<dest && dest>0) {
00244      if(cur<=0)
00245         cur=1; //skip identity matrix
00246      R=links[cur].R;
00247      p=links[cur].p;
00248      for(cur++;cur<dest;cur++) {
00249        p=p+R*links[cur].p;
00250        R=R*links[cur].R;
00251      }
00252      R=R.t();
00253      p=R*-p;
00254    } else {
00255       R=Matrix(3,3);
00256       R<<threebythreeident;
00257       p=ColumnVector(3);
00258       p=0;
00259    }
00260 }
00261 /*! @brief Returns a matrix which can be used to convert from frame @a cur to frame @a dest
00262  *  The Matrix returned will be homogenous (4x4). */
00263 ReturnMatrix Robot_basic::convertFrame(int cur, int dest) const {
00264    Matrix R;
00265    ColumnVector p;
00266    convertFrame(R,p,cur,dest);
00267    return pack4x4(R,p);
00268 }
00269 
00270 /*! @brief converts vector(s) on link @a cur to be vector(s) in joint reference frame @a dest
00271  *  @param[out] R on return contains rotation matrix between @a cur and @a dest
00272  *  @param[out] p on return contains vector from @a cur to @a dest
00273  *  @param[in] cur the link vectors of @a A are currently relative to
00274  *  @param[in] dest the link vectors should be converted to
00275  *
00276  *  Note the difference between this and convertFrame is that a link
00277  *  moves within the frame at its joints origin - the frame is static
00278  *  no matter how the joint moves within it.  If we have a point on
00279  *  the link, then the position of that point will move within the frame
00280  *  as the joint moves.  This function will return vector(s) of A
00281  *  represented as if they were connected to link @a dest.\n
00282  *  The Matrix returned will match the homogenousness of @a A. */
00283 void Robot_basic::convertLinkToFrame(Matrix& R, ColumnVector& p, int cur, int dest) const {
00284    convertFrame(R,p,cur,dest);
00285    if(cur<1)
00286       return;
00287    if(links[cur].get_joint_type()==0) {
00288       Matrix rotz3(3,3);
00289       rotz3 << threebythreeident;
00290       rotz3(1,1)=rotz3(2,2)=cos((links[cur].get_theta()-links[cur].get_joint_offset()));
00291       rotz3(1,2)=-(rotz3(2,1)=sin((links[cur].get_theta()-links[cur].get_joint_offset())));
00292       R*=rotz3;
00293    } else {
00294       p+=R.SubMatrix(1,3,3,3)*((links[cur].get_d()-links[cur].get_joint_offset()));
00295    }
00296 }
00297 
00298 /*! @brief Returns a matrix which can be used to convert from link frame @a cur to joint frame @a dest
00299  *  @param cur the source link
00300  *  @param dest the target frame
00301  *
00302  *  Note the difference between this and convertFrame is that a link
00303  *  moves within the frame at its joints origin - the frame is static
00304  *  no matter how the joint moves within it.  If we have a point on
00305  *  the link, then the position of that point will move within the frame
00306  *  as the joint moves.  This function will return vector(s) of A
00307  *  represented as if they were connected to link @a dest.\n
00308  *  The Matrix returned will be homogenous (4x4). */
00309 ReturnMatrix Robot_basic::convertLinkToFrame(int cur, int dest) const {
00310    Matrix R;
00311    ColumnVector p;
00312    convertLinkToFrame(R,p,cur,dest);
00313    return pack4x4(R,p);
00314 }
00315 
00316 /*! @brief converts vector(s) in joint referenc frame @a cur to be vector(s) on link @a dest
00317  *  @param[out] R on return contains rotation matrix between @a cur and @a dest
00318  *  @param[out] p on return contains vector from @a cur to @a dest
00319  *  @param[in] cur the link vectors of @a A are currently relative to
00320  *  @param[in] dest the link vectors should be converted to
00321  *
00322  *  Note the difference between this and convertFrame is that a link
00323  *  moves within the frame at its joints origin - the frame is static
00324  *  no matter how the joint moves within it.  If we have a point on
00325  *  the link, then the position of that point will move within the frame
00326  *  as the joint moves.  This function will return vector(s) of A
00327  *  represented as if they were connected to link @a dest.\n
00328  *  The Matrix returned will match the homogenousness of @a A. */
00329 void Robot_basic::convertFrameToLink(Matrix& R, ColumnVector& p, int cur, int dest) const {
00330    convertFrame(R,p,cur,dest);
00331    if(dest<1)
00332       return;
00333    if(links[dest].get_joint_type()==0) {
00334       Matrix rotz3(3,3);
00335       rotz3 << threebythreeident;
00336       rotz3(1,1)=rotz3(2,2)=cos(-(links[dest].get_theta()-links[dest].get_joint_offset()));
00337       rotz3(1,2)=-(rotz3(2,1)=sin(-(links[dest].get_theta()-links[dest].get_joint_offset())));
00338       R=rotz3*R;
00339       p=rotz3*p;
00340    } else {
00341       p(3)+=-(links[dest].get_d()-links[dest].get_joint_offset());
00342    }
00343 }
00344 
00345 /*! @brief Returns a matrix which can be used to convert from joint reference frame @a cur to link frame @a dest
00346  *  @param cur the source link
00347  *  @param dest the target link
00348  *
00349  *  Note the difference between this and convertFrame is that a link
00350  *  moves within the frame at its joints origin - the frame is static
00351  *  no matter how the joint moves within it.  If we have a point on
00352  *  the link, then the position of that point will move within the frame
00353  *  as the joint moves.  This function will return vector(s) of A
00354  *  represented as if they were connected to link @a dest.\n
00355  *  The Matrix returned will be homogenous (4x4). */
00356 ReturnMatrix Robot_basic::convertFrameToLink(int cur, int dest) const {
00357    Matrix R;
00358    ColumnVector p;
00359    convertFrameToLink(R,p,cur,dest);
00360    return pack4x4(R,p);
00361 }
00362 
00363 /*! @brief converts vector(s) on link @a cur to be vector(s) on link @a dest
00364  *  @param[out] R on return contains rotation matrix between @a cur and @a dest
00365  *  @param[out] p on return contains vector from @a cur to @a dest
00366  *  @param[in] cur the link vectors of @a A are currently relative to
00367  *  @param[in] dest the link vectors should be converted to
00368  *
00369  *  Note the difference between this and convertFrame is that a link
00370  *  moves within the frame at its joints origin - the frame is static
00371  *  no matter how the joint moves within it.  If we have a point on
00372  *  the link, then the position of that point will move within the frame
00373  *  as the joint moves.  This function will return vector(s) of A
00374  *  represented as if they were connected to link @a dest.\n
00375  *  The Matrix returned will match the homogenousness of @a A. */
00376 void Robot_basic::convertLink(Matrix& R, ColumnVector& p, int cur, int dest) const {
00377    convertFrame(R,p,cur,dest);
00378    if(cur>0) {
00379       if(links[cur].get_joint_type()==0) {
00380          Matrix rotz3(3,3);
00381          rotz3 << threebythreeident;
00382          rotz3(1,1)=rotz3(2,2)=cos((links[cur].get_theta()-links[cur].get_joint_offset()));
00383          rotz3(1,2)=-(rotz3(2,1)=sin((links[cur].get_theta()-links[cur].get_joint_offset())));
00384          R*=rotz3;
00385       } else {
00386          p+=R.SubMatrix(1,3,3,3)*((links[cur].get_d()-links[cur].get_joint_offset()));
00387       }
00388    }
00389    if(dest<1)
00390       return;
00391    if(links[dest].get_joint_type()==0) {
00392       Matrix rotz3(3,3);
00393       rotz3 << threebythreeident;
00394       rotz3(1,1)=rotz3(2,2)=cos(-(links[dest].get_theta()-links[dest].get_joint_offset()));
00395       rotz3(1,2)=-(rotz3(2,1)=sin(-(links[dest].get_theta()-links[dest].get_joint_offset())));
00396       R=rotz3*R;
00397       p=rotz3*p;
00398    } else {
00399       p(3)+=-(links[dest].get_d()-links[dest].get_joint_offset());
00400    }
00401 }
00402 
00403 /*! @brief Returns a matrix which can be used to convert from link @a cur to link @a dest
00404  *  @param cur the source link
00405  *  @param dest the target link
00406  *
00407  *  Note the difference between this and convertFrame is that a link
00408  *  moves within the frame at its joints origin - the frame is static
00409  *  no matter how the joint moves within it.  If we have a point on
00410  *  the link, then the position of that point will move within the frame
00411  *  as the joint moves.  This function will return vector(s) of A
00412  *  represented as if they were connected to link @a dest.\n
00413  *  The Matrix returned will be homogenous (4x4). */
00414 ReturnMatrix Robot_basic::convertLink(int cur, int dest) const {
00415    Matrix R;
00416    ColumnVector p;
00417    convertLink(R,p,cur,dest);
00418    return pack4x4(R,p);
00419 }
00420 
00421 
00422 // ---------------------  R O B O T   DH   N O T A T I O N  --------------------------
00423 
00424 void Robot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00425                     const int j)const
00426 /*!
00427   @brief Direct kinematics with velocity.
00428   @param Rot: Frame j rotation matrix w.r.t to the base frame.
00429   @param pos: Frame j position vector wr.r.t to the base frame.
00430   @param pos_dot: Frame j velocity vector w.r.t to the base frame.
00431   @param j: Frame j.
00432   Print an error on the console if j is out of range.
00433 */
00434 {
00435    if(j < 1 || j > dof)
00436       error("j must be 1 <= j <= dof");
00437 
00438    if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00439        pos = ColumnVector(3); 
00440    if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00441      pos_dot = ColumnVector(3);
00442 
00443    pos = 0.0;
00444    pos_dot = 0.0;
00445    for(int i = 1; i <= j; i++)
00446    {
00447       R[i] = R[i-1]*links[i].R;
00448       pos = pos + R[i-1]*links[i].p;
00449       pos_dot = pos_dot + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
00450    }
00451    Rot = R[j];
00452 }
00453 
00454 void Robot::dTdqi(Matrix & dRot, ColumnVector & dpos,
00455                   const int i, const int endlink)
00456 /*!
00457   @brief Partial derivative of the robot position (homogeneous transf.)
00458 
00459   This function computes the partial derivatives:
00460   \f[
00461   \frac{\partial{}^0 T_n}{\partial q_i} = {}^0 T_{i-1} Q_i \; {}^{i-1} T_n
00462   \f]
00463   in standard notation and
00464   \f[
00465 
00466   \frac{\partial{}^0 T_n}{\partial q_i} = {}^0 T_{i} Q_i \; {}^{i} T_n
00467   \f]
00468   in modified notation,
00469 
00470  with
00471  \f[
00472  Q_i = 
00473   \left [
00474     \begin{array}{cccc}
00475       0 & -1 & 0 & 0 \\
00476       1 & 0 & 0 & 0 \\
00477       0 & 0 & 0 & 0 \\
00478       0 & 0 & 0 & 0
00479     \end{array}
00480   \right ]
00481   \f]
00482  for a revolute joint and
00483  \f[
00484   Q_i = 
00485   \left [
00486    \begin{array}{cccc}
00487      0 & 0 & 0 & 0 \\
00488      0 & 0 & 0 & 0 \\
00489      0 & 0 & 0 & 1 \\
00490      0 & 0 & 0 & 0
00491    \end{array}
00492   \right ]
00493   \f]
00494   for a prismatic joint.
00495 
00496   \f$dRot\f$ and \f$dp\f$ are modified on output.
00497 */
00498 {
00499    int j;
00500    if(i < 1 || i > endlink) error("i must be 1 <= i <= endlink");
00501    if(links[i].get_immobile()) {
00502       dRot = Matrix(3,3);
00503       dpos = Matrix(3,1);
00504       dRot = 0.0;
00505       dpos = 0.0;
00506    } else if(links[i].get_joint_type() == 0) {
00507       Matrix dR(3,3);
00508       dR = 0.0;
00509       Matrix R2 = links[i].R;
00510       ColumnVector p2 = links[i].p;
00511       dRot = Matrix(3,3);
00512       dRot << threebythreeident;
00513       for(j = 1; j < i; j++) {
00514          dRot = dRot*links[j].R;
00515       }
00516       // dRot * Q
00517       for(j = 1; j <= 3; j++) {
00518          dR(j,1) = dRot(j,2);
00519          dR(j,2) = -dRot(j,1);
00520       }
00521       for(j = i+1; j <= endlink; j++) {
00522          p2 = p2 + R2*links[j].p;
00523          R2 = R2*links[j].R;
00524       }
00525       dpos = dR*p2;
00526       dRot = dR*R2;
00527    } else {
00528       dRot = Matrix(3,3);
00529       dpos = Matrix(3,1);
00530       dRot = 0.0;
00531       dpos = 0.0;
00532       dpos(3) = 1.0;
00533       for(j = i-1; j >= 1; j--) {
00534          dpos = links[j].R*dpos;
00535       }
00536    }
00537 }
00538 
00539 ReturnMatrix Robot::dTdqi(const int i, const int endlink)
00540 /*!
00541   @brief Partial derivative of the robot position (homogeneous transf.)
00542 
00543   See Robot::dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00544   for equations.
00545 */
00546 {
00547    Matrix dRot, thomo(4,4);
00548    ColumnVector dpos;
00549 
00550    dTdqi(dRot, dpos, i, endlink);
00551    thomo = (Real) 0.0;
00552    thomo.SubMatrix(1,3,1,3) = dRot;
00553    thomo.SubMatrix(1,3,4,4) = dpos;
00554    thomo.Release(); return thomo;
00555 }
00556 
00557 ReturnMatrix Robot::jacobian(const int endlink, const int ref)const
00558 /*!
00559   @brief Jacobian of mobile links up to endlink expressed at frame ref.
00560 
00561   The Jacobian expressed in based frame is
00562   \f[
00563     ^{0}J(q) = 
00564     \left[ 
00565      \begin{array}{cccc}
00566       ^{0}J_1(q) & ^{0}J_2(q) & \cdots & ^{0}J_n(q) \\
00567      \end{array} 
00568    \right]
00569   \f]
00570   where \f$^{0}J_i(q)\f$ is defined by
00571   \f[
00572     ^{0}J_i(q) = 
00573      \begin{array}{cc}
00574       \left[ 
00575        \begin{array}{c}
00576         z_i \times ^{i}p_n \\
00577         z_i \\
00578        \end{array} 
00579       \right] & \textrm{rotoid joint} 
00580      \end{array} 
00581    \f]
00582    \f[
00583     ^{0}J_i(q) = 
00584       \begin{array}{cc}
00585        \left[ 
00586         \begin{array}{c}
00587          z_i \\
00588          0 \\
00589         \end{array} 
00590        \right] & \textrm{prismatic joint} \\
00591       \end{array}
00592   \f]
00593 
00594   Expressed in a different frame the Jacobian is obtained by
00595   \f[
00596     ^{i}J(q)  = 
00597     \left[
00598      \begin{array}{cc}
00599       ^{0}_iR^T & 0 \\
00600         0 & ^{0}_iR^T
00601      \end{array} 
00602     \right] 
00603    {^{0}}J(q)
00604   \f]
00605 */
00606 {
00607    const int adof=get_available_dof(endlink);
00608    Matrix jac(6,adof);
00609    Matrix pr, temp(3,1);
00610 
00611    if(ref < 0 || ref > dof) error("invalid referential");
00612 
00613    for(int i = 1; i <= dof; i++) {
00614       R[i] = R[i-1]*links[i].R;
00615       p[i] = p[i-1]+R[i-1]*links[i].p;
00616    }
00617 
00618    for(int i=1,j=1; j <= adof; i++) {
00619       if(links[i].get_immobile())
00620          continue;
00621       if(links[i].get_joint_type() == 0) {
00622          temp(1,1) = R[i-1](1,3);
00623          temp(2,1) = R[i-1](2,3);
00624          temp(3,1) = R[i-1](3,3);
00625          pr = p[endlink]-p[i-1];
00626          temp = CrossProduct(temp,pr);
00627          jac(1,j) = temp(1,1);
00628          jac(2,j) = temp(2,1);
00629          jac(3,j) = temp(3,1);
00630          jac(4,j) = R[i-1](1,3);
00631          jac(5,j) = R[i-1](2,3);
00632          jac(6,j) = R[i-1](3,3);
00633       } else {
00634          jac(1,j) = R[i-1](1,3);
00635          jac(2,j) = R[i-1](2,3);
00636          jac(3,j) = R[i-1](3,3);
00637          jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00638       }
00639       j++;
00640    }
00641    if(ref != 0) {
00642       Matrix zeros(3,3);
00643       zeros = (Real) 0.0;
00644       Matrix RT = R[ref].t();
00645       Matrix Rot;
00646       Rot = ((RT & zeros) | (zeros & RT));
00647       jac = Rot*jac;
00648    }
00649 
00650    jac.Release(); return jac;
00651 }
00652 
00653 ReturnMatrix Robot::jacobian_dot(const int ref)const
00654 /*!
00655   @brief Jacobian derivative of mobile joints expressed at frame ref.
00656 
00657   The Jacobian derivative expressed in based frame is
00658   \f[
00659    ^{0}\dot{J}(q,\dot{q}) = 
00660     \left[ 
00661      \begin{array}{cccc}
00662        ^{0}\dot{J}_1(q,\dot{q}) & ^{0}\dot{J}_2(q,\dot{q}) & \cdots & ^{0}\dot{J}_n(q,\dot{q}) \\
00663      \end{array} 
00664     \right]
00665   \f]
00666   where \f$^{0}\dot{J}_i(q,\dot{q})\f$ is defined by
00667   \f[
00668    ^{0}\dot{J}_i(q,\dot{q}) = 
00669     \begin{array}{cc}
00670      \left[ 
00671       \begin{array}{c}
00672        \omega_{i-1} \times z_i \\
00673        \omega_{i-1} \times ^{i-1}p_n + z_i \times ^{i-1}\dot{p}_n
00674      \end{array} 
00675     \right] & \textrm{rotoid joint} 
00676    \end{array}
00677   \f]
00678   \f[
00679    ^{0}\dot{J}_i(q,\dot{q}) =
00680    \begin{array}{cc}
00681     \left[ 
00682      \begin{array}{c}
00683        0 \\
00684        0 \\
00685      \end{array} 
00686     \right] & \textrm{prismatic joint} \\
00687    \end{array}
00688   \f]
00689   Expressed in a different frame the Jacobian derivative is obtained by
00690   \f[
00691     ^{i}J(q)  = 
00692     \left[
00693      \begin{array}{cc}
00694       ^{0}_iR^T & 0 \\
00695         0 & ^{0}_iR^T
00696      \end{array} 
00697     \right] 
00698    {^{0}}J(q)
00699   \f]
00700 */
00701 {
00702    const int adof=get_available_dof();
00703    Matrix jacdot(6,adof);
00704    ColumnVector e(3), temp, pr, ppr;
00705 
00706    if(ref < 0 || ref > dof)
00707       error("invalid referential");
00708 
00709    for(int i = 1; i <= dof; i++)
00710    {
00711       R[i] = R[i-1]*links[i].R;
00712       p[i] = p[i-1] + R[i-1]*links[i].p;
00713       pp[i] = pp[i-1] + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
00714    }
00715 
00716    for(int i=1,j=1; j <= adof; i++) {
00717       if(links[i].get_immobile())
00718          continue;
00719       if(links[i].get_joint_type() == 0)
00720       {
00721          pr = p[dof]-p[i-1];
00722          ppr = pp[dof]-pp[i-1];
00723          e(1) = R[i-1](1,3);
00724          e(2) = R[i-1](2,3);
00725          e(3) = R[i-1](3,3);
00726          temp = CrossProduct(R[i-1]*w[i-1], e);
00727          jacdot(4,j) = temp(1);           // d(e)/dt
00728          jacdot(5,j) = temp(2);
00729          jacdot(6,j) = temp(3);
00730          temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
00731          jacdot(1,j) = temp(1);
00732          jacdot(2,j) = temp(2);
00733          jacdot(3,j) = temp(3);
00734       }
00735       else
00736          jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
00737             jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
00738       j++;
00739    }
00740 
00741    if(ref != 0) {
00742       Matrix zeros(3,3);
00743       zeros = (Real) 0.0;
00744       Matrix RT = R[ref].t();
00745       Matrix Rot;
00746       Rot = ((RT & zeros) | (zeros & RT));
00747       jacdot = Rot*jacdot;
00748    }
00749 
00750    jacdot.Release(); return jacdot;
00751 }
00752 
00753 // ----------------  R O B O T   M O D I F I E D   DH   N O T A T I O N  --------------------------
00754 
00755 void mRobot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00756                      const int j)const
00757 /*!
00758   @brief Direct kinematics with velocity.
00759   @param Rot: Frame j rotation matrix w.r.t to the base frame.
00760   @param pos: Frame j position vector wr.r.t to the base frame.
00761   @param pos_dot: Frame j velocity vector w.r.t to the base frame.
00762   @param j: Frame j.
00763   Print an error on the console if j is out of range.
00764 */
00765 {
00766    if(j < 1 || j > dof+fix)
00767       error("j must be 1 <= j <= dof+fix");
00768 
00769    if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00770        pos = ColumnVector(3); 
00771    if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00772      pos_dot = ColumnVector(3);
00773 
00774    pos = 0.0;
00775    pos_dot = 0.0;
00776    for(int i = 1; i <= j; i++)
00777    {
00778       pos = pos + R[i-1]*links[i].p;
00779       pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
00780       R[i] = R[i-1]*links[i].R;
00781    }
00782    Rot = R[j];
00783 }
00784 
00785 void mRobot::dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00786 /*!
00787   @brief Partial derivative of the robot position (homogeneous transf.)
00788 
00789   This function computes the partial derivatives:
00790   \f[
00791   \frac{\partial{}^0 T_n}{\partial q_i} = {}^0 T_{i} Q_i \; {}^{i} T_n
00792   \f]
00793   with
00794   \f[
00795   Q_i = 
00796   \left [
00797     \begin{array}{cccc}
00798       0 & -1 & 0 & 0 \\
00799       1 & 0 & 0 & 0 \\
00800       0 & 0 & 0 & 0 \\
00801       0 & 0 & 0 & 0
00802     \end{array}
00803   \right ]
00804   \f]
00805  for a revolute joint and
00806  \f[
00807   Q_i = 
00808   \left [
00809    \begin{array}{cccc}
00810      0 & 0 & 0 & 0 \\
00811      0 & 0 & 0 & 0 \\
00812      0 & 0 & 0 & 1 \\
00813      0 & 0 & 0 & 0
00814    \end{array}
00815   \right ]
00816   \f]
00817   for a prismatic joint.
00818 
00819   \f$dRot\f$ and \f$dp\f$ are modified on output.
00820 */
00821 {
00822    int j;
00823    if(i < 1 || i > endlink) error("i must be 1 <= i <= endlink");
00824    if(links[i].get_immobile()) {
00825       dRot = Matrix(3,3);
00826       dpos = Matrix(3,1);
00827       dRot = 0.0;
00828       dpos = 0.0;
00829    } else if(links[i].get_joint_type() == 0) {
00830       Matrix dR(3,3), R2(3,3), p2(3,1);
00831       dR = 0.0;
00832       dRot = Matrix(3,3);
00833       dRot << threebythreeident;
00834       for(j = 1; j <= i; j++) {
00835          dRot = dRot*links[j].R;
00836       }
00837       // dRot * Q
00838       for(j = 1; j <= 3; j++) {
00839          dR(j,1) = dRot(j,2);
00840          dR(j,2) = -dRot(j,1);
00841       }
00842       if(i < endlink) {
00843          R2 = links[i+1].R;
00844          p2 = links[i+1].p;
00845       } else {
00846          R2 <<  threebythreeident;
00847          p2 = 0.0;
00848       }
00849       for(j = i+1; j <= endlink; j++) {
00850          p2 = p2 + R2*links[j].p;
00851          R2 = R2*links[j].R;
00852       }
00853       dpos = dR*p2;
00854       dRot = dR*R2;  // probleme ...
00855    } else {
00856       dRot = Matrix(3,3);
00857       dpos = Matrix(3,1);
00858       dRot = 0.0;
00859       dpos = 0.0;
00860       dpos(3) = 1.0;
00861       for(j = i; j >= 1; j--) {
00862          dpos = links[j].R*dpos;
00863       }
00864    }
00865 }
00866 
00867 ReturnMatrix mRobot::dTdqi(const int i, const int endlink)
00868 /*!
00869   @brief Partial derivative of the robot position (homogeneous transf.)
00870 
00871   See mRobot::dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00872   for equations.
00873 */
00874 {
00875    Matrix dRot, thomo(4,4);
00876    ColumnVector dpos;
00877 
00878    dTdqi(dRot, dpos, i, endlink);
00879    thomo = (Real) 0.0;
00880    thomo.SubMatrix(1,3,1,3) = dRot;
00881    thomo.SubMatrix(1,3,4,4) = dpos;
00882    thomo.Release(); return thomo;
00883 }
00884 
00885 ReturnMatrix mRobot::jacobian(const int endlink, const int ref)const
00886 /*!
00887   @brief Jacobian of mobile joints up to endlink expressed at frame ref.
00888 
00889   See Robot::jacobian for equations.
00890 */
00891 {
00892    const int adof=get_available_dof(endlink);
00893    Matrix jac(6,adof);
00894    ColumnVector pr(3), temp(3);
00895 
00896    if(ref < 0 || ref > dof+fix)
00897       error("invalid referential");
00898 
00899    for(int i = 1; i <= dof+fix; i++) {
00900       R[i] = R[i-1]*links[i].R;
00901       p[i] = p[i-1] + R[i-1]*links[i].p;
00902    }
00903 
00904    for(int i=1,j=1; j <= adof; i++) {
00905       if(links[i].get_immobile())
00906          continue;
00907       if(links[i].get_joint_type() == 0){
00908          temp(1) = R[i](1,3);
00909          temp(2) = R[i](2,3);
00910          temp(3) = R[i](3,3);
00911          pr = p[endlink+fix]-p[i];
00912          temp = CrossProduct(temp,pr);
00913          jac(1,j) = temp(1);
00914          jac(2,j) = temp(2);
00915          jac(3,j) = temp(3);
00916          jac(4,j) = R[i](1,3);
00917          jac(5,j) = R[i](2,3);
00918          jac(6,j) = R[i](3,3);
00919       } else {
00920          jac(1,j) = R[i](1,3);
00921          jac(2,j) = R[i](2,3);
00922          jac(3,j) = R[i](3,3);
00923          jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00924       }
00925       j++;
00926    }
00927    if(ref != 0) {
00928       Matrix zeros(3,3);
00929       zeros = (Real) 0.0;
00930       Matrix RT = R[ref].t();
00931       Matrix Rot;
00932       Rot = ((RT & zeros) | (zeros & RT));
00933       jac = Rot*jac;
00934    }
00935    jac.Release(); return jac;
00936 }
00937 
00938 ReturnMatrix mRobot::jacobian_dot(const int ref)const
00939 /*!
00940   @brief Jacobian derivative of mobile joints expressed at frame ref.
00941 
00942   See Robot::jacobian_dot for equations.
00943 */
00944 {
00945    const int adof=get_available_dof();
00946    Matrix jacdot(6,adof);
00947    ColumnVector e(3), temp, pr, ppr;
00948 
00949    if(ref < 0 || ref > dof+fix)
00950       error("invalid referential");
00951 
00952    for(int i = 1; i <= dof+fix; i++)
00953    {
00954       R[i] = R[i-1]*links[i].R;
00955       p[i] = p[i-1] + R[i-1]*links[i].p;
00956       pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
00957    }
00958 
00959    for(int i=1,j=1; j <= adof; i++) {
00960       if(links[i].get_immobile())
00961          continue;
00962       if(links[i].get_joint_type() == 0)
00963       {
00964          pr = p[dof+fix]-p[i];
00965          ppr = pp[dof+fix]-pp[i];
00966 
00967          e(1) = R[i](1,3);
00968          e(2) = R[i](2,3);
00969          e(3) = R[i](3,3);
00970          temp = CrossProduct(R[i-1]*w[i-1], e);
00971          jacdot(4,j) = temp(1);           // d(e)/dt
00972          jacdot(5,j) = temp(2);
00973          jacdot(6,j) = temp(3);
00974 
00975          temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
00976          jacdot(1,j) = temp(1);
00977          jacdot(2,j) = temp(2);
00978          jacdot(3,j) = temp(3);
00979       }
00980       else
00981          jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
00982             jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
00983       j++;
00984    }
00985 
00986    if(ref != 0) {
00987       Matrix zeros(3,3);
00988       zeros = (Real) 0.0;
00989       Matrix RT = R[ref].t();
00990       Matrix Rot;
00991       Rot = ((RT & zeros) | (zeros & RT));
00992       jacdot = Rot*jacdot;
00993    }
00994 
00995    jacdot.Release(); return jacdot;
00996 }
00997 
00998 // ------------- R O B O T  DH  M O D I F I E D,  M I N I M U M   P A R A M E T E R S  ------------
00999 
01000 void mRobot_min_para::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
01001                               const int j)const
01002 /*!
01003   @brief Direct kinematics with velocity.
01004   @param Rot: Frame j rotation matrix w.r.t to the base frame.
01005   @param pos: Frame j position vector wr.r.t to the base frame.
01006   @param pos_dot: Frame j velocity vector w.r.t to the base frame.
01007   @param j: Frame j.
01008   Print an error on the console if j is out of range.
01009 */
01010 {
01011    if(j < 1 || j > dof+fix)
01012       error("j must be 1 <= j <= dof+fix");
01013 
01014    if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
01015        pos = ColumnVector(3); 
01016    if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
01017      pos_dot = ColumnVector(3);
01018 
01019    pos = 0.0;
01020    pos_dot = 0.0;
01021    for(int i = 1; i <= j; i++)
01022    {
01023       pos = pos + R[i-1]*links[i].p;
01024       pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
01025       R[i] = R[i-1]*links[i].R;
01026    }
01027    Rot = R[j];
01028 }
01029 
01030 void mRobot_min_para::dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
01031 /*!
01032   @brief Partial derivative of the robot position (homogeneous transf.)
01033 
01034   This function computes the partial derivatives:
01035   \f[
01036   \frac{\partial{}^0 T_n}{\partial q_i} = {}^0 T_{i} Q_i \; {}^{i} T_n
01037   \f]
01038 
01039   See mRobot::dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
01040   for equations.
01041 */
01042 {
01043    int j;
01044    if(i < 1 || i > endlink) error("i must be 1 <= i <= endlink");
01045    if(links[i].get_immobile()) {
01046       dRot = Matrix(3,3);
01047       dpos = Matrix(3,1);
01048       dRot = 0.0;
01049       dpos = 0.0;
01050    } else if(links[i].get_joint_type() == 0) {
01051       Matrix dR(3,3), R2, p2(3,1);
01052       dR = 0.0;
01053       dRot = Matrix(3,3);
01054       dRot << threebythreeident;
01055       for(j = 1; j <= i; j++) {
01056          dRot = dRot*links[j].R;
01057       }
01058       // dRot * Q
01059       for(j = 1; j <= 3; j++) {
01060          dR(j,1) = dRot(j,2);
01061          dR(j,2) = -dRot(j,1);
01062       }
01063       if(i < endlink) {
01064          R2 = links[i+1].R;
01065          p2 = links[i+1].p;
01066       } else {
01067          R2 <<  threebythreeident;
01068          p2 = 0.0;
01069       }
01070       for(j = i+1; j <= endlink; j++) {
01071          p2 = p2 + R2*links[j].p;
01072          R2 = R2*links[j].R;
01073       }
01074       dpos = dR*p2;
01075       dRot = dR*R2;
01076    } else {
01077       dRot = Matrix(3,3);
01078       dpos = Matrix(3,1);
01079       dRot = 0.0;
01080       dpos = 0.0;
01081       dpos(3) = 1.0;
01082       for(j = i; j >= 1; j--) {
01083          dpos = links[j].R*dpos;
01084       }
01085    }
01086 }
01087 
01088 ReturnMatrix mRobot_min_para::dTdqi(const int i, const int endlink)
01089 /*!
01090   @brief Partial derivative of the robot position (homogeneous transf.)
01091 
01092   See mRobot::dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
01093   for equations.
01094 */
01095 {
01096    Matrix dRot, thomo(4,4);
01097    ColumnVector dpos;
01098 
01099    dTdqi(dRot, dpos, i, endlink);
01100    thomo = (Real) 0.0;
01101    thomo.SubMatrix(1,3,1,3) = dRot;
01102    thomo.SubMatrix(1,3,4,4) = dpos;
01103    thomo.Release(); return thomo;
01104 }
01105 
01106 ReturnMatrix mRobot_min_para::jacobian(const int endlink, const int ref)const
01107 /*!
01108   @brief Jacobian of mobile joints up to endlink expressed at frame ref.
01109 
01110   See Robot::jacobian for equations.
01111 */
01112 {
01113    const int adof=get_available_dof(endlink);
01114    Matrix jac(6,adof);
01115    ColumnVector pr(3), temp(3);
01116 
01117    if(ref < 0 || ref > dof+fix)
01118       error("invalid referential");
01119 
01120    for(int i = 1; i <= dof+fix; i++) {
01121       R[i] = R[i-1]*links[i].R;
01122       p[i] = p[i-1] + R[i-1]*links[i].p;
01123    }
01124 
01125    for(int i=1,j=1; j<=adof; i++) {
01126       if(links[i].get_immobile())
01127          continue;
01128       if(links[i].get_joint_type() == 0){
01129          temp(1) = R[i](1,3);
01130          temp(2) = R[i](2,3);
01131          temp(3) = R[i](3,3);
01132 
01133          pr = p[endlink+fix]-p[i];
01134          temp = CrossProduct(temp,pr);
01135          jac(1,j) = temp(1);
01136          jac(2,j) = temp(2);
01137          jac(3,j) = temp(3);
01138          jac(4,j) = R[i](1,3);
01139          jac(5,j) = R[i](2,3);
01140          jac(6,j) = R[i](3,3);
01141       } else {
01142          jac(1,j) = R[i](1,3);
01143          jac(2,j) = R[i](2,3);
01144          jac(3,j) = R[i](3,3);
01145          jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
01146       }
01147       j++;
01148    }
01149    if(ref != 0) {
01150       Matrix zeros(3,3);
01151       zeros = (Real) 0.0;
01152       Matrix RT = R[ref].t();
01153       Matrix Rot;
01154       Rot = ((RT & zeros) | (zeros & RT));
01155       jac = Rot*jac;
01156    }
01157 
01158    jac.Release(); return jac;
01159 }
01160 
01161 ReturnMatrix mRobot_min_para::jacobian_dot(const int ref)const
01162 /*!
01163   @brief Jacobian derivative of mobile joints expressed at frame ref.
01164 
01165   See Robot::jacobian_dot for equations.
01166 */
01167 {
01168    const int adof=get_available_dof();
01169    Matrix jacdot(6,adof);
01170    ColumnVector e(3), temp, pr, ppr;
01171 
01172    if(ref < 0 || ref > dof+fix)
01173       error("invalid referential");
01174 
01175    for(int i = 1; i <= dof+fix; i++)
01176    {
01177       R[i] = R[i-1]*links[i].R;
01178       p[i] = p[i-1] + R[i-1]*links[i].p;
01179       pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
01180    }
01181 
01182    for(int i=1,j=1; j <= adof; i++) {
01183       if(links[i].get_immobile())
01184          continue;
01185       if(links[i].get_joint_type() == 0)
01186       {
01187          pr = p[dof+fix]-p[i];
01188          ppr = pp[dof+fix]-pp[i];
01189 
01190          e(1) = R[i](1,3);
01191          e(2) = R[i](2,3);
01192          e(3) = R[i](3,3);
01193          temp = CrossProduct(R[i-1]*w[i-1], e);
01194          jacdot(4,j) = temp(1);           // d(e)/dt
01195          jacdot(5,j) = temp(2);
01196          jacdot(6,j) = temp(3);
01197 
01198          temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
01199          jacdot(1,j) = temp(1);
01200          jacdot(2,j) = temp(2);
01201          jacdot(3,j) = temp(3);
01202       }
01203       else
01204          jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
01205             jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
01206       j++;
01207    }
01208 
01209    if(ref != 0) {
01210       Matrix zeros(3,3);
01211       zeros = (Real) 0.0;
01212       Matrix RT = R[ref].t();
01213       Matrix Rot;
01214       Rot = ((RT & zeros) | (zeros & RT));
01215       jacdot = Rot*jacdot;
01216    }
01217 
01218    jacdot.Release(); return jacdot;
01219 }
01220 
01221 #ifdef use_namespace
01222 }
01223 #endif

ROBOOP v1.21a
Generated Tue Aug 16 16:32:15 2005 by Doxygen 1.4.4