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 Revision_history:
00025 
00026 2005/06/10: Etienne Lachance
00027     -The desired joint acceleration was missing in the computed torque method.
00028 
00029 2005/11/06: Etienne Lachance
00030     - No need to provide a copy constructor and the assignment operator 
00031       (operator=) for Impedance, Resolved_acc, Computed_torque_method and
00032       Proportional_Derivative classes. Instead we use the one provide by the
00033       compiler.
00034 -------------------------------------------------------------------------------
00035 */
00036 
00042 
00043 static const char rcsid[] = "$Id: controller.cpp,v 1.3 2005/11/15 19:06:13 gourdeau Exp $";
00044 
00045 
00046 #include "controller.h"
00047 
00048 #ifdef use_namespace
00049 namespace ROBOOP {
00050   using namespace NEWMAT;
00051 #endif
00052 
00053 
00054 Impedance::Impedance()
00056 {
00057    pc = ColumnVector(3); pc = 0;
00058    pcp = pc;
00059    pcpp = pc;
00060    pcpp_prev = pc;
00061    qc = Quaternion();
00062    qcp = qc;
00063    qcp_prev = qc;
00064    quat = qc;
00065    wc = pc;
00066    wcp = pc;
00067 }
00068 
00069 Impedance::Impedance(const Robot_basic & robot, const DiagonalMatrix & Mp_,
00070                      const DiagonalMatrix & Dp_, const DiagonalMatrix & Kp_,
00071                      const DiagonalMatrix & Mo_, const DiagonalMatrix & Do_, 
00072                      const DiagonalMatrix & Ko_)
00074 {
00075    set_Mp(Mp_);
00076    set_Dp(Dp_);
00077    set_Kp(Kp_);
00078    set_Mo(Mo_);
00079    set_Do(Do_);
00080    set_Ko(Ko_);
00081 
00082    pc = ColumnVector(3); pc = 0;
00083    pcp = pc;
00084    pcp_prev = pc;
00085    pcpp = pc;
00086    pcpp_prev = pc;
00087    qc = Quaternion();
00088    qcp = qc;
00089    qcp_prev = qc;
00090    quat = qc;
00091    wc = pc;
00092    wcp = pc;
00093    wcp_prev = pc;
00094 
00095    Matrix Rot;
00096    robot.kine(Rot, pc);
00097    qc = Quaternion(Rot);
00098 }
00099 
00100 short Impedance::set_Mp(const DiagonalMatrix & Mp_)
00105 {
00106    if(Mp_.Nrows() != 3)
00107    {
00108       Mp = DiagonalMatrix(3); Mp = 1;
00109       cerr << "Impedance::set_Mp: wrong size for input matrix Mp" << endl;
00110       return WRONG_SIZE;
00111    }
00112 
00113    Mp = Mp_;
00114    return 0;
00115 }
00116 
00117 short Impedance::set_Mp(const Real Mp_i, const short i)
00122 {
00123    if( (i < 0) || (i > 3) )
00124    {
00125       cerr << "Impedance::set_Mp: index i out of bound" << endl;
00126       return WRONG_SIZE;
00127    }
00128 
00129    Mp(i) = Mp_i;
00130    return 0;
00131 }
00132 
00133 short Impedance::set_Dp(const DiagonalMatrix & Dp_)
00138 {
00139    if(Dp_.Nrows() != 3)
00140    {
00141       Dp = DiagonalMatrix(3); Dp = 1;
00142       cerr << "Impedance::set_Dp: input matrix Dp of wrong size" << endl;
00143       return WRONG_SIZE;
00144    }
00145 
00146    Dp = Dp_;
00147    return 0;
00148 }
00149 
00150 short Impedance::set_Dp(const Real Dp_i, const short i)
00155 {
00156    if( (i < 0) || (i > 3) )
00157    {
00158       cerr << "Impedance::set_Dp: index i out of bound" << endl;
00159       return WRONG_SIZE;
00160    }
00161 
00162    Dp(i) = Dp_i;
00163    return 0;
00164 }
00165 
00166 short Impedance::set_Kp(const DiagonalMatrix & Kp_)
00171 {
00172    if(Kp_.Nrows() != 3)
00173    {
00174       Kp = DiagonalMatrix(3); Kp = 1;
00175       cerr << "Impedance::set_Kp: wrong size for input matrix Kp" << endl;
00176       return WRONG_SIZE;
00177    }
00178 
00179    Kp = Kp_;
00180    return 0;
00181 }
00182 
00183 short Impedance::set_Kp(const Real Kp_i, const short i)
00188 {
00189    if( (i < 0) || (i > 3) )
00190    {
00191       cerr << "Impedance::set_Mp: index i out of bound" << endl;
00192       return WRONG_SIZE;
00193    }
00194 
00195    Kp(i) = Kp_i;
00196    return 0;
00197 }
00198 
00199 short Impedance::set_Mo(const DiagonalMatrix & Mo_)
00204 {
00205    if(Mo_.Nrows() != 3)
00206    {
00207       Mo = DiagonalMatrix(3); Mo = 1;
00208       cerr << "Impedance::set_Mo: wrong size  input matrix Mo" << endl;
00209       return WRONG_SIZE;
00210    }
00211 
00212    Mo = Mo_;
00213    return 0;
00214 }
00215 
00216 short Impedance::set_Mo(const Real Mo_i, const short i)
00221 {
00222    if( (i < 0) || (i > 3) )
00223    {
00224       cerr << "Impedance::set_Mo: index i out of bound" << endl;
00225       return WRONG_SIZE;
00226    }
00227 
00228    Mo(i) = Mo_i;
00229    return 0;
00230 }
00231 
00232 short Impedance::set_Do(const DiagonalMatrix & Do_)
00237 {
00238    if( Do_.Nrows() != 3)
00239    {
00240       Do = DiagonalMatrix(3); Do = 1;
00241       cerr << "Impedance::set_Do: wrong size  input matrix Do" << endl;
00242       return WRONG_SIZE;
00243    }
00244 
00245    Do = Do_;
00246    return 0;
00247 }
00248 
00249 short Impedance::set_Do(const Real Do_i, const short i)
00254 {
00255    if( (i < 0) || (i > 3) )
00256    {
00257       cerr << "Impedance::set_Do: index i out of bound" << endl;
00258       return WRONG_SIZE;
00259    }
00260 
00261    Do(i) = Do_i;
00262    return 0;
00263 }
00264 
00265 short Impedance::set_Ko(const DiagonalMatrix & Ko_)
00270 {
00271    if(Ko_.Nrows() != 3)
00272    {
00273       Ko = DiagonalMatrix(3); Ko = 1;
00274       cerr << "Impedance::set_Ko: wrong size for  input matrix Ko" << endl;
00275       return WRONG_SIZE;
00276    }
00277 
00278    Ko = Ko_;
00279    return 0;
00280 }
00281 
00282 short Impedance::set_Ko(const Real Ko_i, const short i)
00287 {
00288    if( (i < 0) || (i > 3) )
00289    {
00290       cerr << "Impedance::set_Ko: index i out of bound" << endl;
00291       return WRONG_SIZE;
00292    }
00293 
00294    Ko(i) = Ko_i;
00295    return 0;
00296 }
00297 
00298 short Impedance::control(const ColumnVector & pdpp, const ColumnVector & pdp,
00299                          const ColumnVector & pd, const ColumnVector & wdp,
00300                          const ColumnVector & wd, const Quaternion & qd,
00301                          const ColumnVector & f, const ColumnVector & n,
00302                          const Real dt)
00328 {
00329    if(pdpp.Nrows() !=3)
00330    {
00331       cerr << "Impedance::control: wrong size for input vector pdpp" << endl;
00332       return WRONG_SIZE;
00333    }
00334    if(pdp.Nrows() !=3)
00335    {
00336       cerr << "Impedance::control: wrong size for input vector pdp" << endl;
00337       return WRONG_SIZE;
00338    }
00339    if(pd.Nrows() != 3)
00340    {
00341       cerr << "Impedance::control: wrong size for input vector pd" << endl;
00342       return WRONG_SIZE;
00343    }
00344    if(wdp.Nrows() !=3)
00345    {
00346       cerr << "Impedance::control: wrong size for input vector wdp" << endl;
00347       return WRONG_SIZE;
00348    }
00349    if(wd.Nrows() !=3)
00350    {
00351       cerr << "Impedance::control: wrong size for input vector wd" << endl;
00352       return WRONG_SIZE;
00353    }
00354    if(f.Nrows() !=3)
00355    {
00356       cerr << "Impedance::control: wrong size for input vector f" << endl;
00357       return WRONG_SIZE;
00358    }
00359    if(n.Nrows() !=3)
00360    {
00361       cerr << "Impedance::control: wrong size for input vector f" << endl;
00362       return WRONG_SIZE;
00363    }
00364 
00365    static bool first=true;
00366    if(first)
00367    {
00368       qc = qd;
00369       qcp = qc.dot(wc, BASE_FRAME);
00370       qcp_prev = qcp;
00371       wc = wd;
00372       wcp = wdp;
00373       first = false;
00374    }
00375    if(qc.dot_prod(qd) < 0)
00376       quat = qd*(-1);
00377    else
00378       quat = qd;
00379 
00380    qcd = quat * qc.i();
00381 
00382    // Solving pcpp, pcp, pc with the translational impedance
00383    pcd = pc - pd;
00384    pcdp = pcp - pdp;
00385    //  pcpp = pdpp + Mp.i()*(f - Dp*pcdp - Kp*pcd - 2*qcd.s()*Km.t()*qcd.v()); // (21)
00386    pcpp = pdpp + Mp.i()*(f - Dp*pcdp - Kp*pcd);
00387    pcp = pcp_prev + Integ_Trap(pcpp, pcpp_prev, dt);
00388    pc = pc + Integ_Trap(pcp, pcp_prev, dt);
00389 
00390    // Solving wcp, wc, qc with the rotational impedance
00391    wcd = wc - wd;
00392    Ko_prime = 2*qcd.E(BASE_FRAME)*Ko;                                   //(23)
00393    //  Km_prime = (qcd.s()*qcd.E(BASE_FRAME) - qcd.v()*qcd.v().t())*Km;   // (24)
00394    //  wcp = wdp + Mo.i()*(n - Do*wcd - Ko_prime*qcd.v() - Km_prime*pcd); // (22)
00395    wcp = wdp + Mo.i()*(n - Do*wcd - Ko_prime*qcd.v()); // (22)
00396    wc = wc + Integ_Trap(wcp, wcp_prev, dt);
00397    qcp = qc.dot(wc, BASE_FRAME);
00398    Integ_quat(qcp, qcp_prev, qc, dt);
00399 
00400    return 0;
00401 }
00402 
00403 // ------------------------------------------------------------------------------------------------------
00404 //  Position control based on resolved acceleration.
00405 // ------------------------------------------------------------------------------------------------------
00406 
00407 Resolved_acc::Resolved_acc(const short dof)
00409 {
00410    Kvp = Kpp = Kvo = Kpo = 0;
00411    zero3 = ColumnVector(3); zero3 = 0;
00412    p = zero3;
00413    pp = zero3;
00414 
00415    if(dof>0)
00416    {
00417        qp = ColumnVector(dof); qp = 0;
00418        qpp = qp;
00419    }
00420 
00421    quat_v_error = zero3;
00422    a = ColumnVector(6); a = 0;
00423    quat = Quaternion();
00424 }
00425 
00426 Resolved_acc::Resolved_acc(const Robot_basic & robot,
00427                            const Real Kvp_,
00428                            const Real Kpp_,
00429                            const Real Kvo_,
00430                            const Real Kpo_)
00432 {
00433    set_Kvp(Kvp_);
00434    set_Kpp(Kpp_);
00435    set_Kvo(Kvo_);
00436    set_Kpo(Kpo_);
00437    zero3 = ColumnVector(3); zero3 = 0;
00438    qp = ColumnVector(robot.get_dof()); qp = 0;
00439    qpp = qp;
00440    a = ColumnVector(6); a = 0;
00441    p = zero3;
00442    pp = zero3;
00443    quat_v_error = zero3;
00444    quat = Quaternion();
00445 }
00446 
00447 void Resolved_acc::set_Kvp(const Real Kvp_)
00449 {
00450    Kvp = Kvp_;
00451 }
00452 
00453 void Resolved_acc::set_Kpp(const Real Kpp_)
00455 {
00456    Kpp = Kpp_;
00457 }
00458 
00459 void Resolved_acc::set_Kvo(const Real Kvo_)
00461 {
00462    Kvo = Kvo_;
00463 }
00464 
00465 void Resolved_acc::set_Kpo(const Real Kpo_)
00467 {
00468    Kpo = Kpo_;
00469 }
00470 
00471 ReturnMatrix Resolved_acc::torque_cmd(Robot_basic & robot, const ColumnVector & pdpp,
00472                                       const ColumnVector & pdp, const ColumnVector & pd,
00473                                       const ColumnVector & wdp, const ColumnVector & wd,
00474                                       const Quaternion & quatd, const short link_pc,
00475                                       const Real dt)
00487 {
00488    robot.kine_pd(Rot, p, pp, link_pc);
00489 
00490    Quaternion quate(Rot); // end effector orientation
00491    if(quate.dot_prod(quatd) < 0)
00492       quat = quatd*(-1);
00493    else
00494       quat = quatd;
00495 
00496    quat_v_error = quate.s()*quat.v() - quat.s()*quate.v() +
00497                   x_prod_matrix(quate.v())*quat.v();
00498 
00499    a.SubMatrix(1,3,1,1) = pdpp + Kvp*(pdp-pp) + Kpp*(pd-p);
00500    a.SubMatrix(4,6,1,1) = wdp + Kvo*(wd-Rot*robot.w[robot.get_dof()]) +
00501                           Kpo*quat_v_error;
00502    qp = robot.get_qp();
00503    //                          (eps, lamda_max)
00504    qpp = robot.jacobian_DLS_inv(0.015, 0.2)*(a - robot.jacobian_dot()*qp);
00505    return robot.torque(robot.get_q(),qp, qpp, zero3, zero3);
00506 }
00507 
00508 
00509 // ------------------------------------------------------------------------------
00510 //  Position control based on Computed Torque Method
00511 // ------------------------------------------------------------------------------
00512 
00513 Computed_torque_method::Computed_torque_method(const short dof_)
00515 {
00516     dof = dof_;
00517     qpp = ColumnVector(dof); qpp = 0;
00518     q = qpp;
00519     qp = qpp;
00520     zero3 = ColumnVector(3); zero3 = 0;
00521 }
00522 
00523 Computed_torque_method::Computed_torque_method(const Robot_basic & robot,
00524                                                const DiagonalMatrix & Kp,
00525                                                const DiagonalMatrix & Kd)
00527 {
00528    dof = robot.get_dof();
00529    set_Kd(Kd);
00530    set_Kp(Kp);
00531    qpp = ColumnVector(dof); qpp = 0;
00532    q = qpp;
00533    qp = qpp;
00534    zero3 = ColumnVector(3); zero3 = 0;
00535 }
00536 
00537 ReturnMatrix Computed_torque_method::torque_cmd(Robot_basic & robot,
00538                                                 const ColumnVector & qd,
00539                                                 const ColumnVector & qpd, 
00540                                                 const ColumnVector & qppd )
00542 {
00543    if(qd.Nrows() != dof)
00544    {
00545       ColumnVector tau(dof); tau = 0;
00546       cerr << "Computed_torque_methode::torque_cmd: wrong size for input vector qd" << endl;
00547       tau.Release();
00548       return tau;
00549    }
00550    if(qpd.Nrows() != dof)
00551    {
00552       ColumnVector tau(dof); tau = 0;
00553       cerr << "Computed_torque_methode::torque_cmd: wrong size for input vector qpd" << endl;
00554       tau.Release();
00555       return tau;
00556    }
00557    if(qppd.Nrows() != dof)
00558    {
00559       ColumnVector tau(dof); tau = 0;
00560       cerr << "Computed_torque_methode::torque_cmd: wrong size for input vector qppd" << endl;
00561       tau.Release();
00562       return tau;
00563    }
00564 
00565    q = robot.get_q();
00566    qp = robot.get_qp();
00567    qpp = Kp*(qd-q) + Kd*(qpd-qp) + qppd;
00568    return robot.torque(q, qp, qpp, zero3, zero3);
00569 }
00570 
00571 short Computed_torque_method::set_Kd(const DiagonalMatrix & Kd_)
00576 {
00577    if(Kd_.Nrows() != dof)
00578    {
00579       Kd = DiagonalMatrix(dof); Kd = 0;
00580       cerr << "Computed_torque_method::set_kd: wrong size for input matrix Kd." << endl;
00581       return WRONG_SIZE;
00582    }
00583 
00584    Kd = Kd_;
00585    return 0;
00586 }
00587 
00588 short Computed_torque_method::set_Kp(const DiagonalMatrix & Kp_)
00593 {
00594    if(Kp_.Nrows() != dof)
00595    {
00596       Kp = DiagonalMatrix(dof); Kp = 0;
00597       cerr << "Computed_torque_method::set_kp: wrong size for input matrix Kp." << endl;
00598       return WRONG_SIZE;
00599    }
00600 
00601    Kp = Kp_;
00602    return 0;
00603 }
00604 
00605 // ------------------------------------------------------------------------------
00606 //  Position control based on Proportional_Derivative (PD)
00607 // ------------------------------------------------------------------------------
00608 
00609 Proportional_Derivative::Proportional_Derivative(const short dof_)
00611 {
00612   dof = dof_;
00613   q = ColumnVector(dof); q = 0;
00614   qp = q;
00615   zero3 = ColumnVector(3); zero3 = 0;
00616 }
00617 
00618 Proportional_Derivative::Proportional_Derivative(const Robot_basic & robot, 
00619                                                  const DiagonalMatrix & Kp, 
00620                                                  const DiagonalMatrix & Kd)
00622 {
00623    dof = robot.get_dof();
00624    set_Kp(Kp);
00625    set_Kd(Kd);
00626    q = ColumnVector(dof); q = 0;
00627    qp = q;
00628    tau = ColumnVector(dof); tau = 0;
00629    zero3 = ColumnVector(3); zero3 = 0;
00630 }
00631 
00632 ReturnMatrix Proportional_Derivative::torque_cmd(Robot_basic & robot, 
00633                                                  const ColumnVector & qd,
00634                                                  const ColumnVector & qpd)
00636 {
00637    if(qd.Nrows() != dof)
00638    {
00639       tau = 0;
00640       cerr << "Proportional_Derivative::torque_cmd: wrong size for input vector qd" << endl;
00641       return tau;
00642    }
00643    if(qpd.Nrows() != dof)
00644    {
00645       tau = 0;
00646       cerr << "Proportional_Derivative::torque_cmd: wrong size for input vector qpd" << endl;
00647       return tau;
00648    }
00649 
00650    q = robot.get_q();
00651    qp = robot.get_qp();
00652    tau = Kp*(qd-q) + Kd*(qpd-qp);
00653 
00654    return tau;
00655 }
00656 
00657 short Proportional_Derivative::set_Kd(const DiagonalMatrix & Kd_)
00662 {
00663    if(Kd_.Nrows() != dof)
00664    {
00665       Kd = DiagonalMatrix(dof); Kd = 0;
00666       cerr << "Proportional_Derivative::set_kd: wrong size for input matrix Kd." << endl;
00667       return WRONG_SIZE;
00668    }
00669 
00670    Kd = Kd_;
00671    return 0;
00672 }
00673 
00674 short Proportional_Derivative::set_Kp(const DiagonalMatrix & Kp_)
00679 {
00680    if(Kp_.Nrows() != dof)
00681    {
00682       Kp = DiagonalMatrix(dof); Kp = 0;
00683       cerr << "Computed_torque_method::set_kp: wrong size for input matrix Kp." << endl;
00684       return WRONG_SIZE;
00685    }
00686 
00687    Kp = Kp_;
00688    return 0;
00689 }
00690 
00691 #ifdef use_namespace
00692 }
00693 #endif


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