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

ROBOOP v1.21a
Generated Tue Nov 23 16:35:51 2004 by Doxygen 1.3.9.1