Homepage Demos Overview Downloads Tutorials Reference
Credits

clik.cpp

Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2002-2004  Etienne Lachance
00003 
00004 This library is free software; you can redistribute it and/or modify
00005 it under the terms of the GNU Lesser General Public License as
00006 published by the Free Software Foundation; either version 2.1 of the
00007 License, or (at your option) any later version.
00008 
00009 This library is distributed in the hope that it will be useful,
00010 but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 GNU Lesser General Public License for more details.
00013 
00014 You should have received a copy of the GNU Lesser General Public
00015 License along with this library; if not, write to the Free Software
00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017 
00018 
00019 Report problems and direct all questions to:
00020 
00021 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
00022 -------------------------------------------------------------------------------
00023 Revision_history:
00024 
00025 2004/07/01: Etienne Lachance
00026    -Added doxygen documentation.
00027 
00028 2004/07/01: Ethan Tira-Thompson
00029     -Added support for newmat's use_namespace #define, using ROBOOP namespace.
00030 */
00031 
00032 //! @brief RCS/CVS version.
00033 static const char rcsid[] = "$Id: clik.cpp,v 1.5 2004/09/26 19:55:35 ejt Exp $";
00034 
00035 /*!
00036   @file clik.cpp
00037   @brief Clik member functions.
00038 */
00039 
00040 #include "clik.h"
00041 
00042 #ifdef use_namespace
00043 namespace ROBOOP {
00044   using namespace NEWMAT;
00045 #endif
00046 
00047 /*!
00048   @fn Clik::Clik(const Robot & robot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00049                  const Real eps_, const Real lambda_max_, const Real dt);
00050   @brief Constructor.
00051 */
00052 Clik::Clik(const Robot & robot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00053            const Real eps_, const Real lambda_max_, const Real dt_):
00054       dt(dt_),
00055       eps(eps_),
00056       lambda_max(lambda_max_),
00057       robot(robot_)
00058 {
00059    robot_type = CLICK_DH;
00060    // Initialize with same joint position (and rates) has the robot.
00061    q = robot.get_q();
00062    qp = robot.get_qp();
00063    qp_prev = qp;
00064    Kpep = ColumnVector(3); Kpep = 0;
00065    Koe0Quat = ColumnVector(3); Koe0Quat = 0;
00066    v = ColumnVector(6); v = 0;
00067 
00068    if(Kp_.Nrows()==3)
00069       Kp = Kp_;
00070    else
00071    {
00072       Kp = DiagonalMatrix(3); Kp = 0.0;
00073       cerr << "Clik::Clik-->Robot, Kp if not 3x3, set gain to 0." << endl;
00074    }
00075    if(Ko_.Nrows()==3)
00076       Ko = Ko_;
00077    else
00078    {
00079       Ko = DiagonalMatrix(3); Ko = 0.0;
00080       cerr << "Clik::Clik-->Robot, Ko if not 3x3, set gain to 0." << endl;
00081    }
00082 }
00083 
00084 
00085 /*!
00086   @fn Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00087                  const Real eps_, const Real lambda_max_, const Real dt);
00088   @brief Constructor.
00089 */
00090 Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00091            const Real eps_, const Real lambda_max_, const Real dt_):
00092       dt(dt_),
00093       eps(eps_),
00094       lambda_max(lambda_max_),
00095       mrobot(mrobot_)
00096 {
00097    robot_type = CLICK_mDH;
00098    // Initialize with same joint position (and rates) has the robot.
00099    q = mrobot.get_q();
00100    qp = mrobot.get_qp();
00101    qp_prev = qp;
00102    Kpep = ColumnVector(3); Kpep = 0;
00103    Koe0Quat = ColumnVector(3); Koe0Quat = 0;
00104    v = ColumnVector(6); v = 0;
00105 
00106    if(Kp_.Nrows()==3)
00107       Kp = Kp_;
00108    else
00109    {
00110       Kp = DiagonalMatrix(3); Kp = 0.0;
00111       cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
00112    }
00113    if(Ko_.Nrows()==3)
00114       Ko = Ko_;
00115    else
00116    {
00117       Ko = DiagonalMatrix(3); Ko = 0.0;
00118       cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
00119    }
00120 }
00121 
00122 
00123 /*!
00124   @fn Clik::Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00125                  const Real eps_, const Real lambda_max_, const Real dt);
00126   @brief Constructor.
00127 */
00128 Clik::Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_,
00129            const DiagonalMatrix & Ko_, const Real eps_, const Real lambda_max_,
00130            const Real dt_):
00131       dt(dt_),
00132       eps(eps_),
00133       lambda_max(lambda_max_),
00134       mrobot_min_para(mrobot_min_para_)
00135 {
00136    robot_type = CLICK_mDH_min_para;
00137    // Initialize with same joint position (and rates) has the robot.
00138    q = mrobot_min_para.get_q();
00139    qp = mrobot_min_para.get_qp();
00140    qp_prev = qp;
00141    Kpep = ColumnVector(3); Kpep = 0;
00142    Koe0Quat = ColumnVector(3); Koe0Quat = 0;
00143    v = ColumnVector(6); v = 0;
00144 
00145    if(Kp_.Nrows()==3)
00146       Kp = Kp_;
00147    else
00148    {
00149       Kp = DiagonalMatrix(3); Kp = 0.0;
00150       cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
00151    }
00152    if(Ko_.Nrows()==3)
00153       Ko = Ko_;
00154    else
00155    {
00156       Ko = DiagonalMatrix(3); Ko = 0.0;
00157       cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
00158    }
00159 }
00160 
00161 
00162 Clik::Clik(const Clik & x)
00163 //!  @brief Copy constructor.
00164 {
00165    robot_type = x.robot_type;
00166    switch(robot_type)
00167    {
00168    case CLICK_DH:
00169       robot = x.robot;
00170       break;
00171    case CLICK_mDH:
00172       mrobot = x.mrobot;
00173       break;
00174    case CLICK_mDH_min_para:
00175       mrobot_min_para = x.mrobot_min_para;
00176       break;
00177    }
00178    eps = x.eps;
00179    lambda_max = x.lambda_max;
00180    dt = x.dt;
00181    q = x.q;
00182    qp = x.qp;
00183    qp_prev = x.qp_prev;
00184    Kpep = x.Kpep;
00185    Koe0Quat = x.Koe0Quat;
00186    Kp = x.Kp;
00187    Ko = x.Ko;
00188    v = x.v;
00189 }
00190 
00191 Clik & Clik::operator=(const Clik & x)
00192 //!  @brief Overload = operator.
00193 {
00194    robot_type = x.robot_type;
00195    switch(robot_type)
00196    {
00197    case CLICK_DH:
00198       robot = x.robot;
00199       break;
00200    case CLICK_mDH:
00201       mrobot = x.mrobot;
00202       break;
00203    case CLICK_mDH_min_para:
00204       mrobot_min_para = x.mrobot_min_para;
00205       break;
00206    }
00207    eps = x.eps;
00208    lambda_max = x.lambda_max;
00209    dt = x.dt;
00210    q = x.q;
00211    qp = x.qp;
00212    qp_prev = x.qp_prev;
00213    Kpep = x.Kpep;
00214    Koe0Quat = x.Koe0Quat;
00215    Kp = x.Kp;
00216    Ko = x.Ko;
00217    v = x.v;
00218 
00219    return *this;
00220 }
00221 
00222 
00223 int Clik::endeff_pos_ori_err(const ColumnVector & pd, const ColumnVector & /*pdd*/,
00224                              const Quaternion & qqqd, const ColumnVector & /*wd*/)
00225 /*!
00226   @brief Obtain end effector position and orientation error.
00227   @param pd: Desired eff position in base frame.
00228   @param pdd: Desired eff velocity in base frame. (unused)
00229   @param qqqd: Desired eff orientation in base frame.
00230   @param wd: Desired eff angular velocity in base frame. (unused)
00231 */
00232 {
00233    ColumnVector p;
00234    Matrix R;
00235 
00236    switch(robot_type)
00237    {
00238    case CLICK_DH:
00239       robot.set_q(q);
00240       robot.kine(R, p);  // In base frame
00241       break;
00242    case CLICK_mDH:
00243       mrobot.set_q(q);
00244       mrobot.kine(R, p);
00245       break;
00246    case CLICK_mDH_min_para:
00247       mrobot_min_para.set_q(q);
00248       mrobot_min_para.kine(R, p);
00249       break;
00250    }
00251    Kpep = Kp*(pd - p);
00252    Quaternion qq(R);
00253 
00254    Quaternion qqd;
00255 
00256    if(qq.dot_prod(qqqd) < 0)
00257       qqd = qqqd*(-1);
00258    else
00259       qqd = qqqd;
00260 
00261    // quaternion error on vectoriel part. We used equation 42 [4] instead equation 23 [1].
00262    Koe0Quat = Ko*(qq.s()*qqd.v() - qqd.s()*qq.v() + x_prod_matrix(qq.v())*qqd.v());
00263 
00264    return 0;
00265 }
00266 
00267 
00268 void Clik::q_qdot(const Quaternion & qd, const ColumnVector & pd,
00269                   const ColumnVector & pdd, const ColumnVector & wd,
00270                   ColumnVector & q_, ColumnVector & qp_)
00271 /*!
00272   @brief Obtain joints position and velocity.
00273   @param qd: Desired eff orientatio in base frame.
00274   @param pd: Desired eff position in base frame.
00275   @param pdd: Desired eff velocity in base frame.
00276   @param wd: Desired eff angular velocity in base frame.
00277   @param q_: Output joint position.
00278   @param qp_: Output joint velocity.
00279 */
00280 {
00281    v.SubMatrix(1,3,1,1) = pdd + Kpep;
00282    v.SubMatrix(4,6,1,1) = wd + Koe0Quat;
00283 
00284    switch(robot_type)
00285    {
00286    case CLICK_DH:
00287       robot.set_q(q);
00288       qp = robot.jacobian_DLS_inv(eps, lambda_max)*v;
00289       break;
00290    case CLICK_mDH:
00291       mrobot.set_q(q);
00292       qp = mrobot.jacobian_DLS_inv(eps, lambda_max)*v;
00293       break;
00294    case CLICK_mDH_min_para:
00295       mrobot_min_para.set_q(q);
00296       qp = mrobot_min_para.jacobian_DLS_inv(eps, lambda_max)*v;
00297       break;
00298    }
00299 
00300    q = q + Integ_Trap(qp, qp_prev, dt);
00301    endeff_pos_ori_err(pd, pdd, qd, wd);
00302 
00303    q_ = q;
00304    qp_ = qp;
00305 }
00306 
00307 #ifdef use_namespace
00308 }
00309 #endif

ROBOOP v1.21a
Generated Tue Jan 4 15:42:24 2005 by Doxygen 1.4.0