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

ROBOOP v1.21a
Generated Tue Aug 16 16:32:14 2005 by Doxygen 1.4.4