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
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
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
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
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
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
00140
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
00157
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
00173
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
00190
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
00206
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
00223
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
00239
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
00256
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
00272
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
00289
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
00305
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
00322
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
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
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
00420 pcd = pc - pd;
00421 pcdp = pcp - pdp;
00422
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
00428 wcd = wc - wd;
00429 Ko_prime = 2*qcd.E(BASE_FRAME)*Ko;
00430
00431
00432 wcp = wdp + Mo.i()*(n - Do*wcd - Ko_prime*qcd.v());
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
00442
00443
00444 Resolved_acc::Resolved_acc(const short dof)
00445
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
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
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
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
00522 {
00523 Kvp = Kvp_;
00524 }
00525
00526 void Resolved_acc::set_Kpp(const Real Kpp_)
00527
00528 {
00529 Kpp = Kpp_;
00530 }
00531
00532 void Resolved_acc::set_Kvo(const Real Kvo_)
00533
00534 {
00535 Kvo = Kvo_;
00536 }
00537
00538 void Resolved_acc::set_Kpo(const Real Kpo_)
00539
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
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560 {
00561 robot.kine_pd(Rot, p, pp, link_pc);
00562
00563 Quaternion quate(Rot);
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
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
00584
00585
00586 Computed_torque_method::Computed_torque_method(const short dof_)
00587
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
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
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
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
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
00664
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
00681
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
00697
00698
00699 Proportional_Derivative::Proportional_Derivative(const short dof_)
00700
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
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
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
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
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
00776
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
00793
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
|