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

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