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