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
00031
00032
00033
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
00383 pcd = pc - pd;
00384 pcdp = pcp - pdp;
00385
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
00391 wcd = wc - wd;
00392 Ko_prime = 2*qcd.E(BASE_FRAME)*Ko;
00393
00394
00395 wcp = wdp + Mo.i()*(n - Do*wcd - Ko_prime*qcd.v());
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
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);
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
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
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
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