Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

controller.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 
00024 /*!
00025   @file controller.cpp
00026   @brief Differents controllers class.
00027 */
00028 
00029 #include "controller.h"
00030 
00031 using namespace std;
00032 
00033 #ifdef use_namespace
00034 namespace ROBOOP {
00035   using namespace NEWMAT;
00036 #endif
00037   //! @brief RCS/CVS version.
00038   static const char rcsid[] __UNUSED__ = "$Id: controller.cpp,v 1.5 2007/11/11 23:57:24 ejt Exp $";
00039 
00040 Impedance::Impedance()
00041 //! @brief Constructor.
00042 {
00043    pc = ColumnVector(3); pc = 0;
00044    pcp = pc;
00045    pcpp = pc;
00046    pcpp_prev = pc;
00047    qc = Quaternion();
00048    qcp = qc;
00049    qcp_prev = qc;
00050    quat = qc;
00051    wc = pc;
00052    wcp = pc;
00053 }
00054 
00055 Impedance::Impedance(const Robot_basic & robot, const DiagonalMatrix & Mp_,
00056                      const DiagonalMatrix & Dp_, const DiagonalMatrix & Kp_,
00057                      const DiagonalMatrix & Mo_, const DiagonalMatrix & Do_, 
00058          const DiagonalMatrix & Ko_)
00059 //! @brief Constructor.
00060 {
00061    set_Mp(Mp_);
00062    set_Dp(Dp_);
00063    set_Kp(Kp_);
00064    set_Mo(Mo_);
00065    set_Do(Do_);
00066    set_Ko(Ko_);
00067 
00068    pc = ColumnVector(3); pc = 0;
00069    pcp = pc;
00070    pcp_prev = pc;
00071    pcpp = pc;
00072    pcpp_prev = pc;
00073    qc = Quaternion();
00074    qcp = qc;
00075    qcp_prev = qc;
00076    quat = qc;
00077    wc = pc;
00078    wcp = pc;
00079    wcp_prev = pc;
00080 
00081    Matrix Rot;
00082    robot.kine(Rot, pc);
00083    qc = Quaternion(Rot);
00084 }
00085 
00086 Impedance::Impedance(const Impedance & x)
00087 //! @brief Copy constructor.
00088 {
00089    Mp = x.Mp;
00090    Dp = x.Dp;
00091    Kp = x.Kp;
00092    Mo = x.Mo;
00093    Do = x.Do;
00094    Ko = x.Ko;
00095    Ko_prime = x.Ko_prime;
00096    pc = x.pc;
00097    pcp = x.pcp;
00098    pcp_prev = x.pcp_prev;
00099    pcpp = x.pcpp;
00100    pcpp_prev = x.pcpp_prev;
00101    qc = x.qc;
00102    qcp = x.qcp;
00103    qcp_prev = x.qcp_prev;
00104    quat = x.quat;
00105    wc = x.wc;
00106    wcp = x.wcp;
00107    wcp_prev = x.wcp_prev;
00108 }
00109 
00110 Impedance & Impedance::operator=(const Impedance & x)
00111 //! @brief Overload = operator.
00112 {
00113    Mp = x.Mp;
00114    Dp = x.Dp;
00115    Kp = x.Kp;
00116    Mo = x.Mo;
00117    Do = x.Do;
00118    Ko = x.Ko;
00119    Ko_prime = x.Ko_prime;
00120    pc = x.pc;
00121    pcp = x.pcp;
00122    pcp_prev = x.pcp_prev;
00123    pcpp = x.pcpp;
00124    pcpp_prev = x.pcpp_prev;
00125    qc = x.qc;
00126    qcp = x.qcp;
00127    qcp_prev = x.qcp_prev;
00128    quat = x.quat;
00129    wc = x.wc;
00130    wcp = x.wcp;
00131    wcp_prev = x.wcp_prev;
00132 
00133    return *this;
00134 }
00135 
00136 short Impedance::set_Mp(const DiagonalMatrix & Mp_)
00137 /*!
00138   @brief Assign the translational impedance inertia matrix \f$M_p\f$.
00139   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00140 */
00141 {
00142    if(Mp_.Nrows() != 3)
00143    {
00144       Mp = DiagonalMatrix(3); Mp = 1;
00145       cerr << "Impedance::set_Mp: wrong size for input matrix Mp" << endl;
00146       return WRONG_SIZE;
00147    }
00148 
00149    Mp = Mp_;
00150    return 0;
00151 }
00152 
00153 short Impedance::set_Mp(const Real Mp_i, const short i)
00154 /*!
00155   @brief Assign the translational impedance inertia term \f$M_p(i,i)\f$.
00156   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00157 */
00158 {
00159    if( (i < 0) || (i > 3) )
00160    {
00161       cerr << "Impedance::set_Mp: index i out of bound" << endl;
00162       return WRONG_SIZE;
00163    }
00164 
00165    Mp(i) = Mp_i;
00166    return 0;
00167 }
00168 
00169 short Impedance::set_Dp(const DiagonalMatrix & Dp_)
00170 /*!
00171   @brief Assign the translational impedance damping matrix \f$D_p\f$.
00172   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00173 */
00174 {
00175    if(Dp_.Nrows() != 3)
00176    {
00177       Dp = DiagonalMatrix(3); Dp = 1;
00178       cerr << "Impedance::set_Dp: input matrix Dp of wrong size" << endl;
00179       return WRONG_SIZE;
00180    }
00181 
00182    Dp = Dp_;
00183    return 0;
00184 }
00185 
00186 short Impedance::set_Dp(const Real Dp_i, const short i)
00187 /*!
00188   @brief Assign the translational impedance damping term \f$D_p(i,i)\f$.
00189   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00190 */
00191 {
00192    if( (i < 0) || (i > 3) )
00193    {
00194       cerr << "Impedance::set_Dp: index i out of bound" << endl;
00195       return WRONG_SIZE;
00196    }
00197 
00198    Dp(i) = Dp_i;
00199    return 0;
00200 }
00201 
00202 short Impedance::set_Kp(const DiagonalMatrix & Kp_)
00203 /*!
00204   @brief Assign the translational impedance stifness matrix \f$K_p\f$.
00205   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00206 */
00207 {
00208    if(Kp_.Nrows() != 3)
00209    {
00210       Kp = DiagonalMatrix(3); Kp = 1;
00211       cerr << "Impedance::set_Kp: wrong size for input matrix Kp" << endl;
00212       return WRONG_SIZE;
00213    }
00214 
00215    Kp = Kp_;
00216    return 0;
00217 }
00218 
00219 short Impedance::set_Kp(const Real Kp_i, const short i)
00220 /*!
00221   @brief Assign the translational impedance stifness term \f$K_p(i,i)\f$.
00222   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00223 */
00224 {
00225    if( (i < 0) || (i > 3) )
00226    {
00227       cerr << "Impedance::set_Mp: index i out of bound" << endl;
00228       return WRONG_SIZE;
00229    }
00230 
00231    Kp(i) = Kp_i;
00232    return 0;
00233 }
00234 
00235 short Impedance::set_Mo(const DiagonalMatrix & Mo_)
00236 /*!
00237   @brief Assign the rotational impedance inertia matrix \f$M_o\f$.
00238   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00239 */
00240 {
00241    if(Mo_.Nrows() != 3)
00242    {
00243       Mo = DiagonalMatrix(3); Mo = 1;
00244       cerr << "Impedance::set_Mo: wrong size  input matrix Mo" << endl;
00245       return WRONG_SIZE;
00246    }
00247 
00248    Mo = Mo_;
00249    return 0;
00250 }
00251 
00252 short Impedance::set_Mo(const Real Mo_i, const short i)
00253 /*!
00254   @brief Assign the rotational impedance inertia term \f$M_o(i,i)\f$.
00255   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00256 */
00257 {
00258    if( (i < 0) || (i > 3) )
00259    {
00260       cerr << "Impedance::set_Mo: index i out of bound" << endl;
00261       return WRONG_SIZE;
00262    }
00263 
00264    Mo(i) = Mo_i;
00265    return 0;
00266 }
00267 
00268 short Impedance::set_Do(const DiagonalMatrix & Do_)
00269 /*!
00270   @brief Assign the rotational impedance damping matrix \f$D_o\f$.
00271   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00272 */
00273 {
00274    if( Do_.Nrows() != 3)
00275    {
00276       Do = DiagonalMatrix(3); Do = 1;
00277       cerr << "Impedance::set_Do: wrong size  input matrix Do" << endl;
00278       return WRONG_SIZE;
00279    }
00280 
00281    Do = Do_;
00282    return 0;
00283 }
00284 
00285 short Impedance::set_Do(const Real Do_i, const short i)
00286 /*!
00287   @brief Assign the rotational impedance damping term \f$D_o(i,i)\f$.
00288   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00289 */
00290 {
00291    if( (i < 0) || (i > 3) )
00292    {
00293       cerr << "Impedance::set_Do: index i out of bound" << endl;
00294       return WRONG_SIZE;
00295    }
00296 
00297    Do(i) = Do_i;
00298    return 0;
00299 }
00300 
00301 short Impedance::set_Ko(const DiagonalMatrix & Ko_)
00302 /*!
00303   @brief Assign the rotational impedance stifness matrix \f$K_o\f$.
00304   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00305 */
00306 {
00307    if(Ko_.Nrows() != 3)
00308    {
00309       Ko = DiagonalMatrix(3); Ko = 1;
00310       cerr << "Impedance::set_Ko: wrong size for  input matrix Ko" << endl;
00311       return WRONG_SIZE;
00312    }
00313 
00314    Ko = Ko_;
00315    return 0;
00316 }
00317 
00318 short Impedance::set_Ko(const Real Ko_i, const short i)
00319 /*!
00320   @brief Assign the rotational impedance stifness term \f$K_o(i,i)\f$.
00321   @return short: 0 or WRONG_SIZE if the matrix is not \f$3\times 3\f$.
00322 */
00323 {
00324    if( (i < 0) || (i > 3) )
00325    {
00326       cerr << "Impedance::set_Ko: index i out of bound" << endl;
00327       return WRONG_SIZE;
00328    }
00329 
00330    Ko(i) = Ko_i;
00331    return 0;
00332 }
00333 
00334 short Impedance::control(const ColumnVector & pdpp, const ColumnVector & pdp,
00335                          const ColumnVector & pd, const ColumnVector & wdp,
00336                          const ColumnVector & wd, const Quaternion & qd,
00337                          const ColumnVector & f, const ColumnVector & n,
00338                          const Real dt)
00339 /*!
00340   @brief Generation of a compliance trajectory
00341   @param pdpp: desired end effector acceleration.
00342   @param pdp: desired end effector velocity.
00343   @param pd: desired end effector position.
00344   @param wdp: desired end effector angular acceleration.
00345   @param wd: desired end effector angular velocity.
00346   @param qd: desired quaternion.
00347   @param f: end effector contact force.
00348   @param n: end effector contact moment.
00349   @param dt: time frame.
00350 
00351   @return short: 0 or WRONG_SIZE if one of the vector input is not \f$3\times 1\f$.
00352 
00353   The translational and rotational impedance equations are integrated, with input 
00354   \f$f\f$ and \f$n\f$ to computed \f$\ddot{p}_c\f$ and \f$\dot{\omega}_c\f$, 
00355   \f$\dot{p}_c\f$ and \f$\omega_c\f$, and then \f$p_c\f$ and \f$q_c\f$. The compliant
00356   quaternion \f$q_c\f$ is obtained with the quaternion propagation equations (see 
00357   Quaternion class).
00358 
00359   The quaternion -q represents exactly the same rotation as the quaternion q. The 
00360   temporay quaternion, quat, is quatd plus a sign correction. It is customary to 
00361   choose the sign G on q1 so that q0.Gq1 >=0 (the angle between q0 ang Gq1 is acute). 
00362   This choice avoids extra spinning caused by the interpolated rotations.
00363 */
00364 {
00365    if(pdpp.Nrows() !=3)
00366    {
00367       cerr << "Impedance::control: wrong size for input vector pdpp" << endl;
00368       return WRONG_SIZE;
00369    }
00370    if(pdp.Nrows() !=3)
00371    {
00372       cerr << "Impedance::control: wrong size for input vector pdp" << endl;
00373       return WRONG_SIZE;
00374    }
00375    if(pd.Nrows() != 3)
00376    {
00377       cerr << "Impedance::control: wrong size for input vector pd" << endl;
00378       return WRONG_SIZE;
00379    }
00380    if(wdp.Nrows() !=3)
00381    {
00382       cerr << "Impedance::control: wrong size for input vector wdp" << endl;
00383       return WRONG_SIZE;
00384    }
00385    if(wd.Nrows() !=3)
00386    {
00387       cerr << "Impedance::control: wrong size for input vector wd" << endl;
00388       return WRONG_SIZE;
00389    }
00390    if(f.Nrows() !=3)
00391    {
00392       cerr << "Impedance::control: wrong size for input vector f" << endl;
00393       return WRONG_SIZE;
00394    }
00395    if(n.Nrows() !=3)
00396    {
00397       cerr << "Impedance::control: wrong size for input vector f" << endl;
00398       return WRONG_SIZE;
00399    }
00400 
00401    static bool first=true;
00402    if(first)
00403    {
00404       qc = qd;
00405       qcp = qc.dot(wc, BASE_FRAME);
00406       qcp_prev = qcp;
00407       wc = wd;
00408       wcp = wdp;
00409       first = false;
00410    }
00411    if(qc.dot_prod(qd) < 0)
00412       quat = qd*(-1);
00413    else
00414       quat = qd;
00415 
00416    qcd = quat * qc.i();
00417 
00418    // Solving pcpp, pcp, pc with the translational impedance
00419    pcd = pc - pd;
00420    pcdp = pcp - pdp;
00421    //  pcpp = pdpp + Mp.i()*(f - Dp*pcdp - Kp*pcd - 2*qcd.s()*Km.t()*qcd.v()); // (21)
00422    pcpp = pdpp + Mp.i()*(f - Dp*pcdp - Kp*pcd);
00423    pcp = pcp_prev + Integ_Trap(pcpp, pcpp_prev, dt);
00424    pc = pc + Integ_Trap(pcp, pcp_prev, dt);
00425 
00426    // Solving wcp, wc, qc with the rotational impedance
00427    wcd = wc - wd;
00428    Ko_prime = 2*qcd.E(BASE_FRAME)*Ko;                                   //(23)
00429    //  Km_prime = (qcd.s()*qcd.E(BASE_FRAME) - qcd.v()*qcd.v().t())*Km;   // (24)
00430    //  wcp = wdp + Mo.i()*(n - Do*wcd - Ko_prime*qcd.v() - Km_prime*pcd); // (22)
00431    wcp = wdp + Mo.i()*(n - Do*wcd - Ko_prime*qcd.v()); // (22)
00432    wc = wc + Integ_Trap(wcp, wcp_prev, dt);
00433    qcp = qc.dot(wc, BASE_FRAME);
00434    Integ_quat(qcp, qcp_prev, qc, dt);
00435 
00436    return 0;
00437 }
00438 
00439 // ------------------------------------------------------------------------------------------------------
00440 //  Position control based on resolved acceleration.
00441 // ------------------------------------------------------------------------------------------------------
00442 
00443 Resolved_acc::Resolved_acc(const short dof)
00444 //! @brief Constructor.
00445 {
00446    Kvp = Kpp = Kvo = Kpo = 0;
00447    zero3 = ColumnVector(3); zero3 = 0;
00448    p = zero3;
00449    pp = zero3;
00450 
00451    if(dof>0)
00452    {
00453        qp = ColumnVector(dof); qp = 0;
00454        qpp = qp;
00455    }
00456 
00457    quat_v_error = zero3;
00458    a = ColumnVector(6); a = 0;
00459    quat = Quaternion();
00460 }
00461 
00462 Resolved_acc::Resolved_acc(const Robot_basic & robot,
00463                            const Real Kvp_,
00464                            const Real Kpp_,
00465                            const Real Kvo_,
00466                            const Real Kpo_)
00467 //! @brief Constructor.
00468 {
00469    set_Kvp(Kvp_);
00470    set_Kpp(Kpp_);
00471    set_Kvo(Kvo_);
00472    set_Kpo(Kpo_);
00473    zero3 = ColumnVector(3); zero3 = 0;
00474    qp = ColumnVector(robot.get_dof()); qp = 0;
00475    qpp = qp;
00476    a = ColumnVector(6); a = 0;
00477    p = zero3;
00478    pp = zero3;
00479    quat_v_error = zero3;
00480    quat = Quaternion();
00481 }
00482 
00483 Resolved_acc::Resolved_acc(const Resolved_acc & x)
00484 //! @brief Copy constructor.
00485 {
00486    Kvp = x.Kvp;
00487    Kpp = x.Kpp;
00488    Kvo = x.Kvo;
00489    Kpo = x.Kpo;
00490    zero3 = x.zero3;
00491    qp = x.qp;
00492    qpp = x.qpp;
00493    a = x.a;
00494    p = x.p;
00495    pp = x.pp;
00496    quat_v_error = x.quat_v_error;
00497    quat = x.quat;
00498 }
00499 
00500 Resolved_acc & Resolved_acc::operator=(const Resolved_acc & x)
00501 //! @brief Overload = operator.
00502 {
00503    Kvp = x.Kvp;
00504    Kpp = x.Kpp;
00505    Kvo = x.Kvo;
00506    Kpo = x.Kpo;
00507    zero3 = x.zero3;
00508    qp = x.qp;
00509    qpp = x.qpp;
00510    a = x.a;
00511    p = x.p;
00512    pp = x.pp;
00513    quat_v_error = x.quat_v_error;
00514    quat = x.quat;
00515 
00516    return *this;
00517 }
00518 
00519 void Resolved_acc::set_Kvp(const Real Kvp_)
00520 //! @brief Assign the gain \f$k_{vp}\f$.
00521 {
00522    Kvp = Kvp_;
00523 }
00524 
00525 void Resolved_acc::set_Kpp(const Real Kpp_)
00526 //! @brief Assign the gain \f$k_{pp}\f$.
00527 {
00528    Kpp = Kpp_;
00529 }
00530 
00531 void Resolved_acc::set_Kvo(const Real Kvo_)
00532 //! @brief Assign the gain \f$k_{vo}\f$.
00533 {
00534    Kvo = Kvo_;
00535 }
00536 
00537 void Resolved_acc::set_Kpo(const Real Kpo_)
00538 //! @brief Assign the gain \f$k_{po}\f$.
00539 {
00540    Kpo = Kpo_;
00541 }
00542 
00543 ReturnMatrix Resolved_acc::torque_cmd(Robot_basic & robot, const ColumnVector & pdpp,
00544                                       const ColumnVector & pdp, const ColumnVector & pd,
00545                                       const ColumnVector & wdp, const ColumnVector & wd,
00546                                       const Quaternion & quatd, const short link_pc,
00547                                       const Real /*dt*/)
00548 /*!
00549   @brief Output torque.
00550 
00551   For more robustess the damped least squares inverse Jacobian is used instead of
00552   the inverse Jacobian.
00553 
00554   The quaternion -q represents exactly the same rotation as the quaternion q. The 
00555   temporay quaternion, quat, is quatd plus a sign correction. It is customary to 
00556   choose the sign G on q1 so that q0.Gq1 >=0 (the angle between q0 ang Gq1 is acute). 
00557   This choice avoids extra spinning caused by the interpolated rotations.
00558 */
00559 {
00560    robot.kine_pd(Rot, p, pp, link_pc);
00561 
00562    Quaternion quate(Rot); // end effector orientation
00563    if(quate.dot_prod(quatd) < 0)
00564       quat = quatd*(-1);
00565    else
00566       quat = quatd;
00567 
00568    quat_v_error = quate.s()*quat.v() - quat.s()*quate.v() +
00569                   x_prod_matrix(quate.v())*quat.v();
00570 
00571    a.SubMatrix(1,3,1,1) = pdpp + Kvp*(pdp-pp) + Kpp*(pd-p);
00572    a.SubMatrix(4,6,1,1) = wdp + Kvo*(wd-Rot*robot.w[robot.get_dof()]) +
00573                           Kpo*quat_v_error;
00574    qp = robot.get_qp();
00575    //                          (eps, lamda_max)
00576    qpp = robot.jacobian_DLS_inv(0.015, 0.2)*(a - robot.jacobian_dot()*qp);
00577    return robot.torque(robot.get_q(),qp, qpp, zero3, zero3);
00578 }
00579 
00580 
00581 // ------------------------------------------------------------------------------
00582 //  Position control based on Computed Torque Method
00583 // ------------------------------------------------------------------------------
00584 
00585 Computed_torque_method::Computed_torque_method(const short dof_)
00586 //! @brief Constructor.
00587 {
00588     dof = dof_;
00589     qpp = ColumnVector(dof); qpp = 0;
00590     q = qpp;
00591     qp = qpp;
00592     zero3 = ColumnVector(3); zero3 = 0;
00593 }
00594 
00595 Computed_torque_method::Computed_torque_method(const Robot_basic & robot,
00596                  const DiagonalMatrix & Kp,
00597                  const DiagonalMatrix & Kd)
00598 //! @brief Constructor.
00599 {
00600    dof = robot.get_dof();
00601    set_Kd(Kd);
00602    set_Kp(Kp);
00603    qpp = ColumnVector(dof); qpp = 0;
00604    q = qpp;
00605    qp = qpp;
00606    zero3 = ColumnVector(3); zero3 = 0;
00607 }
00608 
00609 Computed_torque_method::Computed_torque_method(const Computed_torque_method & x)
00610 //! @brief Copy constructor.
00611 {
00612    dof = x.dof;
00613    Kd = x.Kd;
00614    Kp = x.Kp;
00615    qpp = x.qpp;
00616    q = x.q;
00617    qp = x.qp;
00618    zero3 = x.zero3;
00619 }
00620 
00621 Computed_torque_method & Computed_torque_method::operator=(const Computed_torque_method & x)
00622 //! @brief Overload = operator.
00623 {
00624    dof = x.dof;
00625    Kd = x.Kd;
00626    Kp = x.Kp;
00627    qpp = x.qpp;
00628    q = x.q;
00629    qp = x.qp;
00630    zero3 = x.zero3;
00631 
00632    return *this;
00633 }
00634 
00635 ReturnMatrix Computed_torque_method::torque_cmd(Robot_basic & robot, const ColumnVector & qd,
00636             const ColumnVector & qpd)
00637 //! @brief Output torque.
00638 {
00639    if(qd.Nrows() != dof)
00640    {
00641       ColumnVector tau(dof); tau = 0;
00642       cerr << "Computed_torque_methode::torque_cmd: wrong size for input vector qd" << endl;
00643       tau.Release();
00644       return tau;
00645    }
00646    if(qpd.Nrows() != dof)
00647    {
00648       ColumnVector tau(dof); tau = 0;
00649       cerr << "Computed_torque_methode::torque_cmd: wrong size for input vector qpd" << endl;
00650       tau.Release();
00651       return tau;
00652    }
00653 
00654    q = robot.get_q();
00655    qp = robot.get_qp();
00656    qpp = Kp*(qd-q) + Kd*(qpd-qp);
00657    return robot.torque(q, qp, qpp, zero3, zero3);
00658 }
00659 
00660 short Computed_torque_method::set_Kd(const DiagonalMatrix & Kd_)
00661 /*!
00662   @brief Assign the velocity error gain matrix \f$K_d(i,i)\f$.
00663   @return short: 0 or WRONG_SIZE if the matrix is not \f$dof \times dof\f$.
00664 */
00665 {
00666    if(Kd_.Nrows() != dof)
00667    {
00668       Kd = DiagonalMatrix(dof); Kd = 0;
00669       cerr << "Computed_torque_method::set_kd: wrong size for input matrix Kd." << endl;
00670       return WRONG_SIZE;
00671    }
00672 
00673    Kd = Kd_;
00674    return 0;
00675 }
00676 
00677 short Computed_torque_method::set_Kp(const DiagonalMatrix & Kp_)
00678 /*!
00679   @brief Assign the position error gain matrix \f$K_p(i,i)\f$.
00680   @return short: 0 or WRONG_SIZE if the matrix is not \f$dof \times dof\f$.
00681 */
00682 {
00683    if(Kp_.Nrows() != dof)
00684    {
00685       Kp = DiagonalMatrix(dof); Kp = 0;
00686       cerr << "Computed_torque_method::set_kp: wrong size for input matrix Kp." << endl;
00687       return WRONG_SIZE;
00688    }
00689 
00690    Kp = Kp_;
00691    return 0;
00692 }
00693 
00694 // ------------------------------------------------------------------------------
00695 //  Position control based on Proportional_Derivative (PD)
00696 // ------------------------------------------------------------------------------
00697 
00698 Proportional_Derivative::Proportional_Derivative(const short dof_)
00699 //! @brief Constructor.
00700 {
00701   dof = dof_;
00702   q = ColumnVector(dof); q = 0;
00703   qp = q;
00704   zero3 = ColumnVector(3); zero3 = 0;
00705 }
00706 
00707 Proportional_Derivative::Proportional_Derivative(const Robot_basic & robot, 
00708              const DiagonalMatrix & Kp, 
00709              const DiagonalMatrix & Kd)
00710 //! @brief Constructor.
00711 {
00712    dof = robot.get_dof();
00713    set_Kp(Kp);
00714    set_Kd(Kd);
00715    q = ColumnVector(dof); q = 0;
00716    qp = q;
00717    tau = ColumnVector(dof); tau = 0;
00718    zero3 = ColumnVector(3); zero3 = 0;
00719 }
00720 
00721 Proportional_Derivative::Proportional_Derivative(const Proportional_Derivative & x)
00722 //! @brief Copy constructor.
00723 {
00724    dof = x.dof;
00725    Kp = x.Kp;
00726    Kd = x.Kd;
00727    q = x.q;
00728    qp = x.qp;
00729    tau = x.tau;
00730    zero3 = x.zero3;
00731 }
00732 
00733 Proportional_Derivative & Proportional_Derivative::operator=(const Proportional_Derivative & x)
00734 //! @brief Overload = operator.
00735 {
00736    dof = x.dof;
00737    Kp = x.Kp;
00738    Kd = x.Kd;
00739    q = x.q;
00740    qp = x.qp;
00741    tau = x.tau;
00742    zero3 = x.zero3;
00743 
00744    return *this;
00745 }
00746 
00747 ReturnMatrix Proportional_Derivative::torque_cmd(Robot_basic & robot, 
00748              const ColumnVector & qd,
00749              const ColumnVector & qpd)
00750 //! @brief Output torque.
00751 {
00752    if(qd.Nrows() != dof)
00753    {
00754       tau = 0;
00755       cerr << "Proportional_Derivative::torque_cmd: wrong size for input vector qd" << endl;
00756       return tau;
00757    }
00758    if(qpd.Nrows() != dof)
00759    {
00760       tau = 0;
00761       cerr << "Proportional_Derivative::torque_cmd: wrong size for input vector qpd" << endl;
00762       return tau;
00763    }
00764 
00765    q = robot.get_q();
00766    qp = robot.get_qp();
00767    tau = Kp*(qd-q) + Kd*(qpd-qp);
00768 
00769    return tau;
00770 }
00771 
00772 short Proportional_Derivative::set_Kd(const DiagonalMatrix & Kd_)
00773 /*!
00774   @brief Assign the velocity error gain matrix \f$K_p(i,i)\f$.
00775   @return short: 0 or WRONG_SIZE if the matrix is not \f$dof \times dof\f$.
00776 */
00777 {
00778    if(Kd_.Nrows() != dof)
00779    {
00780       Kd = DiagonalMatrix(dof); Kd = 0;
00781       cerr << "Proportional_Derivative::set_kd: wrong size for input matrix Kd." << endl;
00782       return WRONG_SIZE;
00783    }
00784 
00785    Kd = Kd_;
00786    return 0;
00787 }
00788 
00789 short Proportional_Derivative::set_Kp(const DiagonalMatrix & Kp_)
00790 /*!
00791   @brief Assign the position error gain matrix \f$K_p(i,i)\f$.
00792   @return short: 0 or WRONG_SIZE if the matrix is not \f$dof \times dof\f$.
00793 */
00794 {
00795    if(Kp_.Nrows() != dof)
00796    {
00797       Kp = DiagonalMatrix(dof); Kp = 0;
00798       cerr << "Computed_torque_method::set_kp: wrong size for input matrix Kp." << endl;
00799       return WRONG_SIZE;
00800    }
00801 
00802    Kp = Kp_;
00803    return 0;
00804 }
00805 
00806 #ifdef use_namespace
00807 }
00808 #endif

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