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
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070 static const char rcsid[] = "$Id: dynamics.cpp,v 1.6 2004/07/14 03:22:32 ejt Exp $";
00071
00072 #include "robot.h"
00073
00074 #ifdef use_namespace
00075 namespace ROBOOP {
00076 using namespace NEWMAT;
00077 #endif
00078
00079 ReturnMatrix Robot_basic::inertia(const ColumnVector & q)
00080
00081 {
00082 Matrix M(dof,dof);
00083 ColumnVector torque(dof);
00084 set_q(q);
00085 for(int i = 1; i <= dof; i++) {
00086 for(int j = 1; j <= dof; j++) {
00087 torque(j) = (i == j ? 1.0 : 0.0);
00088 }
00089 torque = torque_novelocity(torque);
00090 M.Column(i) = torque;
00091 }
00092 M.Release(); return M;
00093 }
00094
00095
00096 ReturnMatrix Robot_basic::acceleration(const ColumnVector & q,
00097 const ColumnVector & qp,
00098 const ColumnVector & tau_cmd)
00099
00100 {
00101 ColumnVector qpp(dof);
00102 qpp = 0.0;
00103 qpp = inertia(q).i()*(tau_cmd-torque(q,qp,qpp));
00104 qpp.Release();
00105 return qpp;
00106 }
00107
00108 ReturnMatrix Robot_basic::acceleration(const ColumnVector & q, const ColumnVector & qp,
00109 const ColumnVector & tau_cmd, const ColumnVector & Fext,
00110 const ColumnVector & Next)
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123 {
00124 ColumnVector qpp(dof);
00125 qpp = 0.0;
00126 qpp = inertia(q).i()*(tau_cmd-torque(q, qp, qpp, Fext, Next));
00127 qpp.Release();
00128 return qpp;
00129 }
00130
00131 ReturnMatrix Robot::torque(const ColumnVector & q, const ColumnVector & qp,
00132 const ColumnVector & qpp)
00133
00134
00135
00136
00137 {
00138 ColumnVector Fext(3), Next(3);
00139 Fext = 0;
00140 Next = 0;
00141 return torque(q, qp, qpp, Fext, Next);
00142 }
00143
00144 ReturnMatrix Robot::torque(const ColumnVector & q, const ColumnVector & qp,
00145 const ColumnVector & qpp, const ColumnVector & Fext,
00146 const ColumnVector & Next)
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211 {
00212 int i;
00213 ColumnVector ltorque(dof);
00214 Matrix Rt, temp;
00215 if(q.Nrows() != dof) error("q has wrong dimension");
00216 if(qp.Nrows() != dof) error("qp has wrong dimension");
00217 if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00218 set_q(q);
00219 set_qp(qp);
00220
00221 vp[0] = gravity;
00222 for(i = 1; i <= dof; i++) {
00223 Rt = links[i].R.t();
00224 if(links[i].get_joint_type() == 0) {
00225 w[i] = Rt*(w[i-1] + z0*qp(i));
00226 wp[i] = Rt*(wp[i-1] + z0*qpp(i)
00227 + CrossProduct(w[i-1],z0*qp(i)));
00228 vp[i] = CrossProduct(wp[i],p[i])
00229 + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00230 + Rt*(vp[i-1]);
00231 } else {
00232 w[i] = Rt*w[i-1];
00233 wp[i] = Rt*wp[i-1];
00234 vp[i] = Rt*(vp[i-1] + z0*qpp(i))
00235 + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
00236 + CrossProduct(wp[i],p[i])
00237 + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00238 }
00239 a[i] = CrossProduct(wp[i],links[i].r)
00240 + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00241 + vp[i];
00242 }
00243
00244 for(i = dof; i >= 1; i--) {
00245 F[i] = a[i] * links[i].m;
00246 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00247 if(i == dof) {
00248 f[i] = F[i] + Fext;
00249 n[i] = CrossProduct(p[i],f[i])
00250 + CrossProduct(links[i].r,F[i]) + N[i] + Next;
00251 } else {
00252 f[i] = links[i+1].R*f[i+1] + F[i];
00253 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00254 + CrossProduct(links[i].r,F[i]) + N[i];
00255 }
00256 if(links[i].get_joint_type() == 0)
00257 temp = ((z0.t()*links[i].R)*n[i]);
00258 else
00259 temp = ((z0.t()*links[i].R)*f[i]);
00260 ltorque(i) = temp(1,1)
00261 + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
00262 + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00263 }
00264
00265 ltorque.Release(); return ltorque;
00266 }
00267
00268 ReturnMatrix Robot::torque_novelocity(const ColumnVector & qpp)
00269
00270
00271
00272
00273 {
00274 int i;
00275 ColumnVector ltorque(dof);
00276 Matrix Rt, temp;
00277 if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00278
00279 vp[0] = 0.0;
00280 for(i = 1; i <= dof; i++) {
00281 Rt = links[i].R.t();
00282 if(links[i].get_joint_type() == 0) {
00283 wp[i] = Rt*(wp[i-1] + z0*qpp(i));
00284 vp[i] = CrossProduct(wp[i],p[i])
00285 + Rt*(vp[i-1]);
00286 } else {
00287 wp[i] = Rt*wp[i-1];
00288 vp[i] = Rt*(vp[i-1] + z0*qpp(i))
00289 + CrossProduct(wp[i],p[i]);
00290 }
00291 a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
00292 }
00293
00294 for(i = dof; i >= 1; i--) {
00295 F[i] = a[i] * links[i].m;
00296 N[i] = links[i].I*wp[i];
00297 if(i == dof) {
00298 f_nv[i] = F[i];
00299 n_nv[i] = CrossProduct(p[i],f_nv[i])
00300 + CrossProduct(links[i].r,F[i]) + N[i];
00301 } else {
00302 f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
00303 n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i],f_nv[i])
00304 + CrossProduct(links[i].r,F[i]) + N[i];
00305 }
00306 if(links[i].get_joint_type() == 0)
00307 temp = ((z0.t()*links[i].R)*n_nv[i]);
00308 else
00309 temp = ((z0.t()*links[i].R)*f_nv[i]);
00310 ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
00311
00312 }
00313
00314 ltorque.Release(); return ltorque;
00315 }
00316
00317 ReturnMatrix Robot::G()
00318
00319 {
00320 int i;
00321 ColumnVector ltorque(dof);
00322 Matrix Rt, temp;
00323
00324 vp[0] = gravity;
00325 for(i = 1; i <= dof; i++) {
00326 Rt = links[i].R.t();
00327 if(links[i].get_joint_type() == 0)
00328 vp[i] = Rt*(vp[i-1]);
00329 else
00330 vp[i] = Rt*vp[i-1];
00331
00332 a[i] = vp[i];
00333 }
00334
00335 for(i = dof; i >= 1; i--) {
00336 F[i] = a[i] * links[i].m;
00337 if(i == dof) {
00338 f[i] = F[i];
00339 n[i] = CrossProduct(p[i],f[i])
00340 + CrossProduct(links[i].r,F[i]);
00341 } else {
00342 f[i] = links[i+1].R*f[i+1] + F[i];
00343 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00344 + CrossProduct(links[i].r,F[i]);
00345 }
00346 if(links[i].get_joint_type() == 0)
00347 temp = ((z0.t()*links[i].R)*n[i]);
00348 else
00349 temp = ((z0.t()*links[i].R)*f[i]);
00350 ltorque(i) = temp(1,1);
00351 }
00352
00353 ltorque.Release(); return ltorque;
00354 }
00355
00356 ReturnMatrix Robot::C(const ColumnVector & qp)
00357
00358 {
00359 int i;
00360 ColumnVector ltorque(dof);
00361 Matrix Rt, temp;
00362 if(qp.Nrows() != dof) error("qp has wrong dimension");
00363
00364 vp[0]=0;
00365 for(i = 1; i <= dof; i++) {
00366 Rt = links[i].R.t();
00367 if(links[i].get_joint_type() == 0) {
00368 w[i] = Rt*(w[i-1] + z0*qp(i));
00369 wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
00370 vp[i] = CrossProduct(wp[i],p[i])
00371 + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00372 + Rt*(vp[i-1]);
00373 } else {
00374 w[i] = Rt*w[i-1];
00375 wp[i] = Rt*wp[i-1];
00376 vp[i] = Rt*vp[i-1] + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
00377 + CrossProduct(wp[i],p[i])
00378 + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00379 }
00380 a[i] = CrossProduct(wp[i],links[i].r)
00381 + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00382 + vp[i];
00383 }
00384
00385 for(i = dof; i >= 1; i--) {
00386 F[i] = a[i] * links[i].m;
00387 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00388 if(i == dof) {
00389 f[i] = F[i];
00390 n[i] = CrossProduct(p[i],f[i])
00391 + CrossProduct(links[i].r,F[i]) + N[i];
00392 } else {
00393 f[i] = links[i+1].R*f[i+1] + F[i];
00394 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00395 + CrossProduct(links[i].r,F[i]) + N[i];
00396 }
00397 if(links[i].get_joint_type() == 0)
00398 temp = ((z0.t()*links[i].R)*n[i]);
00399 else
00400 temp = ((z0.t()*links[i].R)*f[i]);
00401 ltorque(i) = temp(1,1)
00402 + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00403 }
00404
00405 ltorque.Release(); return ltorque;
00406 }
00407
00408 ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp,
00409 const ColumnVector & qpp)
00410
00411 {
00412 ColumnVector Fext(3), Next(3);
00413 Fext = 0;
00414 Next = 0;
00415 return torque(q, qp, qpp, Fext, Next);
00416 }
00417
00418 ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp,
00419 const ColumnVector & qpp, const ColumnVector & Fext_,
00420 const ColumnVector & Next_)
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483 {
00484 int i;
00485 ColumnVector ltorque(dof);
00486 Matrix Rt, temp;
00487 if(q.Nrows() != dof) error("q has wrong dimension");
00488 if(qp.Nrows() != dof) error("qp has wrong dimension");
00489 if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00490 set_q(q);
00491 set_qp(qp);
00492
00493 vp[0] = gravity;
00494 for(i = 1; i <= dof; i++) {
00495 Rt = links[i].R.t();
00496 if(links[i].get_joint_type() == 0) {
00497 w[i] = Rt*w[i-1] + z0*qp(i);
00498 wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00499 + z0*qpp(i);
00500 vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00501 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00502 + vp[i-1]);
00503 } else {
00504 w[i] = Rt*w[i-1];
00505 wp[i] = Rt*wp[i-1];
00506 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00507 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00508 + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00509 }
00510 a[i] = CrossProduct(wp[i],links[i].r)
00511 + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00512 + vp[i];
00513 }
00514
00515
00516 ColumnVector Fext(3), Next(3);
00517 if(fix)
00518 {
00519 Fext = links[dof+fix].R*Fext_;
00520 Next = links[dof+fix].R*Next_;
00521 }
00522 else
00523 {
00524 Fext = Fext_;
00525 Next = Next_;
00526 }
00527
00528 for(i = dof; i >= 1; i--) {
00529 F[i] = a[i] * links[i].m;
00530 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00531 if(i == dof) {
00532 f[i] = F[i] + Fext;
00533 n[i] = CrossProduct(links[i].r,F[i]) + N[i] + Next;
00534 } else {
00535 f[i] = links[i+1].R*f[i+1] + F[i];
00536 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00537 + CrossProduct(links[i].r,F[i]) + N[i];
00538 }
00539 if(links[i].get_joint_type() == 0)
00540 temp = (z0.t()*n[i]);
00541 else
00542 temp = (z0.t()*f[i]);
00543 ltorque(i) = temp(1,1)
00544 + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
00545 + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00546 }
00547
00548 ltorque.Release(); return ltorque;
00549 }
00550
00551 ReturnMatrix mRobot::torque_novelocity(const ColumnVector & qpp)
00552
00553 {
00554 int i;
00555 ColumnVector ltorque(dof);
00556 Matrix Rt, temp;
00557 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00558
00559 vp[0] = 0.0;
00560 for(i = 1; i <= dof; i++) {
00561 Rt = links[i].R.t();
00562 if(links[i].get_joint_type() == 0) {
00563 wp[i] = Rt*wp[i-1] + z0*qpp(i);
00564 vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
00565 } else {
00566 wp[i] = Rt*wp[i-1];
00567 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
00568 + z0*qpp(i);
00569 }
00570 a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
00571 }
00572
00573 for(i = dof; i >= 1; i--) {
00574 F[i] = a[i] * links[i].m;
00575 N[i] = links[i].I*wp[i];
00576 if(i == dof) {
00577 f_nv[i] = F[i];
00578 n_nv[i] = CrossProduct(links[i].r,F[i]) + N[i];
00579 } else {
00580 f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
00581 n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i+1],links[i+1].R*f_nv[i+1])
00582 + CrossProduct(links[i].r,F[i]) + N[i];
00583 }
00584
00585 if(links[i].get_joint_type() == 0)
00586 temp = (z0.t()*n_nv[i]);
00587 else
00588 temp = (z0.t()*f_nv[i]);
00589 ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
00590 }
00591
00592 ltorque.Release(); return ltorque;
00593 }
00594
00595 ReturnMatrix mRobot::G()
00596
00597 {
00598 int i;
00599 ColumnVector ltorque(dof);
00600 Matrix Rt, temp;
00601
00602 vp[0] = gravity;
00603 for(i = 1; i <= dof; i++) {
00604 Rt = links[i].R.t();
00605 if(links[i].get_joint_type() == 0)
00606 vp[i] = Rt*vp[i-1];
00607 else
00608 vp[i] = Rt*vp[i-1];
00609 a[i] = vp[i];
00610 }
00611
00612 for(i = dof; i >= 1; i--) {
00613 F[i] = a[i] * links[i].m;
00614 if(i == dof) {
00615 f[i] = F[i];
00616 n[i] = CrossProduct(links[i].r,F[i]);
00617 } else {
00618 f[i] = links[i+1].R*f[i+1] + F[i];
00619 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00620 + CrossProduct(links[i].r,F[i]);
00621 }
00622 if(links[i].get_joint_type() == 0)
00623 temp = (z0.t()*n[i]);
00624 else
00625 temp = (z0.t()*f[i]);
00626 ltorque(i) = temp(1,1);
00627 }
00628
00629 ltorque.Release(); return ltorque;
00630 }
00631
00632 ReturnMatrix mRobot::C(const ColumnVector & qp)
00633
00634 {
00635 int i;
00636 ColumnVector ltorque(dof);
00637 Matrix Rt, temp;
00638 if(qp.Nrows() != dof) error("qp has wrong dimension");
00639
00640 vp[0] = 0;
00641 for(i = 1; i <= dof; i++) {
00642 Rt = links[i].R.t();
00643 if(links[i].get_joint_type() == 0) {
00644 w[i] = Rt*w[i-1] + z0*qp(i);
00645 wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i));
00646 vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00647 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00648 + vp[i-1]);
00649 } else {
00650 w[i] = Rt*w[i-1];
00651 wp[i] = Rt*wp[i-1];
00652 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00653 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00654 + 2.0*CrossProduct(w[i],z0*qp(i));
00655 }
00656 a[i] = CrossProduct(wp[i],links[i].r)
00657 + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00658 + vp[i];
00659 }
00660
00661 for(i = dof; i >= 1; i--) {
00662 F[i] = a[i] * links[i].m;
00663 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00664 if(i == dof) {
00665 f[i] = F[i];
00666 n[i] = CrossProduct(links[i].r,F[i]) + N[i];
00667 } else {
00668 f[i] = links[i+1].R*f[i+1] + F[i];
00669 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00670 + CrossProduct(links[i].r,F[i]) + N[i];
00671 }
00672 if(links[i].get_joint_type() == 0)
00673 temp = (z0.t()*n[i]);
00674 else
00675 temp = (z0.t()*f[i]);
00676 ltorque(i) = temp(1,1)
00677 + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00678 }
00679
00680 ltorque.Release(); return ltorque;
00681 }
00682
00683 ReturnMatrix mRobot_min_para::torque(const ColumnVector & q, const ColumnVector & qp,
00684 const ColumnVector & qpp)
00685
00686 {
00687 ColumnVector Fext(3), Next(3);
00688 Fext = 0;
00689 Next = 0;
00690 return torque(q, qp, qpp, Fext, Next);
00691 }
00692
00693 ReturnMatrix mRobot_min_para::torque(const ColumnVector & q, const ColumnVector & qp,
00694 const ColumnVector & qpp, const ColumnVector & Fext_,
00695 const ColumnVector & Next_)
00696
00697
00698
00699
00700
00701
00702
00703
00704 {
00705 int i;
00706 ColumnVector ltorque(dof);
00707 Matrix Rt, temp;
00708 if(q.Nrows() != dof) error("q has wrong dimension");
00709 if(qp.Nrows() != dof) error("qp has wrong dimension");
00710 if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00711 set_q(q);
00712 set_qp(qp);
00713
00714 vp[0] = gravity;
00715 for(i = 1; i <= dof; i++) {
00716 Rt = links[i].R.t();
00717 if(links[i].get_joint_type() == 0)
00718 {
00719 w[i] = Rt*w[i-1] + z0*qp(i);
00720 wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)))
00721 + z0*qpp(i);
00722 vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00723 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00724 + vp[i-1]);
00725 }
00726 else
00727 {
00728 w[i] = Rt*w[i-1];
00729 wp[i] = Rt*wp[i-1];
00730 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00731 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00732 + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00733 }
00734 }
00735
00736 ColumnVector Fext(3), Next(3);
00737 if(fix)
00738 {
00739 Fext = links[dof+fix].R*Fext_;
00740 Next = links[dof+fix].R*Next_;
00741 }
00742 else
00743 {
00744 Fext = Fext_;
00745 Next = Next_;
00746 }
00747
00748 for(i = dof; i >= 1; i--)
00749 {
00750 F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00751 CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00752 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
00753 CrossProduct(-vp[i], links[i].mc);
00754 if(i == dof)
00755 {
00756 f[i] = F[i] + Fext;
00757 n[i] = N[i] + Next;
00758 }
00759 else
00760 {
00761 f[i] = links[i+1].R*f[i+1] + F[i];
00762 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
00763 }
00764
00765 if(links[i].get_joint_type() == 0)
00766 temp = (z0.t()*n[i]);
00767 else
00768 temp = (z0.t()*f[i]);
00769 ltorque(i) = temp(1,1)
00770 + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
00771 + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00772 }
00773
00774 ltorque.Release(); return ltorque;
00775 }
00776
00777
00778 ReturnMatrix mRobot_min_para::torque_novelocity(const ColumnVector & qpp)
00779
00780 {
00781 int i;
00782 ColumnVector ltorque(dof);
00783 Matrix Rt, temp;
00784 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00785
00786 vp[0] = 0.0;
00787 for(i = 1; i <= dof; i++)
00788 {
00789 Rt = links[i].R.t();
00790 if(links[i].get_joint_type() == 0)
00791 {
00792 wp[i] = Rt*wp[i-1] + z0*qpp(i);
00793 vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
00794 }
00795 else
00796 {
00797 wp[i] = Rt*wp[i-1];
00798 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
00799 + z0*qpp(i);
00800 }
00801 }
00802
00803 for(i = dof; i >= 1; i--)
00804 {
00805 F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc);
00806 N[i] = links[i].I*wp[i] + CrossProduct(-vp[i], links[i].mc);
00807 if(i == dof)
00808 {
00809 f_nv[i] = F[i];
00810 n_nv[i] = N[i];
00811 }
00812 else
00813 {
00814 f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
00815 n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i+1],links[i+1].R*f_nv[i+1]) + N[i];
00816 }
00817
00818 if(links[i].get_joint_type() == 0)
00819 temp = (z0.t()*n_nv[i]);
00820 else
00821 temp = (z0.t()*f_nv[i]);
00822 ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
00823 }
00824
00825 ltorque.Release(); return ltorque;
00826 }
00827
00828 ReturnMatrix mRobot_min_para::G()
00829
00830 {
00831 int i;
00832 ColumnVector ltorque(dof);
00833 Matrix Rt, temp;
00834
00835 vp[0] = gravity;
00836 for(i = 1; i <= dof; i++) {
00837 Rt = links[i].R.t();
00838 if(links[i].get_joint_type() == 0)
00839 vp[i] = Rt*vp[i-1];
00840 else
00841 vp[i] = Rt*vp[i-1];
00842 }
00843
00844 for(i = dof; i >= 1; i--)
00845 {
00846 F[i] = vp[i]*links[i].m;
00847 N[i] = CrossProduct(-vp[i], links[i].mc);
00848 if(i == dof)
00849 {
00850 f[i] = F[i];
00851 n[i] = N[i];
00852 }
00853 else
00854 {
00855 f[i] = links[i+1].R*f[i+1] + F[i];
00856 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
00857 }
00858
00859 if(links[i].get_joint_type() == 0)
00860 temp = (z0.t()*n[i]);
00861 else
00862 temp = (z0.t()*f[i]);
00863 ltorque(i) = temp(1,1);
00864 }
00865
00866 ltorque.Release(); return ltorque;
00867 }
00868
00869 ReturnMatrix mRobot_min_para::C(const ColumnVector & qp)
00870
00871 {
00872 int i;
00873 ColumnVector ltorque(dof);
00874 Matrix Rt, temp;
00875 if(qp.Nrows() != dof) error("qp has wrong dimension");
00876 set_qp(qp);
00877
00878 vp[0] = 0;
00879 for(i = 1; i <= dof; i++) {
00880 Rt = links[i].R.t();
00881 if(links[i].get_joint_type() == 0)
00882 {
00883 w[i] = Rt*w[i-1] + z0*qp(i);
00884 wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
00885 vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00886 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00887 + vp[i-1]);
00888 }
00889 else
00890 {
00891 w[i] = Rt*w[i-1];
00892 wp[i] = Rt*wp[i-1];
00893 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00894 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00895 + 2.0*CrossProduct(w[i],z0*qp(i));
00896 }
00897 }
00898
00899 for(i = dof; i >= 1; i--)
00900 {
00901 F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00902 CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00903 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
00904 CrossProduct(-vp[i], links[i].mc);
00905 if(i == dof)
00906 {
00907 f[i] = F[i];
00908 n[i] = N[i];
00909 }
00910 else
00911 {
00912 f[i] = links[i+1].R*f[i+1] + F[i];
00913 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
00914 }
00915
00916 if(links[i].get_joint_type() == 0)
00917 temp = (z0.t()*n[i]);
00918 else
00919 temp = (z0.t()*f[i]);
00920 ltorque(i) = temp(1,1)
00921 + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00922 }
00923
00924 ltorque.Release(); return ltorque;
00925 }
00926
00927 #ifdef use_namespace
00928 }
00929 #endif
00930