Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
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 /*!
00033   @file clik.cpp
00034   @brief Clik member functions.
00035 */
00036 
00037 #include "clik.h"
00038 
00039 using namespace std;
00040 
00041 #ifdef use_namespace
00042 namespace ROBOOP {
00043   using namespace NEWMAT;
00044 #endif
00045 
00046   //! @brief RCS/CVS version.
00047   static const char rcsid[] __UNUSED__ = "$Id: clik.cpp,v 1.7 2007/11/11 23:57:24 ejt Exp $";
00048   
00049 /*!
00050   @fn Clik::Clik(const Robot & robot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00051                  const Real eps_, const Real lambda_max_, const Real dt);
00052   @brief Constructor.
00053 */
00054 Clik::Clik(const Robot & robot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00055            const Real eps_, const Real lambda_max_, const Real dt_):
00056       dt(dt_),
00057       eps(eps_),
00058       lambda_max(lambda_max_),
00059       robot(robot_)
00060 {
00061    robot_type = CLICK_DH;
00062    // Initialize with same joint position (and rates) has the robot.
00063    q = robot.get_q();
00064    qp = robot.get_qp();
00065    qp_prev = qp;
00066    Kpep = ColumnVector(3); Kpep = 0;
00067    Koe0Quat = ColumnVector(3); Koe0Quat = 0;
00068    v = ColumnVector(6); v = 0;
00069 
00070    if(Kp_.Nrows()==3)
00071       Kp = Kp_;
00072    else
00073    {
00074       Kp = DiagonalMatrix(3); Kp = 0.0;
00075       cerr << "Clik::Clik-->Robot, Kp if not 3x3, set gain to 0." << endl;
00076    }
00077    if(Ko_.Nrows()==3)
00078       Ko = Ko_;
00079    else
00080    {
00081       Ko = DiagonalMatrix(3); Ko = 0.0;
00082       cerr << "Clik::Clik-->Robot, Ko if not 3x3, set gain to 0." << endl;
00083    }
00084 }
00085 
00086 
00087 /*!
00088   @fn Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00089                  const Real eps_, const Real lambda_max_, const Real dt);
00090   @brief Constructor.
00091 */
00092 Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00093            const Real eps_, const Real lambda_max_, const Real dt_):
00094       dt(dt_),
00095       eps(eps_),
00096       lambda_max(lambda_max_),
00097       mrobot(mrobot_)
00098 {
00099    robot_type = CLICK_mDH;
00100    // Initialize with same joint position (and rates) has the robot.
00101    q = mrobot.get_q();
00102    qp = mrobot.get_qp();
00103    qp_prev = qp;
00104    Kpep = ColumnVector(3); Kpep = 0;
00105    Koe0Quat = ColumnVector(3); Koe0Quat = 0;
00106    v = ColumnVector(6); v = 0;
00107 
00108    if(Kp_.Nrows()==3)
00109       Kp = Kp_;
00110    else
00111    {
00112       Kp = DiagonalMatrix(3); Kp = 0.0;
00113       cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
00114    }
00115    if(Ko_.Nrows()==3)
00116       Ko = Ko_;
00117    else
00118    {
00119       Ko = DiagonalMatrix(3); Ko = 0.0;
00120       cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
00121    }
00122 }
00123 
00124 
00125 /*!
00126   @fn Clik::Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
00127                  const Real eps_, const Real lambda_max_, const Real dt);
00128   @brief Constructor.
00129 */
00130 Clik::Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_,
00131            const DiagonalMatrix & Ko_, const Real eps_, const Real lambda_max_,
00132            const Real dt_):
00133       dt(dt_),
00134       eps(eps_),
00135       lambda_max(lambda_max_),
00136       mrobot_min_para(mrobot_min_para_)
00137 {
00138    robot_type = CLICK_mDH_min_para;
00139    // Initialize with same joint position (and rates) has the robot.
00140    q = mrobot_min_para.get_q();
00141    qp = mrobot_min_para.get_qp();
00142    qp_prev = qp;
00143    Kpep = ColumnVector(3); Kpep = 0;
00144    Koe0Quat = ColumnVector(3); Koe0Quat = 0;
00145    v = ColumnVector(6); v = 0;
00146 
00147    if(Kp_.Nrows()==3)
00148       Kp = Kp_;
00149    else
00150    {
00151       Kp = DiagonalMatrix(3); Kp = 0.0;
00152       cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
00153    }
00154    if(Ko_.Nrows()==3)
00155       Ko = Ko_;
00156    else
00157    {
00158       Ko = DiagonalMatrix(3); Ko = 0.0;
00159       cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
00160    }
00161 }
00162 
00163 
00164 Clik::Clik(const Clik & x)
00165 //!  @brief Copy constructor.
00166 {
00167    robot_type = x.robot_type;
00168    switch(robot_type)
00169    {
00170    case CLICK_DH:
00171       robot = x.robot;
00172       break;
00173    case CLICK_mDH:
00174       mrobot = x.mrobot;
00175       break;
00176    case CLICK_mDH_min_para:
00177       mrobot_min_para = x.mrobot_min_para;
00178       break;
00179    }
00180    eps = x.eps;
00181    lambda_max = x.lambda_max;
00182    dt = x.dt;
00183    q = x.q;
00184    qp = x.qp;
00185    qp_prev = x.qp_prev;
00186    Kpep = x.Kpep;
00187    Koe0Quat = x.Koe0Quat;
00188    Kp = x.Kp;
00189    Ko = x.Ko;
00190    v = x.v;
00191 }
00192 
00193 Clik & Clik::operator=(const Clik & x)
00194 //!  @brief Overload = operator.
00195 {
00196    robot_type = x.robot_type;
00197    switch(robot_type)
00198    {
00199    case CLICK_DH:
00200       robot = x.robot;
00201       break;
00202    case CLICK_mDH:
00203       mrobot = x.mrobot;
00204       break;
00205    case CLICK_mDH_min_para:
00206       mrobot_min_para = x.mrobot_min_para;
00207       break;
00208    }
00209    eps = x.eps;
00210    lambda_max = x.lambda_max;
00211    dt = x.dt;
00212    q = x.q;
00213    qp = x.qp;
00214    qp_prev = x.qp_prev;
00215    Kpep = x.Kpep;
00216    Koe0Quat = x.Koe0Quat;
00217    Kp = x.Kp;
00218    Ko = x.Ko;
00219    v = x.v;
00220 
00221    return *this;
00222 }
00223 
00224 
00225 int Clik::endeff_pos_ori_err(const ColumnVector & pd, const ColumnVector & /*pdd*/,
00226                              const Quaternion & qqqd, const ColumnVector & /*wd*/)
00227 /*!
00228   @brief Obtain end effector position and orientation error.
00229   @param pd: Desired eff position in base frame.
00230   @param pdd: Desired eff velocity in base frame. (unused)
00231   @param qqqd: Desired eff orientation in base frame.
00232   @param wd: Desired eff angular velocity in base frame. (unused)
00233 */
00234 {
00235    ColumnVector p;
00236    Matrix R;
00237 
00238    switch(robot_type)
00239    {
00240    case CLICK_DH:
00241       robot.set_q(q);
00242       robot.kine(R, p);  // In base frame
00243       break;
00244    case CLICK_mDH:
00245       mrobot.set_q(q);
00246       mrobot.kine(R, p);
00247       break;
00248    case CLICK_mDH_min_para:
00249       mrobot_min_para.set_q(q);
00250       mrobot_min_para.kine(R, p);
00251       break;
00252    }
00253    Kpep = Kp*(pd - p);
00254    Quaternion qq(R);
00255 
00256    Quaternion qqd;
00257 
00258    if(qq.dot_prod(qqqd) < 0)
00259       qqd = qqqd*(-1);
00260    else
00261       qqd = qqqd;
00262 
00263    // quaternion error on vectoriel part. We used equation 42 [4] instead equation 23 [1].
00264    Koe0Quat = Ko*(qq.s()*qqd.v() - qqd.s()*qq.v() + x_prod_matrix(qq.v())*qqd.v());
00265 
00266    return 0;
00267 }
00268 
00269 
00270 void Clik::q_qdot(const Quaternion & qd, const ColumnVector & pd,
00271                   const ColumnVector & pdd, const ColumnVector & wd,
00272                   ColumnVector & q_, ColumnVector & qp_)
00273 /*!
00274   @brief Obtain joints position and velocity.
00275   @param qd: Desired eff orientatio in base frame.
00276   @param pd: Desired eff position in base frame.
00277   @param pdd: Desired eff velocity in base frame.
00278   @param wd: Desired eff angular velocity in base frame.
00279   @param q_: Output joint position.
00280   @param qp_: Output joint velocity.
00281 */
00282 {
00283    v.SubMatrix(1,3,1,1) = pdd + Kpep;
00284    v.SubMatrix(4,6,1,1) = wd + Koe0Quat;
00285 
00286    switch(robot_type)
00287    {
00288    case CLICK_DH:
00289       robot.set_q(q);
00290       qp = robot.jacobian_DLS_inv(eps, lambda_max)*v;
00291       break;
00292    case CLICK_mDH:
00293       mrobot.set_q(q);
00294       qp = mrobot.jacobian_DLS_inv(eps, lambda_max)*v;
00295       break;
00296    case CLICK_mDH_min_para:
00297       mrobot_min_para.set_q(q);
00298       qp = mrobot_min_para.jacobian_DLS_inv(eps, lambda_max)*v;
00299       break;
00300    }
00301 
00302    q = q + Integ_Trap(qp, qp_prev, dt);
00303    endeff_pos_ori_err(pd, pdd, qd, wd);
00304 
00305    q_ = q;
00306    qp_ = qp;
00307 }
00308 
00309 #ifdef use_namespace
00310 }
00311 #endif

ROBOOP v1.21a
Generated Thu Nov 22 00:51:28 2007 by Doxygen 1.5.4