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