controller.cppGo to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include "controller.h"
00030
00031 #ifdef use_namespace
00032 namespace ROBOOP {
00033 using namespace NEWMAT;
00034 #endif
00035
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
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
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
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
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
00137
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
00154
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
00170
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
00187
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
00203
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
00220
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
00236
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
00253
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
00269
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
00286
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
00302
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
00319
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
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
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
00417 pcd = pc - pd;
00418 pcdp = pcp - pdp;
00419
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
00425 wcd = wc - wd;
00426 Ko_prime = 2*qcd.E(BASE_FRAME)*Ko;
00427
00428
00429 wcp = wdp + Mo.i()*(n - Do*wcd - Ko_prime*qcd.v());
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
00439
00440
00441 Resolved_acc::Resolved_acc(const short dof)
00442
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
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
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
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
00519 {
00520 Kvp = Kvp_;
00521 }
00522
00523 void Resolved_acc::set_Kpp(const Real Kpp_)
00524
00525 {
00526 Kpp = Kpp_;
00527 }
00528
00529 void Resolved_acc::set_Kvo(const Real Kvo_)
00530
00531 {
00532 Kvo = Kvo_;
00533 }
00534
00535 void Resolved_acc::set_Kpo(const Real Kpo_)
00536
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 )
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557 {
00558 robot.kine_pd(Rot, p, pp, link_pc);
00559
00560 Quaternion quate(Rot);
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
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
00581
00582
00583 Computed_torque_method::Computed_torque_method(const short dof_)
00584
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
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
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
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
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
00661
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
00678
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
00694
00695
00696 Proportional_Derivative::Proportional_Derivative(const short dof_)
00697
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
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
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
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
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
00773
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
00790
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
|