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