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
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089 #include "robot.h"
00090
00091 #ifdef use_namespace
00092 namespace ROBOOP {
00093 using namespace NEWMAT;
00094 #endif
00095
00096 static const char rcsid[] __UNUSED__ = "$Id: kinemat.cpp,v 1.17 2005/07/26 03:22:09 ejt Exp $";
00097
00098 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos)const
00099
00100
00101
00102
00103
00104 {
00105 kine(Rot,pos,dof+fix);
00106 }
00107
00108 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos, const int j)const
00109
00110
00111
00112
00113
00114
00115 {
00116 if(j < 0 || j > dof+fix) error("j must be 0 <= j <= dof+fix");
00117 if(j == 0) {
00118 Rot = R[0];
00119 pos = p[0];
00120 return;
00121 }
00122
00123 Rot = links[1].R;
00124 pos = links[1].p;
00125 for (int i = 2; i <= j; i++) {
00126 pos = pos + Rot*links[i].p;
00127 Rot = Rot*links[i].R;
00128 }
00129 }
00130
00131 ReturnMatrix Robot_basic::kine(void)const
00132
00133 {
00134 Matrix thomo;
00135
00136 thomo = kine(dof+fix);
00137 thomo.Release(); return thomo;
00138 }
00139
00140 ReturnMatrix Robot_basic::kine(const int j)const
00141
00142 {
00143 Matrix Rot, thomo(4,4);
00144 ColumnVector pos;
00145
00146 kine(Rot,pos,j);
00147 thomo << fourbyfourident;
00148 thomo.SubMatrix(1,3,1,3) = Rot;
00149 thomo.SubMatrix(1,3,4,4) = pos;
00150 thomo.Release(); return thomo;
00151 }
00152
00153 ReturnMatrix Robot_basic::kine_pd(const int j)const
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164 {
00165 Matrix temp(3,5), Rot;
00166 ColumnVector pos, pos_dot;
00167
00168 if(j < 1 || j > dof)
00169 error("j must be 1 <= j <= dof");
00170
00171 kine_pd(Rot, pos, pos_dot, j);
00172
00173 temp.SubMatrix(1,3,1,3) = Rot;
00174 temp.SubMatrix(1,3,4,4) = pos;
00175 temp.SubMatrix(1,3,5,5) = pos_dot;
00176 temp.Release();
00177 return temp;
00178 }
00179
00180 ReturnMatrix Robot_basic::jacobian_DLS_inv(const double eps, const double lambda_max,
00181 const int ref)const
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 Matrix jacob_inv_DLS, U, V;
00213 DiagonalMatrix Q;
00214 SVD(jacobian(ref), Q, U, V);
00215
00216 if(Q(6,6) >= eps)
00217 jacob_inv_DLS = V*Q.i()*U.t();
00218 else
00219 {
00220 Q(6,6) += (1 - pow(Q(6,6)/eps,2))*lambda_max*lambda_max;
00221 jacob_inv_DLS = V*Q.i()*U.t();
00222 }
00223
00224 jacob_inv_DLS.Release();
00225 return(jacob_inv_DLS);
00226 }
00227
00228
00229
00230
00231
00232
00233 void Robot_basic::convertFrame(Matrix& R, ColumnVector& p, int cur, int dest) const {
00234 if(cur>dest && cur>0) {
00235 if(dest<=0)
00236 dest=1;
00237 R=links[dest].R;
00238 p=links[dest].p;
00239 for(dest++;dest<cur;dest++) {
00240 p=p+R*links[dest].p;
00241 R=R*links[dest].R;
00242 }
00243 } else if(cur<dest && dest>0) {
00244 if(cur<=0)
00245 cur=1;
00246 R=links[cur].R;
00247 p=links[cur].p;
00248 for(cur++;cur<dest;cur++) {
00249 p=p+R*links[cur].p;
00250 R=R*links[cur].R;
00251 }
00252 R=R.t();
00253 p=R*-p;
00254 } else {
00255 R=Matrix(3,3);
00256 R<<threebythreeident;
00257 p=ColumnVector(3);
00258 p=0;
00259 }
00260 }
00261
00262
00263 ReturnMatrix Robot_basic::convertFrame(int cur, int dest) const {
00264 Matrix R;
00265 ColumnVector p;
00266 convertFrame(R,p,cur,dest);
00267 return pack4x4(R,p);
00268 }
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283 void Robot_basic::convertLinkToFrame(Matrix& R, ColumnVector& p, int cur, int dest) const {
00284 convertFrame(R,p,cur,dest);
00285 if(cur<1)
00286 return;
00287 if(links[cur].get_joint_type()==0) {
00288 Matrix rotz3(3,3);
00289 rotz3 << threebythreeident;
00290 rotz3(1,1)=rotz3(2,2)=cos((links[cur].get_theta()-links[cur].get_joint_offset()));
00291 rotz3(1,2)=-(rotz3(2,1)=sin((links[cur].get_theta()-links[cur].get_joint_offset())));
00292 R*=rotz3;
00293 } else {
00294 p+=R.SubMatrix(1,3,3,3)*((links[cur].get_d()-links[cur].get_joint_offset()));
00295 }
00296 }
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309 ReturnMatrix Robot_basic::convertLinkToFrame(int cur, int dest) const {
00310 Matrix R;
00311 ColumnVector p;
00312 convertLinkToFrame(R,p,cur,dest);
00313 return pack4x4(R,p);
00314 }
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329 void Robot_basic::convertFrameToLink(Matrix& R, ColumnVector& p, int cur, int dest) const {
00330 convertFrame(R,p,cur,dest);
00331 if(dest<1)
00332 return;
00333 if(links[dest].get_joint_type()==0) {
00334 Matrix rotz3(3,3);
00335 rotz3 << threebythreeident;
00336 rotz3(1,1)=rotz3(2,2)=cos(-(links[dest].get_theta()-links[dest].get_joint_offset()));
00337 rotz3(1,2)=-(rotz3(2,1)=sin(-(links[dest].get_theta()-links[dest].get_joint_offset())));
00338 R=rotz3*R;
00339 p=rotz3*p;
00340 } else {
00341 p(3)+=-(links[dest].get_d()-links[dest].get_joint_offset());
00342 }
00343 }
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356 ReturnMatrix Robot_basic::convertFrameToLink(int cur, int dest) const {
00357 Matrix R;
00358 ColumnVector p;
00359 convertFrameToLink(R,p,cur,dest);
00360 return pack4x4(R,p);
00361 }
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376 void Robot_basic::convertLink(Matrix& R, ColumnVector& p, int cur, int dest) const {
00377 convertFrame(R,p,cur,dest);
00378 if(cur>0) {
00379 if(links[cur].get_joint_type()==0) {
00380 Matrix rotz3(3,3);
00381 rotz3 << threebythreeident;
00382 rotz3(1,1)=rotz3(2,2)=cos((links[cur].get_theta()-links[cur].get_joint_offset()));
00383 rotz3(1,2)=-(rotz3(2,1)=sin((links[cur].get_theta()-links[cur].get_joint_offset())));
00384 R*=rotz3;
00385 } else {
00386 p+=R.SubMatrix(1,3,3,3)*((links[cur].get_d()-links[cur].get_joint_offset()));
00387 }
00388 }
00389 if(dest<1)
00390 return;
00391 if(links[dest].get_joint_type()==0) {
00392 Matrix rotz3(3,3);
00393 rotz3 << threebythreeident;
00394 rotz3(1,1)=rotz3(2,2)=cos(-(links[dest].get_theta()-links[dest].get_joint_offset()));
00395 rotz3(1,2)=-(rotz3(2,1)=sin(-(links[dest].get_theta()-links[dest].get_joint_offset())));
00396 R=rotz3*R;
00397 p=rotz3*p;
00398 } else {
00399 p(3)+=-(links[dest].get_d()-links[dest].get_joint_offset());
00400 }
00401 }
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414 ReturnMatrix Robot_basic::convertLink(int cur, int dest) const {
00415 Matrix R;
00416 ColumnVector p;
00417 convertLink(R,p,cur,dest);
00418 return pack4x4(R,p);
00419 }
00420
00421
00422
00423
00424 void Robot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00425 const int j)const
00426
00427
00428
00429
00430
00431
00432
00433
00434 {
00435 if(j < 1 || j > dof)
00436 error("j must be 1 <= j <= dof");
00437
00438 if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00439 pos = ColumnVector(3);
00440 if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00441 pos_dot = ColumnVector(3);
00442
00443 pos = 0.0;
00444 pos_dot = 0.0;
00445 for(int i = 1; i <= j; i++)
00446 {
00447 R[i] = R[i-1]*links[i].R;
00448 pos = pos + R[i-1]*links[i].p;
00449 pos_dot = pos_dot + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
00450 }
00451 Rot = R[j];
00452 }
00453
00454 void Robot::dTdqi(Matrix & dRot, ColumnVector & dpos,
00455 const int i, const int endlink)
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
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498 {
00499 int j;
00500 if(i < 1 || i > endlink) error("i must be 1 <= i <= endlink");
00501 if(links[i].get_immobile()) {
00502 dRot = Matrix(3,3);
00503 dpos = Matrix(3,1);
00504 dRot = 0.0;
00505 dpos = 0.0;
00506 } else if(links[i].get_joint_type() == 0) {
00507 Matrix dR(3,3);
00508 dR = 0.0;
00509 Matrix R2 = links[i].R;
00510 ColumnVector p2 = links[i].p;
00511 dRot = Matrix(3,3);
00512 dRot << threebythreeident;
00513 for(j = 1; j < i; j++) {
00514 dRot = dRot*links[j].R;
00515 }
00516
00517 for(j = 1; j <= 3; j++) {
00518 dR(j,1) = dRot(j,2);
00519 dR(j,2) = -dRot(j,1);
00520 }
00521 for(j = i+1; j <= endlink; j++) {
00522 p2 = p2 + R2*links[j].p;
00523 R2 = R2*links[j].R;
00524 }
00525 dpos = dR*p2;
00526 dRot = dR*R2;
00527 } else {
00528 dRot = Matrix(3,3);
00529 dpos = Matrix(3,1);
00530 dRot = 0.0;
00531 dpos = 0.0;
00532 dpos(3) = 1.0;
00533 for(j = i-1; j >= 1; j--) {
00534 dpos = links[j].R*dpos;
00535 }
00536 }
00537 }
00538
00539 ReturnMatrix Robot::dTdqi(const int i, const int endlink)
00540
00541
00542
00543
00544
00545
00546 {
00547 Matrix dRot, thomo(4,4);
00548 ColumnVector dpos;
00549
00550 dTdqi(dRot, dpos, i, endlink);
00551 thomo = (Real) 0.0;
00552 thomo.SubMatrix(1,3,1,3) = dRot;
00553 thomo.SubMatrix(1,3,4,4) = dpos;
00554 thomo.Release(); return thomo;
00555 }
00556
00557 ReturnMatrix Robot::jacobian(const int endlink, const int ref)const
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606 {
00607 const int adof=get_available_dof(endlink);
00608 Matrix jac(6,adof);
00609 Matrix pr, temp(3,1);
00610
00611 if(ref < 0 || ref > dof) error("invalid referential");
00612
00613 for(int i = 1; i <= dof; i++) {
00614 R[i] = R[i-1]*links[i].R;
00615 p[i] = p[i-1]+R[i-1]*links[i].p;
00616 }
00617
00618 for(int i=1,j=1; j <= adof; i++) {
00619 if(links[i].get_immobile())
00620 continue;
00621 if(links[i].get_joint_type() == 0) {
00622 temp(1,1) = R[i-1](1,3);
00623 temp(2,1) = R[i-1](2,3);
00624 temp(3,1) = R[i-1](3,3);
00625 pr = p[endlink]-p[i-1];
00626 temp = CrossProduct(temp,pr);
00627 jac(1,j) = temp(1,1);
00628 jac(2,j) = temp(2,1);
00629 jac(3,j) = temp(3,1);
00630 jac(4,j) = R[i-1](1,3);
00631 jac(5,j) = R[i-1](2,3);
00632 jac(6,j) = R[i-1](3,3);
00633 } else {
00634 jac(1,j) = R[i-1](1,3);
00635 jac(2,j) = R[i-1](2,3);
00636 jac(3,j) = R[i-1](3,3);
00637 jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00638 }
00639 j++;
00640 }
00641 if(ref != 0) {
00642 Matrix zeros(3,3);
00643 zeros = (Real) 0.0;
00644 Matrix RT = R[ref].t();
00645 Matrix Rot;
00646 Rot = ((RT & zeros) | (zeros & RT));
00647 jac = Rot*jac;
00648 }
00649
00650 jac.Release(); return jac;
00651 }
00652
00653 ReturnMatrix Robot::jacobian_dot(const int ref)const
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701 {
00702 const int adof=get_available_dof();
00703 Matrix jacdot(6,adof);
00704 ColumnVector e(3), temp, pr, ppr;
00705
00706 if(ref < 0 || ref > dof)
00707 error("invalid referential");
00708
00709 for(int i = 1; i <= dof; i++)
00710 {
00711 R[i] = R[i-1]*links[i].R;
00712 p[i] = p[i-1] + R[i-1]*links[i].p;
00713 pp[i] = pp[i-1] + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
00714 }
00715
00716 for(int i=1,j=1; j <= adof; i++) {
00717 if(links[i].get_immobile())
00718 continue;
00719 if(links[i].get_joint_type() == 0)
00720 {
00721 pr = p[dof]-p[i-1];
00722 ppr = pp[dof]-pp[i-1];
00723 e(1) = R[i-1](1,3);
00724 e(2) = R[i-1](2,3);
00725 e(3) = R[i-1](3,3);
00726 temp = CrossProduct(R[i-1]*w[i-1], e);
00727 jacdot(4,j) = temp(1);
00728 jacdot(5,j) = temp(2);
00729 jacdot(6,j) = temp(3);
00730 temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
00731 jacdot(1,j) = temp(1);
00732 jacdot(2,j) = temp(2);
00733 jacdot(3,j) = temp(3);
00734 }
00735 else
00736 jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
00737 jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
00738 j++;
00739 }
00740
00741 if(ref != 0) {
00742 Matrix zeros(3,3);
00743 zeros = (Real) 0.0;
00744 Matrix RT = R[ref].t();
00745 Matrix Rot;
00746 Rot = ((RT & zeros) | (zeros & RT));
00747 jacdot = Rot*jacdot;
00748 }
00749
00750 jacdot.Release(); return jacdot;
00751 }
00752
00753
00754
00755 void mRobot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00756 const int j)const
00757
00758
00759
00760
00761
00762
00763
00764
00765 {
00766 if(j < 1 || j > dof+fix)
00767 error("j must be 1 <= j <= dof+fix");
00768
00769 if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00770 pos = ColumnVector(3);
00771 if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00772 pos_dot = ColumnVector(3);
00773
00774 pos = 0.0;
00775 pos_dot = 0.0;
00776 for(int i = 1; i <= j; i++)
00777 {
00778 pos = pos + R[i-1]*links[i].p;
00779 pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
00780 R[i] = R[i-1]*links[i].R;
00781 }
00782 Rot = R[j];
00783 }
00784
00785 void mRobot::dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
00786
00787
00788
00789
00790
00791
00792
00793
00794
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804
00805
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815
00816
00817
00818
00819
00820
00821 {
00822 int j;
00823 if(i < 1 || i > endlink) error("i must be 1 <= i <= endlink");
00824 if(links[i].get_immobile()) {
00825 dRot = Matrix(3,3);
00826 dpos = Matrix(3,1);
00827 dRot = 0.0;
00828 dpos = 0.0;
00829 } else if(links[i].get_joint_type() == 0) {
00830 Matrix dR(3,3), R2(3,3), p2(3,1);
00831 dR = 0.0;
00832 dRot = Matrix(3,3);
00833 dRot << threebythreeident;
00834 for(j = 1; j <= i; j++) {
00835 dRot = dRot*links[j].R;
00836 }
00837
00838 for(j = 1; j <= 3; j++) {
00839 dR(j,1) = dRot(j,2);
00840 dR(j,2) = -dRot(j,1);
00841 }
00842 if(i < endlink) {
00843 R2 = links[i+1].R;
00844 p2 = links[i+1].p;
00845 } else {
00846 R2 << threebythreeident;
00847 p2 = 0.0;
00848 }
00849 for(j = i+1; j <= endlink; j++) {
00850 p2 = p2 + R2*links[j].p;
00851 R2 = R2*links[j].R;
00852 }
00853 dpos = dR*p2;
00854 dRot = dR*R2;
00855 } else {
00856 dRot = Matrix(3,3);
00857 dpos = Matrix(3,1);
00858 dRot = 0.0;
00859 dpos = 0.0;
00860 dpos(3) = 1.0;
00861 for(j = i; j >= 1; j--) {
00862 dpos = links[j].R*dpos;
00863 }
00864 }
00865 }
00866
00867 ReturnMatrix mRobot::dTdqi(const int i, const int endlink)
00868
00869
00870
00871
00872
00873
00874 {
00875 Matrix dRot, thomo(4,4);
00876 ColumnVector dpos;
00877
00878 dTdqi(dRot, dpos, i, endlink);
00879 thomo = (Real) 0.0;
00880 thomo.SubMatrix(1,3,1,3) = dRot;
00881 thomo.SubMatrix(1,3,4,4) = dpos;
00882 thomo.Release(); return thomo;
00883 }
00884
00885 ReturnMatrix mRobot::jacobian(const int endlink, const int ref)const
00886
00887
00888
00889
00890
00891 {
00892 const int adof=get_available_dof(endlink);
00893 Matrix jac(6,adof);
00894 ColumnVector pr(3), temp(3);
00895
00896 if(ref < 0 || ref > dof+fix)
00897 error("invalid referential");
00898
00899 for(int i = 1; i <= dof+fix; i++) {
00900 R[i] = R[i-1]*links[i].R;
00901 p[i] = p[i-1] + R[i-1]*links[i].p;
00902 }
00903
00904 for(int i=1,j=1; j <= adof; i++) {
00905 if(links[i].get_immobile())
00906 continue;
00907 if(links[i].get_joint_type() == 0){
00908 temp(1) = R[i](1,3);
00909 temp(2) = R[i](2,3);
00910 temp(3) = R[i](3,3);
00911 pr = p[endlink+fix]-p[i];
00912 temp = CrossProduct(temp,pr);
00913 jac(1,j) = temp(1);
00914 jac(2,j) = temp(2);
00915 jac(3,j) = temp(3);
00916 jac(4,j) = R[i](1,3);
00917 jac(5,j) = R[i](2,3);
00918 jac(6,j) = R[i](3,3);
00919 } else {
00920 jac(1,j) = R[i](1,3);
00921 jac(2,j) = R[i](2,3);
00922 jac(3,j) = R[i](3,3);
00923 jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00924 }
00925 j++;
00926 }
00927 if(ref != 0) {
00928 Matrix zeros(3,3);
00929 zeros = (Real) 0.0;
00930 Matrix RT = R[ref].t();
00931 Matrix Rot;
00932 Rot = ((RT & zeros) | (zeros & RT));
00933 jac = Rot*jac;
00934 }
00935 jac.Release(); return jac;
00936 }
00937
00938 ReturnMatrix mRobot::jacobian_dot(const int ref)const
00939
00940
00941
00942
00943
00944 {
00945 const int adof=get_available_dof();
00946 Matrix jacdot(6,adof);
00947 ColumnVector e(3), temp, pr, ppr;
00948
00949 if(ref < 0 || ref > dof+fix)
00950 error("invalid referential");
00951
00952 for(int i = 1; i <= dof+fix; i++)
00953 {
00954 R[i] = R[i-1]*links[i].R;
00955 p[i] = p[i-1] + R[i-1]*links[i].p;
00956 pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
00957 }
00958
00959 for(int i=1,j=1; j <= adof; i++) {
00960 if(links[i].get_immobile())
00961 continue;
00962 if(links[i].get_joint_type() == 0)
00963 {
00964 pr = p[dof+fix]-p[i];
00965 ppr = pp[dof+fix]-pp[i];
00966
00967 e(1) = R[i](1,3);
00968 e(2) = R[i](2,3);
00969 e(3) = R[i](3,3);
00970 temp = CrossProduct(R[i-1]*w[i-1], e);
00971 jacdot(4,j) = temp(1);
00972 jacdot(5,j) = temp(2);
00973 jacdot(6,j) = temp(3);
00974
00975 temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
00976 jacdot(1,j) = temp(1);
00977 jacdot(2,j) = temp(2);
00978 jacdot(3,j) = temp(3);
00979 }
00980 else
00981 jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
00982 jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
00983 j++;
00984 }
00985
00986 if(ref != 0) {
00987 Matrix zeros(3,3);
00988 zeros = (Real) 0.0;
00989 Matrix RT = R[ref].t();
00990 Matrix Rot;
00991 Rot = ((RT & zeros) | (zeros & RT));
00992 jacdot = Rot*jacdot;
00993 }
00994
00995 jacdot.Release(); return jacdot;
00996 }
00997
00998
00999
01000 void mRobot_min_para::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
01001 const int j)const
01002
01003
01004
01005
01006
01007
01008
01009
01010 {
01011 if(j < 1 || j > dof+fix)
01012 error("j must be 1 <= j <= dof+fix");
01013
01014 if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
01015 pos = ColumnVector(3);
01016 if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
01017 pos_dot = ColumnVector(3);
01018
01019 pos = 0.0;
01020 pos_dot = 0.0;
01021 for(int i = 1; i <= j; i++)
01022 {
01023 pos = pos + R[i-1]*links[i].p;
01024 pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
01025 R[i] = R[i-1]*links[i].R;
01026 }
01027 Rot = R[j];
01028 }
01029
01030 void mRobot_min_para::dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink)
01031
01032
01033
01034
01035
01036
01037
01038
01039
01040
01041
01042 {
01043 int j;
01044 if(i < 1 || i > endlink) error("i must be 1 <= i <= endlink");
01045 if(links[i].get_immobile()) {
01046 dRot = Matrix(3,3);
01047 dpos = Matrix(3,1);
01048 dRot = 0.0;
01049 dpos = 0.0;
01050 } else if(links[i].get_joint_type() == 0) {
01051 Matrix dR(3,3), R2, p2(3,1);
01052 dR = 0.0;
01053 dRot = Matrix(3,3);
01054 dRot << threebythreeident;
01055 for(j = 1; j <= i; j++) {
01056 dRot = dRot*links[j].R;
01057 }
01058
01059 for(j = 1; j <= 3; j++) {
01060 dR(j,1) = dRot(j,2);
01061 dR(j,2) = -dRot(j,1);
01062 }
01063 if(i < endlink) {
01064 R2 = links[i+1].R;
01065 p2 = links[i+1].p;
01066 } else {
01067 R2 << threebythreeident;
01068 p2 = 0.0;
01069 }
01070 for(j = i+1; j <= endlink; j++) {
01071 p2 = p2 + R2*links[j].p;
01072 R2 = R2*links[j].R;
01073 }
01074 dpos = dR*p2;
01075 dRot = dR*R2;
01076 } else {
01077 dRot = Matrix(3,3);
01078 dpos = Matrix(3,1);
01079 dRot = 0.0;
01080 dpos = 0.0;
01081 dpos(3) = 1.0;
01082 for(j = i; j >= 1; j--) {
01083 dpos = links[j].R*dpos;
01084 }
01085 }
01086 }
01087
01088 ReturnMatrix mRobot_min_para::dTdqi(const int i, const int endlink)
01089
01090
01091
01092
01093
01094
01095 {
01096 Matrix dRot, thomo(4,4);
01097 ColumnVector dpos;
01098
01099 dTdqi(dRot, dpos, i, endlink);
01100 thomo = (Real) 0.0;
01101 thomo.SubMatrix(1,3,1,3) = dRot;
01102 thomo.SubMatrix(1,3,4,4) = dpos;
01103 thomo.Release(); return thomo;
01104 }
01105
01106 ReturnMatrix mRobot_min_para::jacobian(const int endlink, const int ref)const
01107
01108
01109
01110
01111
01112 {
01113 const int adof=get_available_dof(endlink);
01114 Matrix jac(6,adof);
01115 ColumnVector pr(3), temp(3);
01116
01117 if(ref < 0 || ref > dof+fix)
01118 error("invalid referential");
01119
01120 for(int i = 1; i <= dof+fix; i++) {
01121 R[i] = R[i-1]*links[i].R;
01122 p[i] = p[i-1] + R[i-1]*links[i].p;
01123 }
01124
01125 for(int i=1,j=1; j<=adof; i++) {
01126 if(links[i].get_immobile())
01127 continue;
01128 if(links[i].get_joint_type() == 0){
01129 temp(1) = R[i](1,3);
01130 temp(2) = R[i](2,3);
01131 temp(3) = R[i](3,3);
01132
01133 pr = p[endlink+fix]-p[i];
01134 temp = CrossProduct(temp,pr);
01135 jac(1,j) = temp(1);
01136 jac(2,j) = temp(2);
01137 jac(3,j) = temp(3);
01138 jac(4,j) = R[i](1,3);
01139 jac(5,j) = R[i](2,3);
01140 jac(6,j) = R[i](3,3);
01141 } else {
01142 jac(1,j) = R[i](1,3);
01143 jac(2,j) = R[i](2,3);
01144 jac(3,j) = R[i](3,3);
01145 jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
01146 }
01147 j++;
01148 }
01149 if(ref != 0) {
01150 Matrix zeros(3,3);
01151 zeros = (Real) 0.0;
01152 Matrix RT = R[ref].t();
01153 Matrix Rot;
01154 Rot = ((RT & zeros) | (zeros & RT));
01155 jac = Rot*jac;
01156 }
01157
01158 jac.Release(); return jac;
01159 }
01160
01161 ReturnMatrix mRobot_min_para::jacobian_dot(const int ref)const
01162
01163
01164
01165
01166
01167 {
01168 const int adof=get_available_dof();
01169 Matrix jacdot(6,adof);
01170 ColumnVector e(3), temp, pr, ppr;
01171
01172 if(ref < 0 || ref > dof+fix)
01173 error("invalid referential");
01174
01175 for(int i = 1; i <= dof+fix; i++)
01176 {
01177 R[i] = R[i-1]*links[i].R;
01178 p[i] = p[i-1] + R[i-1]*links[i].p;
01179 pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
01180 }
01181
01182 for(int i=1,j=1; j <= adof; i++) {
01183 if(links[i].get_immobile())
01184 continue;
01185 if(links[i].get_joint_type() == 0)
01186 {
01187 pr = p[dof+fix]-p[i];
01188 ppr = pp[dof+fix]-pp[i];
01189
01190 e(1) = R[i](1,3);
01191 e(2) = R[i](2,3);
01192 e(3) = R[i](3,3);
01193 temp = CrossProduct(R[i-1]*w[i-1], e);
01194 jacdot(4,j) = temp(1);
01195 jacdot(5,j) = temp(2);
01196 jacdot(6,j) = temp(3);
01197
01198 temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
01199 jacdot(1,j) = temp(1);
01200 jacdot(2,j) = temp(2);
01201 jacdot(3,j) = temp(3);
01202 }
01203 else
01204 jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
01205 jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
01206 j++;
01207 }
01208
01209 if(ref != 0) {
01210 Matrix zeros(3,3);
01211 zeros = (Real) 0.0;
01212 Matrix RT = R[ref].t();
01213 Matrix Rot;
01214 Rot = ((RT & zeros) | (zeros & RT));
01215 jacdot = Rot*jacdot;
01216 }
01217
01218 jacdot.Release(); return jacdot;
01219 }
01220
01221 #ifdef use_namespace
01222 }
01223 #endif