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


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32