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
00081
00082 static const char rcsid[] = "$Id: kinemat.cpp,v 1.31 2004/08/16 00:37:53 gourdeau Exp $";
00083
00084 #include "robot.h"
00085
00086 #ifdef use_namespace
00087 namespace ROBOOP {
00088 using namespace NEWMAT;
00089 #endif
00090
00091
00092 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos)const
00098 {
00099 kine(Rot,pos,dof+fix);
00100 }
00101
00102 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos, const int j)const
00109 {
00110 if(j < 1 || j > dof+fix) error("j must be 1 <= j <= dof+fix");
00111
00112 Rot = links[1].R;
00113 pos = links[1].p;
00114 for (int i = 2; i <= j; i++) {
00115 pos = pos + Rot*links[i].p;
00116 Rot = Rot*links[i].R;
00117 }
00118 }
00119
00120 ReturnMatrix Robot_basic::kine(void)const
00122 {
00123 Matrix thomo;
00124
00125 thomo = kine(dof+fix);
00126 thomo.Release(); return thomo;
00127 }
00128
00129 ReturnMatrix Robot_basic::kine(const int j)const
00131 {
00132 Matrix Rot, thomo(4,4);
00133 ColumnVector pos;
00134
00135 kine(Rot,pos,j);
00136 thomo << fourbyfourident;
00137 thomo.SubMatrix(1,3,1,3) = Rot;
00138 thomo.SubMatrix(1,3,4,4) = pos;
00139 thomo.Release(); return thomo;
00140 }
00141
00142 ReturnMatrix Robot_basic::kine_pd(const int j)const
00153 {
00154 Matrix temp(3,5), Rot;
00155 ColumnVector pos, pos_dot;
00156
00157 if(j < 1 || j > dof)
00158 error("j must be 1 <= j <= dof");
00159
00160 kine_pd(Rot, pos, pos_dot, j);
00161
00162 temp.SubMatrix(1,3,1,3) = Rot;
00163 temp.SubMatrix(1,3,4,4) = pos;
00164 temp.SubMatrix(1,3,5,5) = pos_dot;
00165 temp.Release();
00166 return temp;
00167 }
00168
00169 ReturnMatrix Robot_basic::jacobian_DLS_inv(const double eps, const double lambda_max,
00170 const int ref)const
00200 {
00201 Matrix jacob_inv_DLS, U, V;
00202 DiagonalMatrix Q;
00203 SVD(jacobian(ref), Q, U, V);
00204
00205 if(Q(6,6) >= eps)
00206 jacob_inv_DLS = V*Q.i()*U.t();
00207 else
00208 {
00209 Q(6,6) += (1 - pow(Q(6,6)/eps,2))*lambda_max*lambda_max;
00210 jacob_inv_DLS = V*Q.i()*U.t();
00211 }
00212
00213 jacob_inv_DLS.Release();
00214 return(jacob_inv_DLS);
00215 }
00216
00217
00218
00219 void Robot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00220 const int j)const
00229 {
00230 if(j < 1 || j > dof)
00231 error("j must be 1 <= j <= dof");
00232
00233 if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00234 pos = ColumnVector(3);
00235 if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00236 pos_dot = ColumnVector(3);
00237
00238 pos = 0.0;
00239 pos_dot = 0.0;
00240 for(int i = 1; i <= j; i++)
00241 {
00242 R[i] = R[i-1]*links[i].R;
00243 pos = pos + R[i-1]*links[i].p;
00244 pos_dot = pos_dot + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
00245 }
00246 Rot = R[j];
00247 }
00248
00249 void Robot::dTdqi(Matrix & dRot, ColumnVector & dp,
00250 const int i)
00293 {
00294 int j;
00295 if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00296 if(links[i].get_immobile()) {
00297 dRot = Matrix(3,3);
00298 dp = Matrix(3,1);
00299 dRot = 0.0;
00300 dp = 0.0;
00301 } else if(links[i].get_joint_type() == 0) {
00302 Matrix dR(3,3);
00303 dR = 0.0;
00304 Matrix R2 = links[i].R;
00305 ColumnVector p2 = links[i].p;
00306 dRot = Matrix(3,3);
00307 dRot << threebythreeident;
00308 for(j = 1; j < i; j++) {
00309 dRot = dRot*links[j].R;
00310 }
00311
00312 for(j = 1; j <= 3; j++) {
00313 dR(j,1) = dRot(j,2);
00314 dR(j,2) = -dRot(j,1);
00315 }
00316 for(j = i+1; j <= dof; j++) {
00317 p2 = p2 + R2*links[j].p;
00318 R2 = R2*links[j].R;
00319 }
00320 dp = dR*p2;
00321 dRot = dR*R2;
00322 } else {
00323 dRot = Matrix(3,3);
00324 dp = Matrix(3,1);
00325 dRot = 0.0;
00326 dp = 0.0;
00327 dp(3) = 1.0;
00328 for(j = i-1; j >= 1; j--) {
00329 dp = links[j].R*dp;
00330 }
00331 }
00332 }
00333
00334 ReturnMatrix Robot::dTdqi(const int i)
00341 {
00342 Matrix dRot, thomo(4,4);
00343 ColumnVector dpos;
00344
00345 dTdqi(dRot, dpos, i);
00346 thomo = (Real) 0.0;
00347 thomo.SubMatrix(1,3,1,3) = dRot;
00348 thomo.SubMatrix(1,3,4,4) = dpos;
00349 thomo.Release(); return thomo;
00350 }
00351
00352 ReturnMatrix Robot::jacobian(const int endlink, const int ref)const
00401 {
00402 int i, j;
00403 const int adof=get_available_dof(endlink);
00404 Matrix jac(6,adof);
00405 Matrix pr, temp(3,1);
00406
00407 if(ref < 0 || ref > dof) error("invalid referential");
00408
00409 for(i = 1; i <= dof; i++) {
00410 R[i] = R[i-1]*links[i].R;
00411 p[i] = p[i-1]+R[i-1]*links[i].p;
00412 }
00413
00414 for(i=1,j=1; j <= adof; i++) {
00415 if(links[i].get_immobile())
00416 continue;
00417 if(links[i].get_joint_type() == 0) {
00418 temp(1,1) = R[i-1](1,3);
00419 temp(2,1) = R[i-1](2,3);
00420 temp(3,1) = R[i-1](3,3);
00421 pr = p[dof]-p[i-1];
00422 temp = CrossProduct(temp,pr);
00423 jac(1,j) = temp(1,1);
00424 jac(2,j) = temp(2,1);
00425 jac(3,j) = temp(3,1);
00426 jac(4,j) = R[i-1](1,3);
00427 jac(5,j) = R[i-1](2,3);
00428 jac(6,j) = R[i-1](3,3);
00429 } else {
00430 jac(1,j) = R[i-1](1,3);
00431 jac(2,j) = R[i-1](2,3);
00432 jac(3,j) = R[i-1](3,3);
00433 jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00434 }
00435 j++;
00436 }
00437
00438 if(ref != 0) {
00439 Matrix zeros(3,3);
00440 zeros = (Real) 0.0;
00441 Matrix RT = R[ref].t();
00442 Matrix Rot;
00443 Rot = ((RT & zeros) | (zeros & RT));
00444 jac = Rot*jac;
00445 }
00446 jac.Release(); return jac;
00447 }
00448
00449 ReturnMatrix Robot::jacobian_dot(const int ref)const
00497 {
00498 int i, j;
00499 const int adof=get_available_dof();
00500 Matrix jacdot(6,adof);
00501 ColumnVector e(3), temp, pr, ppr;
00502
00503 if(ref < 0 || ref > dof)
00504 error("invalid referential");
00505
00506 for(i = 1; i <= dof; i++)
00507 {
00508 R[i] = R[i-1]*links[i].R;
00509 p[i] = p[i-1] + R[i-1]*links[i].p;
00510 pp[i] = pp[i-1] + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
00511 }
00512
00513 for(i=1,j=1; j <= adof; i++) {
00514 if(links[i].get_immobile())
00515 continue;
00516 if(links[i].get_joint_type() == 0)
00517 {
00518 pr = p[dof]-p[i-1];
00519 ppr = pp[dof]-pp[i-1];
00520 e(1) = R[i-1](1,3);
00521 e(2) = R[i-1](2,3);
00522 e(3) = R[i-1](3,3);
00523 temp = CrossProduct(R[i-1]*w[i-1], e);
00524 jacdot(4,j) = temp(1);
00525 jacdot(5,j) = temp(2);
00526 jacdot(6,j) = temp(3);
00527 temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
00528 jacdot(1,j) = temp(1);
00529 jacdot(2,j) = temp(2);
00530 jacdot(3,j) = temp(3);
00531 }
00532 else
00533 jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
00534 jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
00535 j++;
00536 }
00537
00538 if(ref != 0) {
00539 Matrix zeros(3,3);
00540 zeros = (Real) 0.0;
00541 Matrix RT = R[ref].t();
00542 Matrix Rot;
00543 Rot = ((RT & zeros) | (zeros & RT));
00544 jacdot = Rot*jacdot;
00545 }
00546
00547 jacdot.Release(); return jacdot;
00548 }
00549
00550
00551
00552 void mRobot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00553 const int j)const
00562 {
00563 if(j < 1 || j > dof+fix)
00564 error("j must be 1 <= j <= dof+fix");
00565
00566 if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00567 pos = ColumnVector(3);
00568 if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00569 pos_dot = ColumnVector(3);
00570
00571 pos = 0.0;
00572 pos_dot = 0.0;
00573 for(int i = 1; i <= j; i++)
00574 {
00575 pos = pos + R[i-1]*links[i].p;
00576 pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
00577 R[i] = R[i-1]*links[i].R;
00578 }
00579 Rot = R[j];
00580 }
00581
00582 void mRobot::dTdqi(Matrix & dRot, ColumnVector & dp, const int i)
00618 {
00619 int j;
00620 if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00621 if(links[i].get_immobile()) {
00622 dRot = Matrix(3,3);
00623 dp = Matrix(3,1);
00624 dRot = 0.0;
00625 dp = 0.0;
00626 } else if(links[i].get_joint_type() == 0) {
00627 Matrix dR(3,3), R2(3,3), p2(3,1);
00628 dR = 0.0;
00629 dRot = Matrix(3,3);
00630 dRot << threebythreeident;
00631 for(j = 1; j <= i; j++) {
00632 dRot = dRot*links[j].R;
00633 }
00634
00635 for(j = 1; j <= 3; j++) {
00636 dR(j,1) = dRot(j,2);
00637 dR(j,2) = -dRot(j,1);
00638 }
00639 if(i < dof) {
00640 R2 = links[i+1].R;
00641 p2 = links[i+1].p;
00642 } else {
00643 R2 << threebythreeident;
00644 p2 = 0.0;
00645 }
00646 for(j = i+1; j <= dof; j++) {
00647 p2 = p2 + R2*links[j].p;
00648 R2 = R2*links[j].R;
00649 }
00650 dp = dR*p2;
00651 dRot = dR*R2;
00652 } else {
00653 dRot = Matrix(3,3);
00654 dp = Matrix(3,1);
00655 dRot = 0.0;
00656 dp = 0.0;
00657 dp(3) = 1.0;
00658 for(j = i; j >= 1; j--) {
00659 dp = links[j].R*dp;
00660 }
00661 }
00662 }
00663
00664 ReturnMatrix mRobot::dTdqi(const int i)
00671 {
00672 Matrix dRot, thomo(4,4);
00673 ColumnVector dpos;
00674
00675 dTdqi(dRot, dpos, i);
00676 thomo = (Real) 0.0;
00677 thomo.SubMatrix(1,3,1,3) = dRot;
00678 thomo.SubMatrix(1,3,4,4) = dpos;
00679 thomo.Release(); return thomo;
00680 }
00681
00682 ReturnMatrix mRobot::jacobian(const int endlink, const int ref)const
00688 {
00689 int i, j;
00690 const int adof=get_available_dof(endlink);
00691 Matrix jac(6,adof);
00692 ColumnVector pr(3), temp(3);
00693
00694 if(ref < 0 || ref > dof+fix)
00695 error("invalid referential");
00696
00697 for(i = 1; i <= dof+fix; i++) {
00698 R[i] = R[i-1]*links[i].R;
00699 p[i] = p[i-1] + R[i-1]*links[i].p;
00700 }
00701
00702 for(i=1,j=1; j <= adof; i++) {
00703 if(links[i].get_immobile())
00704 continue;
00705 if(links[i].get_joint_type() == 0){
00706 temp(1) = R[i](1,3);
00707 temp(2) = R[i](2,3);
00708 temp(3) = R[i](3,3);
00709 pr = p[dof+fix]-p[i];
00710 temp = CrossProduct(temp,pr);
00711 jac(1,j) = temp(1);
00712 jac(2,j) = temp(2);
00713 jac(3,j) = temp(3);
00714 jac(4,j) = R[i](1,3);
00715 jac(5,j) = R[i](2,3);
00716 jac(6,j) = R[i](3,3);
00717 } else {
00718 jac(1,j) = R[i](1,3);
00719 jac(2,j) = R[i](2,3);
00720 jac(3,j) = R[i](3,3);
00721 jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00722 }
00723 j++;
00724 }
00725 if(ref != 0) {
00726 Matrix zeros(3,3);
00727 zeros = (Real) 0.0;
00728 Matrix RT = R[ref].t();
00729 Matrix Rot;
00730 Rot = ((RT & zeros) | (zeros & RT));
00731 jac = Rot*jac;
00732 }
00733 jac.Release(); return jac;
00734 }
00735
00736 ReturnMatrix mRobot::jacobian_dot(const int ref)const
00742 {
00743 int i, j;
00744 const int adof=get_available_dof();
00745 Matrix jacdot(6,adof);
00746 ColumnVector e(3), temp, pr, ppr;
00747
00748 if(ref < 0 || ref > dof+fix)
00749 error("invalid referential");
00750
00751 for(i = 1; i <= dof+fix; i++)
00752 {
00753 R[i] = R[i-1]*links[i].R;
00754 p[i] = p[i-1] + R[i-1]*links[i].p;
00755 pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
00756 }
00757
00758 for(i=1,j=1; j <= adof; i++) {
00759 if(links[i].get_immobile())
00760 continue;
00761 if(links[i].get_joint_type() == 0)
00762 {
00763 pr = p[dof+fix]-p[i];
00764 ppr = pp[dof+fix]-pp[i];
00765
00766 e(1) = R[i](1,3);
00767 e(2) = R[i](2,3);
00768 e(3) = R[i](3,3);
00769 temp = CrossProduct(R[i-1]*w[i-1], e);
00770 jacdot(4,j) = temp(1);
00771 jacdot(5,j) = temp(2);
00772 jacdot(6,j) = temp(3);
00773
00774 temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
00775 jacdot(1,j) = temp(1);
00776 jacdot(2,j) = temp(2);
00777 jacdot(3,j) = temp(3);
00778 }
00779 else
00780 jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
00781 jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
00782 j++;
00783 }
00784
00785 if(ref != 0) {
00786 Matrix zeros(3,3);
00787 zeros = (Real) 0.0;
00788 Matrix RT = R[ref].t();
00789 Matrix Rot;
00790 Rot = ((RT & zeros) | (zeros & RT));
00791 jacdot = Rot*jacdot;
00792 }
00793
00794 jacdot.Release(); return jacdot;
00795 }
00796
00797
00798
00799 void mRobot_min_para::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00800 const int j)const
00809 {
00810 if(j < 1 || j > dof+fix)
00811 error("j must be 1 <= j <= dof+fix");
00812
00813 if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00814 pos = ColumnVector(3);
00815 if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00816 pos_dot = ColumnVector(3);
00817
00818 pos = 0.0;
00819 pos_dot = 0.0;
00820 for(int i = 1; i <= j; i++)
00821 {
00822 pos = pos + R[i-1]*links[i].p;
00823 pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
00824 R[i] = R[i-1]*links[i].R;
00825 }
00826 Rot = R[j];
00827 }
00828
00829 void mRobot_min_para::dTdqi(Matrix & dRot, ColumnVector & dp, const int i)
00841 {
00842 int j;
00843 if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00844 if(links[i].get_immobile()) {
00845 dRot = Matrix(3,3);
00846 dp = Matrix(3,1);
00847 dRot = 0.0;
00848 dp = 0.0;
00849 } else if(links[i].get_joint_type() == 0) {
00850 Matrix dR(3,3), R2, p2(3,1);
00851 dR = 0.0;
00852 dRot = Matrix(3,3);
00853 dRot << threebythreeident;
00854 for(j = 1; j <= i; j++) {
00855 dRot = dRot*links[j].R;
00856 }
00857
00858 for(j = 1; j <= 3; j++) {
00859 dR(j,1) = dRot(j,2);
00860 dR(j,2) = -dRot(j,1);
00861 }
00862 if(i < dof) {
00863 R2 = links[i+1].R;
00864 p2 = links[i+1].p;
00865 } else {
00866 R2 << threebythreeident;
00867 p2 = 0.0;
00868 }
00869 for(j = i+1; j <= dof; j++) {
00870 p2 = p2 + R2*links[j].p;
00871 R2 = R2*links[j].R;
00872 }
00873 dp = dR*p2;
00874 dRot = dR*R2;
00875 } else {
00876 dRot = Matrix(3,3);
00877 dp = Matrix(3,1);
00878 dRot = 0.0;
00879 dp = 0.0;
00880 dp(3) = 1.0;
00881 for(j = i; j >= 1; j--) {
00882 dp = links[j].R*dp;
00883 }
00884 }
00885 }
00886
00887 ReturnMatrix mRobot_min_para::dTdqi(const int i)
00894 {
00895 Matrix dRot, thomo(4,4);
00896 ColumnVector dpos;
00897
00898 dTdqi(dRot, dpos, i);
00899 thomo = (Real) 0.0;
00900 thomo.SubMatrix(1,3,1,3) = dRot;
00901 thomo.SubMatrix(1,3,4,4) = dpos;
00902 thomo.Release(); return thomo;
00903 }
00904
00905 ReturnMatrix mRobot_min_para::jacobian(const int endlink, const int ref)const
00911 {
00912 int i, j;
00913 const int adof=get_available_dof(endlink);
00914 Matrix jac(6,adof);
00915 ColumnVector pr(3), temp(3);
00916
00917 if(ref < 0 || ref > dof+fix)
00918 error("invalid referential");
00919
00920 for(i = 1; i <= dof+fix; i++) {
00921 R[i] = R[i-1]*links[i].R;
00922 p[i] = p[i-1] + R[i-1]*links[i].p;
00923 }
00924
00925 for(i=1,j=1; j<=adof; i++) {
00926 if(links[i].get_immobile())
00927 continue;
00928 if(links[i].get_joint_type() == 0){
00929 temp(1) = R[i](1,3);
00930 temp(2) = R[i](2,3);
00931 temp(3) = R[i](3,3);
00932
00933 pr = p[dof+fix]-p[i];
00934 temp = CrossProduct(temp,pr);
00935 jac(1,j) = temp(1);
00936 jac(2,j) = temp(2);
00937 jac(3,j) = temp(3);
00938 jac(4,j) = R[i](1,3);
00939 jac(5,j) = R[i](2,3);
00940 jac(6,j) = R[i](3,3);
00941 } else {
00942 jac(1,j) = R[i](1,3);
00943 jac(2,j) = R[i](2,3);
00944 jac(3,j) = R[i](3,3);
00945 jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00946 }
00947 j++;
00948 }
00949 if(ref != 0) {
00950 Matrix zeros(3,3);
00951 zeros = (Real) 0.0;
00952 Matrix RT = R[ref].t();
00953 Matrix Rot;
00954 Rot = ((RT & zeros) | (zeros & RT));
00955 jac = Rot*jac;
00956 }
00957
00958 jac.Release(); return jac;
00959 }
00960
00961 ReturnMatrix mRobot_min_para::jacobian_dot(const int ref)const
00967 {
00968 int i, j;
00969 const int adof=get_available_dof();
00970 Matrix jacdot(6,adof);
00971 ColumnVector e(3), temp, pr, ppr;
00972
00973 if(ref < 0 || ref > dof+fix)
00974 error("invalid referential");
00975
00976 for(i = 1; i <= dof+fix; i++)
00977 {
00978 R[i] = R[i-1]*links[i].R;
00979 p[i] = p[i-1] + R[i-1]*links[i].p;
00980 pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
00981 }
00982
00983 for(i=1,j=1; j <= dof; i++) {
00984 if(links[i].get_immobile())
00985 continue;
00986 if(links[i].get_joint_type() == 0)
00987 {
00988 pr = p[dof+fix]-p[i];
00989 ppr = pp[dof+fix]-pp[i];
00990
00991 e(1) = R[i](1,3);
00992 e(2) = R[i](2,3);
00993 e(3) = R[i](3,3);
00994 temp = CrossProduct(R[i-1]*w[i-1], e);
00995 jacdot(4,j) = temp(1);
00996 jacdot(5,j) = temp(2);
00997 jacdot(6,j) = temp(3);
00998
00999 temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
01000 jacdot(1,j) = temp(1);
01001 jacdot(2,j) = temp(2);
01002 jacdot(3,j) = temp(3);
01003 }
01004 else
01005 jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
01006 jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
01007 j++;
01008 }
01009
01010 if(ref != 0) {
01011 Matrix zeros(3,3);
01012 zeros = (Real) 0.0;
01013 Matrix RT = R[ref].t();
01014 Matrix Rot;
01015 Rot = ((RT & zeros) | (zeros & RT));
01016 jacdot = Rot*jacdot;
01017 }
01018
01019 jacdot.Release(); return jacdot;
01020 }
01021
01022 #ifdef use_namespace
01023 }
01024 #endif