00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00030
00031 static const char rcsid[] = "$Id: stewart.cpp,v 1.6 2006/05/16 19:24:26 gourdeau Exp $";
00032
00033 #include "config.h"
00034 #include "stewart.h"
00035
00036 #ifdef use_namespace
00037 namespace ROBOOP {
00038 #endif
00039
00040
00045 LinkStewart::LinkStewart ()
00046 {
00047 b = ColumnVector(3);
00048 b = 0.0;
00049 ap = ColumnVector(3);
00050 ap = 0.0;
00051
00052 I1aa = 0.0;
00053 I1nn = 0.0;
00054 I2aa = 0.0;
00055 I2nn = 0.0;
00056 m1 = 0.0;
00057 m2 = 0.0;
00058 Lenght1 = 0.0;
00059 Lenght2 = 0.0;
00060
00061 ColumnVector ZeroValue(3);
00062 ZeroValue<<0.0<<0.0<<0.0;
00063
00064 UnitV = ZeroValue;
00065 aPos = ZeroValue;
00066 Vu = ZeroValue;
00067 Vc = ZeroValue;
00068 Vv = ZeroValue;
00069 da = ZeroValue;
00070 dda = ZeroValue;
00071 LOmega = ZeroValue;
00072 LAlpha = ZeroValue;
00073 ACM1 = ZeroValue;
00074 N = ZeroValue;
00075 gravity = ZeroValue;
00076 L = 0.0;
00077 }
00078
00087 LinkStewart::LinkStewart (const ColumnVector & InitLink, const Matrix wRp, const ColumnVector q)
00088 {
00089 b = InitLink.Rows(1,3);
00090 ap = InitLink.Rows(4,6);
00091 I1aa = InitLink(7);
00092 I1nn = InitLink(8);
00093 I2aa = InitLink(9);
00094 I2nn = InitLink(10);
00095 m1 = InitLink(11);
00096 m2 = InitLink(12);
00097 Lenght1 = InitLink(13);
00098 Lenght2 = InitLink(14);
00099
00100 ColumnVector ZeroValue(3);
00101 ZeroValue<<0.0<<0.0<<0.0;
00102
00103 UnitV = ZeroValue;
00104 aPos = ZeroValue;
00105 Vc = ZeroValue;
00106 Vv = ZeroValue;
00107 da = ZeroValue;
00108 dda = ZeroValue;
00109 LOmega = ZeroValue;
00110 LAlpha = ZeroValue;
00111 ACM1 = ZeroValue;
00112 N = ZeroValue;
00113 gravity = ZeroValue;
00114 L = 0.0;
00115
00116 aPos = Find_a(wRp,q);
00117 L = Find_Lenght();
00118 UnitV = Find_UnitV();
00119 Vu = Find_VctU();
00120 }
00121
00126 LinkStewart::LinkStewart(const LinkStewart & x)
00127 {
00128 b = x.b;
00129 ap = x.ap;
00130 I1aa = x.I1aa;
00131 I1nn = x.I1nn;
00132 I2aa = x.I2aa;
00133 I2nn = x.I2nn;
00134 m1 = x.m1;
00135 m2 = x.m2;
00136 Lenght1 = x.Lenght1;
00137 Lenght2 = x.Lenght2;
00138
00139 UnitV = x.UnitV;
00140 aPos = x.aPos;
00141 Vu = x.Vu;
00142 Vc = x.Vc;
00143 Vv = x.Vv;
00144 da = x.da;
00145 dda = x.dda;
00146 LOmega = x.LOmega;
00147 LAlpha = x.LAlpha;
00148 ACM1 = x.ACM1;
00149 N = x.N;
00150 gravity = x.gravity;
00151 L = x.L;
00152 }
00153
00154 LinkStewart::~LinkStewart()
00156 {
00157 }
00158
00159 const LinkStewart & LinkStewart::operator = (const LinkStewart& x)
00160 {
00162 b = x.b;
00163 ap = x.ap;
00164 I1aa = x.I1aa;
00165 I1nn = x.I1nn;
00166 I2aa = x.I2aa;
00167 I2nn = x.I2nn;
00168 m1 = x.m1;
00169 m2 = x.m2;
00170 Lenght1 = x.Lenght1;
00171 Lenght2 = x.Lenght2;
00172
00173 UnitV = x.UnitV;
00174 aPos = x.aPos;
00175 Vu = x.Vu;
00176 Vc = x.Vc;
00177 Vv = x.Vv;
00178 da = x.da;
00179 dda = x.dda;
00180 LOmega = x.LOmega;
00181 LAlpha = x.LAlpha;
00182 ACM1 = x.ACM1;
00183 N = x.N;
00184 gravity = x.gravity;
00185 L = x.L;
00186 return *this;
00187 }
00188
00189 void LinkStewart::set_b(const ColumnVector Newb)
00191 {
00192 if(Newb.Nrows() == 3)
00193 b = Newb;
00194 else
00195 cerr<< "LinkStewart::set_b: wrong size in input vector."<< endl;
00196 }
00197
00198 void LinkStewart::set_ap(const ColumnVector NewAp)
00200 {
00201 if(NewAp.Nrows()== 3)
00202 ap = NewAp;
00203 else
00204 cerr<< "LinkStewart::set_Ap: wrong size in input vector."<< endl;
00205 }
00206
00207 void LinkStewart::set_I1aa(const Real NewI1aa)
00208
00209 {
00210 I1aa = NewI1aa;
00211 }
00212
00213 void LinkStewart::set_I1nn(const Real NewI1nn)
00214
00215 {
00216 I1nn = NewI1nn;
00217 }
00218
00219 void LinkStewart::set_I2aa(const Real NewI2aa)
00220
00221 {
00222 I2aa = NewI2aa;
00223 }
00224
00225 void LinkStewart::set_I2nn(const Real NewI2nn)
00226
00227 {
00228 I2nn = NewI2nn;
00229 }
00230
00231 void LinkStewart::set_m1(const Real Newm1)
00232
00233 {
00234 m1 = Newm1;
00235 }
00236
00237 void LinkStewart::set_m2(const Real Newm2)
00238
00239 {
00240 m2 = Newm2;
00241 }
00242
00243 void LinkStewart::set_Lenght1(const Real NewLenght1)
00245 {
00246 Lenght1 = NewLenght1;
00247 }
00248
00249 void LinkStewart::set_Lenght2(const Real NewLenght2)
00251 {
00252 Lenght2 = NewLenght2;
00253 }
00254
00255 ReturnMatrix LinkStewart::get_ap() const
00257 {
00258 return ap;
00259 }
00260
00261 ReturnMatrix LinkStewart::get_b() const
00263 {
00264 return b;
00265 }
00266
00267 Real LinkStewart::get_I1aa() const
00269 {
00270 return I1aa;
00271 }
00272
00273 Real LinkStewart::get_I1nn() const
00275 {
00276 return I1nn;
00277 }
00278
00279 Real LinkStewart::get_I2aa() const
00281 {
00282 return I2aa;
00283 }
00284
00285 Real LinkStewart::get_I2nn() const
00287 {
00288 return I2nn;
00289 }
00290
00291 Real LinkStewart::get_m1() const
00293 {
00294 return m1;
00295 }
00296
00297 Real LinkStewart::get_m2() const
00299 {
00300 return m2;
00301 }
00302
00303 Real LinkStewart::get_Lenght1() const
00305 {
00306 return Lenght1;
00307 }
00308
00309 Real LinkStewart::get_Lenght2() const
00311 {
00312 return Lenght2;
00313 }
00314
00315 void LinkStewart::LTransform(const Matrix wRp, const ColumnVector q)
00321 {
00322 aPos = Find_a(wRp,q);
00323 L = Find_Lenght();
00324 UnitV = Find_UnitV();
00325 Vv = Find_VctV();
00326 Vc = Find_VctC();
00327 }
00328
00329 void LinkStewart::d_LTransform(const ColumnVector dq, const ColumnVector Omega,
00330 const Real dl, const Real ddl)
00338 {
00339 Matrix AngularKin;
00340
00341 da = Find_da(dq,Omega);
00342 AngularKin = Find_AngularKin(dl, ddl);
00343 LOmega = AngularKin.Column(1);
00344 LAlpha = AngularKin.Column(2);
00345 }
00346
00347 void LinkStewart::dd_LTransform(const ColumnVector ddq, const ColumnVector Omega,
00348 const ColumnVector Alpha, const Real dl, const Real ddl)
00357 {
00358 Matrix AngularKin;
00359
00360 dda = Find_dda(ddq,Omega,Alpha);
00361 AngularKin = Find_AngularKin(dl, ddl);
00362 LOmega = AngularKin.Column(1);
00363 LAlpha = AngularKin.Column(2);
00364 }
00365
00366 void LinkStewart::tau_LTransform(const Real dl, const Real ddl,const Real Gravity)
00373 {
00374 ACM1 = Find_ACM1(dl,ddl);
00375 N = Find_N(Gravity);
00376 }
00377
00378 ReturnMatrix LinkStewart::Find_a(const Matrix wRp, const ColumnVector q)
00396 {
00397 ColumnVector a;
00398 a = q.Rows(1,3) + wRp*ap;
00399 a.Release();
00400 return a;
00401 }
00402
00416 ReturnMatrix LinkStewart::Find_UnitV()
00417 {
00418 Matrix Tmp (1,3);
00419 Tmp = (aPos - b)/L;
00420 Tmp.Release();
00421 return Tmp;
00422 }
00423
00438 ReturnMatrix LinkStewart::Find_da(const ColumnVector dq, const ColumnVector Omega)
00439 {
00440 ColumnVector da;
00441
00442 da = dq.Rows(1,3) + CrossProduct(Omega,aPos);
00443
00444 da.Release();
00445 return da;
00446 }
00447
00464 ReturnMatrix LinkStewart::Find_dda(const ColumnVector ddq,const ColumnVector Omega,const ColumnVector Alpha)
00465 {
00466 ColumnVector dda;
00467 dda = ddq.Rows(1,3) + CrossProduct(Alpha,aPos) +
00468 CrossProduct(Omega,CrossProduct(Omega,aPos));
00469
00470 dda.Release();
00471 return dda;
00472 }
00473
00486 Real LinkStewart::Find_Lenght()
00487 {
00488 return sqrt(DotProduct((aPos - b),(aPos - b)));
00489 }
00490
00503 ReturnMatrix LinkStewart::Find_VctU()
00504 {
00505 ColumnVector u(3);
00506
00507 u(1) = -UnitV(1)/sqrt(UnitV(1)*UnitV(1) + UnitV(3)*UnitV(3));
00508 u(2) = 0.0;
00509 u(3) = -UnitV(3)/sqrt(UnitV(1)*UnitV(1) + UnitV(3)*UnitV(3));
00510
00511 u.Release();
00512 return u;
00513 }
00514
00527 ReturnMatrix LinkStewart::Find_VctV()
00528 {
00529 ColumnVector v;
00530 v = CrossProduct(Vu,UnitV)/(CrossProduct(Vu,UnitV).NormFrobenius());
00531
00532 v.Release();
00533 return v;
00534 }
00535
00548 ReturnMatrix LinkStewart::Find_VctC()
00549 {
00550 return CrossProduct(Vu, Vv);
00551 }
00552
00585 ReturnMatrix LinkStewart::Find_AngularKin(const Real dl, const Real ddl)
00586 {
00587 Matrix Temp(3,2);
00588 ColumnVector tmp_dda(3), omega(3), alpha(3);
00589 Real wu, wv, au, av;
00590
00591 wu = DotProduct(-(da-dl*UnitV),Vv/(DotProduct(L*UnitV,Vc)));
00592 wv = DotProduct((da-dl*UnitV),Vu/(DotProduct(L*UnitV,Vc)));
00593
00594 omega = wu*Vu + wv*Vv;
00595
00596 tmp_dda = dda - wu*wv*L*CrossProduct(Vc,UnitV)-ddl*UnitV-2*dl*CrossProduct(omega,UnitV)
00597 -L*CrossProduct(omega,CrossProduct(omega,UnitV));
00598
00599 au = DotProduct(-tmp_dda,Vv/(L*DotProduct(UnitV,Vc)));
00600 av = DotProduct(tmp_dda,Vu/(L*DotProduct(UnitV,Vc)));
00601
00602 alpha = au*Vu + av*Vv + wu*wv*Vc;
00603
00604 Temp.Column(1) = omega; Temp.Column(2) = alpha;
00605
00606 Temp.Release(); return Temp;
00607 }
00608
00641 ReturnMatrix LinkStewart::Find_N(const Real Gravity)
00642 {
00643 ColumnVector Accel2(3);
00644 Matrix Fn(3,1), N_N(3,1), I1(3,3), I2(3,3);
00645
00646 gravity = 0;
00647 gravity(2) = -Gravity;
00648
00649 Accel2 = Lenght2*CrossProduct(LOmega,CrossProduct(LOmega, UnitV))
00650 + Lenght2*CrossProduct(LAlpha,UnitV);
00651
00652 IdentityMatrix Identity(3);
00653
00654 I1 = I1aa*UnitV*UnitV.t() + I1nn*(Identity - UnitV*UnitV.t());
00655 I2 = I2aa*UnitV*UnitV.t() + I2nn*(Identity - UnitV*UnitV.t());
00656
00657 N_N = -m1*(L-Lenght1)*CrossProduct(UnitV, gravity) - m2*Lenght2*CrossProduct(UnitV, gravity) +
00658 (I1+I2)*LAlpha - (I1+I2)*CrossProduct(LOmega,LOmega)
00659 + m1*(L-Lenght1)*CrossProduct(UnitV,ACM1)
00660 +m2*Lenght2*CrossProduct(UnitV,Accel2);
00661
00662 N_N.Release();
00663 return N_N;
00664 }
00665
00666 ReturnMatrix LinkStewart::Moment()
00680 {
00681 Matrix M(3,1);
00682 M = DotProduct(N, UnitV)/DotProduct(Vc, UnitV);
00683 M.Release();
00684 return M;
00685 }
00686
00687 ReturnMatrix LinkStewart::NormalForce()
00702 {
00703 Matrix Fn;
00704 Fn = (CrossProduct(N, UnitV) - CrossProduct(Moment(), UnitV))/L;
00705 Fn.Release();
00706 return Fn;
00707 }
00708
00727 ReturnMatrix LinkStewart::AxialForce(const Matrix J1, const ColumnVector C,
00728 const int Index)
00729 {
00730 Matrix Fa;
00731 ColumnVector tmp;
00732 tmp = (J1.t()*C);
00733 Fa = tmp(Index)*UnitV;
00734
00735 Fa.Release();
00736 return Fa;
00737 }
00738
00759 Real LinkStewart::ActuationForce(const Matrix J1, const ColumnVector C,
00760 const int Index, const Real Gravity)
00761 {
00762 Real f, fa;
00763
00764 ColumnVector Fa(3);
00765 gravity = 0;
00766 gravity(2) = -Gravity;
00767
00768 Fa = AxialForce(J1, C, Index);
00769
00770 if (Fa(1) != 0)
00771 fa = Fa(1)/UnitV(1);
00772 else if (Fa(2) != 0)
00773 fa = Fa(2)/UnitV(2);
00774 else if (Fa(3) != 0)
00775 fa = Fa(3)/UnitV(3);
00776 else
00777 fa = 0;
00778
00779 f = m1*DotProduct(ACM1,UnitV) - fa - m1*DotProduct(gravity, UnitV);
00780
00781 return f;
00782 }
00802 ReturnMatrix LinkStewart::Find_ACM1(const Real dl, const Real ddl)
00803 {
00804 ColumnVector Accel1;
00805
00806 Accel1 = (L-Lenght1)*CrossProduct(LOmega, CrossProduct(LOmega, UnitV)) +
00807 (L-Lenght1)*CrossProduct(LAlpha,UnitV) +
00808 CrossProduct(2*LOmega,dl*UnitV)+ddl*UnitV;
00809
00810 Accel1.Release();
00811 return Accel1;
00812 }
00813
00814
00819 Stewart::Stewart()
00820 {
00821 q = ColumnVector(6);
00822 q = 0.0;
00823 dq = ColumnVector(6);
00824 dq = 0.0;
00825 ddq = ColumnVector(6);
00826 ddq = 0.0;
00827 pR = ColumnVector(3);
00828 pR = 0.0;
00829 gravity = pR;
00830 pIp = Matrix(3,3);
00831 pIp = 0.0;
00832 mp = 0.0;
00833 p = 0.0;
00834 n = 0.0;
00835 Js = 0.0;
00836 Jm = 0.0;
00837 bs = 0.0;
00838 bm = 0.0;
00839 p = 0.0;
00840 n = 0.0;
00841 Kb = 0.0;
00842 L = 0.0;
00843 R = 0.0;
00844 Kt = 0.0;
00845 }
00846
00853 Stewart::Stewart(const Matrix InitPlatt, bool Joint)
00854 {
00855 ColumnVector InitLink (14);
00856 Matrix Inertia (3,3);
00857
00858 pR = InitPlatt.SubMatrix(7,7,1,3).t();
00859
00860 Inertia = 0.0;
00861 Inertia(1,1) = InitPlatt(7,4);
00862 Inertia(2,2) = InitPlatt(7,5);
00863 Inertia(3,3) = InitPlatt(7,6);
00864 pIp = Inertia;
00865 mp = InitPlatt(7,7);
00866 Js = InitPlatt(7,8);
00867 Jm = InitPlatt(7,9);
00868 bs = InitPlatt(7,10);
00869 bm = InitPlatt(7,11);
00870 p = InitPlatt(7,12);
00871 n = InitPlatt(7,13);
00872 Kb = InitPlatt(7,14);
00873 L = InitPlatt(7,15);
00874 R = InitPlatt(7,16);
00875 Kt = InitPlatt(7,17);
00876
00877 UJointAtBase = Joint;
00878
00879 q = ColumnVector(6); q <<0.0 <<0.0 <<1.0 <<0.0 <<M_PI/2 <<0.0;
00880 dq = ColumnVector(6); dq =0.0;
00881 ddq = ColumnVector(6); ddq =0.0;
00882
00883 wRp = Find_wRp();
00884
00885 gravity = ColumnVector(3);
00886 gravity = 0;
00887
00888 for (int i =0; i<6; i++)
00889 {
00890 InitLink = InitPlatt.Row(i+1).t();
00891 Links[i] = LinkStewart (InitLink,wRp,q);
00892 }
00893 }
00894
00901 Stewart::Stewart(const string & FileName, const string & PlatFormName)
00902 {
00903 pR = ColumnVector(3);
00904 pR = 0.0;
00905 gravity = pR;
00906 pIp = Matrix(3,3);
00907 pIp = 0.0;
00908 mp = 0.0;
00909
00910 ColumnVector InitLink(14);
00911
00912 Config platData;
00913 ifstream inconffile(FileName.c_str(), std::ios::in);
00914 if (platData.read_conf(inconffile))
00915 {
00916 cerr << "Stewart::Stewart: can not read input config file" << endl;
00917 }
00918
00919 platData.select(PlatFormName, "mp", mp);
00920 platData.select(PlatFormName, "Joint", UJointAtBase);
00921 platData.select(PlatFormName, "pRx", pR(1));
00922 platData.select(PlatFormName, "pRy", pR(2));
00923 platData.select(PlatFormName, "pRz", pR(3));
00924 platData.select(PlatFormName, "Ixx", pIp(1,1));
00925 platData.select(PlatFormName, "Iyy", pIp(2,2));
00926 platData.select(PlatFormName, "Izz", pIp(3,3));
00927 platData.select(PlatFormName, "Js", Js);
00928 platData.select(PlatFormName, "Jm", Jm);
00929 platData.select(PlatFormName, "bs", bs);
00930 platData.select(PlatFormName, "bm", bm);
00931 platData.select(PlatFormName, "p", p);
00932 platData.select(PlatFormName, "n", n);
00933 platData.select(PlatFormName, "Kb", Kb);
00934 platData.select(PlatFormName, "L", L);
00935 platData.select(PlatFormName, "R", R);
00936 platData.select(PlatFormName, "Kt", Kt);
00937
00938 q = ColumnVector(6); q <<0.0 <<0.0 <<1.0 <<0.0 <<M_PI/2 <<0.0;
00939 dq = ColumnVector(6); dq =0.0;
00940 ddq = ColumnVector(6); ddq =0.0;
00941
00942 wRp = Find_wRp();
00943
00944 for(int j = 1; j <= 6; j++)
00945 {
00946 string platformName_link;
00947 ostringstream ostr;
00948 ostr << PlatFormName << "_LINK" << j;
00949 platformName_link = ostr.str();
00950
00951 platData.select(platformName_link, "bx", InitLink(1));
00952 platData.select(platformName_link, "by", InitLink(2));
00953 platData.select(platformName_link, "bz", InitLink(3));
00954 platData.select(platformName_link, "ax", InitLink(4));
00955 platData.select(platformName_link, "ay", InitLink(5));
00956 platData.select(platformName_link, "az", InitLink(6));
00957 platData.select(platformName_link, "Iaa1", InitLink(7));
00958 platData.select(platformName_link, "Inn1", InitLink(8));
00959 platData.select(platformName_link, "Iaa2", InitLink(9));
00960 platData.select(platformName_link, "Inn2", InitLink(10));
00961 platData.select(platformName_link, "m1", InitLink(11));
00962 platData.select(platformName_link, "m2", InitLink(12));
00963 platData.select(platformName_link, "L1", InitLink(13));
00964 platData.select(platformName_link, "L2", InitLink(14));
00965
00966 Links[j-1] = LinkStewart(InitLink,wRp,q);
00967 }
00968 }
00969
00974 Stewart::Stewart(const Stewart & x)
00975 {
00976 for(int i=0;i<6;i++)
00977 Links[i] = x.Links[i];
00978 UJointAtBase = x.UJointAtBase;
00979 q = x.q;
00980 dq = x.dq;
00981 ddq = x.ddq;
00982 pR = x.pR;
00983 pIp = x.pIp;
00984 mp = x.mp;
00985 p = x.p;
00986 n = x.n;
00987 Js = x.Js;
00988 Jm = x.Jm;
00989 bs = x.bs;
00990 bm = x.bm;
00991 Kb = x.Kb;
00992 L = x.L;
00993 R = x.R;
00994 Kt = x.Kt;
00995 gravity = x.gravity;
00996 }
00997
00998
00999 Stewart::~Stewart()
01001 {
01002 }
01003
01004 const Stewart & Stewart::operator = (const Stewart& x)
01005 {
01007 for (int i=0; i<6; i++)
01008 Links[i] = x.Links[i];
01009 UJointAtBase = x.UJointAtBase;
01010 q = x.q;
01011 dq = x.dq;
01012 ddq = x.ddq;
01013 pR = x.pR;
01014 pIp = x.pIp;
01015 mp = x.mp;
01016 p = x.p;
01017 n = x.n;
01018 Js = x.Js;
01019 Jm = x.Jm;
01020 bs = x.bs;
01021 bm = x.bm;
01022 Kb = x.Kb;
01023 L = x.L;
01024 R = x.R;
01025 Kt = x.Kt;
01026 gravity = x.gravity;
01027 return *this;
01028 }
01029
01030 void Stewart::set_Joint(const bool Joint)
01031
01032 {
01033 UJointAtBase = Joint;
01034 }
01035
01036 void Stewart::set_q(const ColumnVector _q)
01038 {
01039 if(_q.Nrows()== 6)
01040 {
01041 q = _q;
01042 Transform();
01043 }
01044 else
01045 cerr<< "Stewart::set_q: wrong size in input vector."<< endl;
01046 }
01047
01048 void Stewart::set_dq(const ColumnVector dq_)
01050 {
01051 if(dq_.Nrows()== 6)
01052 {
01053 dq = dq_;
01054
01055 Omega = Find_Omega();
01056 dl = Find_dl();
01057 ddl = Find_ddl();
01058
01059 for(int i =0; i<6; i++)
01060 Links[i].d_LTransform(dq,Omega, dl(i+1),ddl(i+1));
01061 }
01062 else
01063 cerr<< "Stewart::set_dq: wrong size in input vector."<< endl;
01064 }
01065
01066 void Stewart::set_ddq(const ColumnVector _ddq)
01068 {
01069 if(_ddq.Nrows()== 6)
01070 {
01071 ddq = _ddq;
01072
01073 Omega = Find_Omega();
01074 Alpha = Find_Alpha();
01075 ddl = Find_ddl();
01076 for(int i =0; i<6; i++)
01077 Links[i].dd_LTransform(ddq,Omega,Alpha, dl(i+1),ddl(i+1));
01078 }
01079 else
01080 cerr<< "Stewart::set_ddq: wrong size in input vector."<< endl;
01081 }
01082
01083 void Stewart::set_pR(const ColumnVector _pR)
01085 {
01086 if(_pR.Nrows()== 3)
01087 pR = _pR;
01088 else
01089 cerr<< "Stewart::set_pR: wrong size in input vector."<< endl;
01090 }
01091
01092 void Stewart::set_pIp(const Matrix _pIp)
01094 {
01095 if((_pIp.Nrows()== 3)&&(_pIp.Ncols() == 3))
01096 pIp = _pIp;
01097 else
01098 cerr<< "Stewart::set_pIp: wrong size in input vector."<< endl;
01099 }
01100
01101 void Stewart::set_mp (const Real _mp)
01103 {
01104 mp = _mp;
01105 }
01106
01107 bool Stewart::get_Joint () const
01109 {
01110 return UJointAtBase;
01111 }
01112
01113 ReturnMatrix Stewart::get_q () const
01115 {
01116 return q;
01117 }
01118
01119 ReturnMatrix Stewart::get_dq () const
01121 {
01122 return dq;
01123 }
01124
01125 ReturnMatrix Stewart::get_ddq () const
01127 {
01128 return ddq;
01129 }
01130
01131 ReturnMatrix Stewart::get_pR () const
01133 {
01134 return pR;
01135 }
01136
01137 ReturnMatrix Stewart::get_pIp () const
01139 {
01140 return pIp;
01141 }
01142
01143 Real Stewart::get_mp() const
01145 {
01146 return mp;
01147 }
01148
01160 void Stewart::Transform()
01161 {
01162 wRp = Find_wRp();
01163
01164 for(int i=0; i<6; i++)
01165 Links[i].LTransform(wRp, q);
01166
01167 IJ1 = Find_InvJacob1();
01168 IJ2 = Find_InvJacob2();
01169 Jacobian = jacobian();
01170 }
01171
01186 ReturnMatrix Stewart::Find_wRp ()
01187 {
01188 Matrix _wRp(3,3);
01189
01190 _wRp(1,1) = cos(q(6))*cos(q(4)) - cos(q(5))*sin(q(4))*sin(q(6));
01191 _wRp(1,2) = -sin(q(6))*cos(q(4)) - cos(q(5))*sin(q(4))*cos(q(6));
01192 _wRp(1,3) = sin(q(5))*sin(q(4));
01193 _wRp(2,1) = sin(q(6))*cos(q(4)) + cos(q(5))*sin(q(4))*cos(q(6));
01194 _wRp(2,2) = -sin(q(6))*sin(q(4)) + cos(q(6))*cos(q(4))*cos(q(5));
01195 _wRp(2,3) = -sin(q(5))*cos(q(4));
01196 _wRp(3,1) = sin(q(6))*sin(q(5));
01197 _wRp(3,2) = sin(q(5))*cos(q(6));
01198 _wRp(3,3) = cos(q(5));
01199
01200 _wRp.Release();
01201 return _wRp;
01202 }
01203
01223 ReturnMatrix Stewart::Find_Omega()
01224 {
01225 ColumnVector w(3);
01226
01227 w(1) = cos(q(4))*dq(5) + sin(q(4))*cos(q(5))*dq(6);
01228 w(2) = sin(q(4))*dq(5) - cos(q(4))*sin(q(5))*dq(6);
01229 w(3) = dq(4) + cos(q(5))*dq(6);
01230
01231 w.Release();
01232 return w;
01233 }
01234
01261 ReturnMatrix Stewart::Find_Alpha()
01262 {
01263 Matrix A, Temp(3,3),Temp2(3,3);
01264
01265 Temp = 0.0; Temp(3,1) = 1; Temp(1,2) = cos(q(4)); Temp(2,2) = sin(q(4));
01266 Temp(1,3) = sin(q(4))*cos(q(5)); Temp(2,3)=-cos(q(4))*sin(q(5)); Temp(3,3) = cos(q(5));
01267
01268 Temp2 = 0.0; Temp2(1,2) = -dq(4)*sin(q(4)); Temp2(2,2) = dq(4)*cos(q(4));
01269 Temp2(1,3) = dq(4)*cos(q(4))*sin(q(5))+dq(5)*sin(q(4))*cos(q(5));
01270 Temp2(2,3) = dq(4)*sin(q(4))*sin(q(5))-dq(5)*cos(q(4))*cos(q(5));
01271 Temp2(3,3) = -dq(5)*sin(q(5));
01272
01273 A = Temp*ddq.Rows(4,6) + Temp2*dq.Rows(4,6);
01274
01275 A.Release();
01276 return A;
01277 }
01278
01290 ReturnMatrix Stewart::jacobian()
01291 {
01292 Matrix _Jacobi;
01293
01294 _Jacobi = (IJ1*IJ2).i();
01295
01296 _Jacobi.Release();
01297 return _Jacobi;
01298 }
01299
01316 ReturnMatrix Stewart::Find_InvJacob1()
01317 {
01318 Matrix tmp_Jacobi1 (6,6);
01319
01320 for(int i = 0; i<6; i++)
01321 tmp_Jacobi1.Row(i+1) = Links[i].UnitV.t() | CrossProduct(wRp*Links[i].ap,Links[i].UnitV).t();
01322
01323 tmp_Jacobi1.Release();
01324 return tmp_Jacobi1;
01325 }
01326
01344 ReturnMatrix Stewart::Find_InvJacob2()
01345 {
01346 Matrix tmp_Jacobi2;
01347
01348 tmp_Jacobi2 = IdentityMatrix(6);
01349 tmp_Jacobi2(4,4) = 0;
01350 tmp_Jacobi2(6,4) = 1;
01351 tmp_Jacobi2(4,5) = cos(q(4));
01352 tmp_Jacobi2(5,5) = sin(q(4));
01353 tmp_Jacobi2(4,6) = sin(q(4))*sin(q(5));
01354 tmp_Jacobi2(5,6) = -cos(q(4))*sin(q(5));
01355 tmp_Jacobi2(6,6) = cos(q(5));
01356
01357 tmp_Jacobi2.Release();
01358 return tmp_Jacobi2;
01359 }
01389 ReturnMatrix Stewart::jacobian_dot()
01390 {
01391 Matrix tmp_dJ2(6,6), tmp_dn(6,3), tmp_sol(6,3), tmp_dJ1(6,6), tmp_dJ(6,6);
01392 ColumnVector VctNorm, a;
01393
01394 tmp_dJ2 = 0.0;
01395 tmp_dJ2(4,5) = -dq(4)*sin(q(4));
01396 tmp_dJ2(5,5) = dq(4)*cos(q(4));
01397 tmp_dJ2(4,6) = dq(4)*cos(q(4))*sin(q(5))+dq(5)*sin(q(4))*cos(q(5));
01398 tmp_dJ2(5,6) = dq(4)*sin(q(4))*sin(q(5))-dq(5)*cos(q(4))*cos(q(5));
01399 tmp_dJ2(6,6) = -dq(5)*sin(q(5));
01400
01401 for (int i = 0; i<6; i++)
01402 {
01403 tmp_dn.Row(i+1) = ((Links[i].UnitV*Links[i].L -
01404 dl(i+1)*Links[i].UnitV)/Links[i].L).t();
01405 tmp_sol.Row(i+1) = (CrossProduct(CrossProduct(Omega,Links[i].aPos.t()),
01406 Links[i].UnitV.t()) +
01407 (CrossProduct(Links[i].aPos,tmp_dn.Row(i+1)))).t();
01408 }
01409
01410 for (int j = 1; j < 7; j++)
01411 for(int k = 1; k < 4; k++)
01412 {
01413 tmp_dJ1(j,k) = tmp_dn(j,k);
01414 tmp_dJ1(j,k+3) = tmp_sol(j,k);
01415 }
01416
01417 tmp_dJ = tmp_dJ1*IJ2 + IJ1*tmp_dJ2;
01418
01419 tmp_dJ.Release();
01420 return tmp_dJ;
01421 }
01429 ReturnMatrix Stewart::InvPosKine()
01430 {
01431 ColumnVector Vct_L(6);
01432
01433 for (int i = 1; i < 7; i++)
01434 Vct_L(i) = Links[i-1].L;
01435
01436 Vct_L.Release(); return Vct_L;
01437 }
01438
01451 ReturnMatrix Stewart::Find_dl()
01452 {
01453 ColumnVector tmp_dl;
01454 tmp_dl = Jacobian.i()*dq;
01455
01456 tmp_dl.Release();
01457 return tmp_dl;
01458 }
01459
01472 ReturnMatrix Stewart::Find_ddl()
01473 {
01474 ColumnVector tmp_ddl;
01475 tmp_ddl = Jacobian.i() * ddq + jacobian_dot() * dq;
01476 tmp_ddl.Release();
01477 return tmp_ddl;
01478 }
01479
01512 ReturnMatrix Stewart::Find_C(const Real Gravity)
01513 {
01514 Matrix C(6,1), I(3,3);
01515 ColumnVector Sum(3), Sum2(3), Sum3(3), ddxg(3), LNormalForce(3);
01516
01517 gravity = 0;
01518 gravity(2) = -Gravity;
01519
01520 ddxg = ddq.Rows(1,3) + CrossProduct(Alpha,wRp*pR) + CrossProduct(Omega,CrossProduct(Omega,wRp*pR));
01521 I = wRp*pIp*(wRp.t());
01522
01523 Sum = 0.0;
01524 Sum2 = 0.0;
01525 Sum3 = 0.0;
01526 for (int i=0; i<6; i++)
01527 {
01528 LNormalForce = Links[i].NormalForce();
01529 Sum = Sum + LNormalForce;
01530 Sum2 = Sum2 + CrossProduct(Links[i].aPos,LNormalForce);
01531 if(!UJointAtBase)
01532 {
01533 Sum3 = Sum3 +Links[i].Moment();
01534 }
01535 }
01536
01537 C.Rows(1,3) = mp*gravity - mp*ddxg - Sum;
01538 C.Rows(4,6) = mp*CrossProduct(wRp*pR, gravity) - mp*CrossProduct(wRp*pR,ddxg)-I*Alpha
01539 + I*CrossProduct(Omega,Omega)- Sum2 - Sum3;
01540
01541 C.Release();
01542 return C;
01543 }
01544
01567 ReturnMatrix Stewart::ForwardKine(const ColumnVector guess_q, const ColumnVector l_given,
01568 const Real tolerance)
01569 {
01570 ColumnVector next_q, tmp_long(6);
01571 Real Diff = 1;
01572
01573 q = guess_q;
01574 while (Diff>tolerance)
01575 {
01576 for(int i=0; i<6; i++)
01577 tmp_long(i+1) = Links[i].L - l_given(i+1);
01578
01579 next_q = q - Jacobian*(tmp_long);
01580 Diff = (next_q - q).MaximumAbsoluteValue();
01581
01582 set_q(next_q);
01583 }
01584 next_q.Release();
01585 return next_q;
01586 }
01587
01595 ReturnMatrix Stewart::JointSpaceForceVct(const Real Gravity)
01596 {
01597 Matrix F(6,1), C, IJ1(6,6);
01598
01599 IJ1 = Find_InvJacob1();
01600 C = Find_C(Gravity);
01601
01602 for (int i =0; i<6; i++)
01603 {
01604 F(i+1,1) = Links[i].ActuationForce(IJ1, C, i+1, Gravity);
01605 }
01606
01607 F.Release();
01608 return F;
01609 }
01610
01625 ReturnMatrix Stewart::Torque(const Real Gravity)
01626 {
01627 Matrix T;
01628
01629 for(int i=0;i<6;i++)
01630 Links[i].tau_LTransform(dl(i+1), ddl(i+1), Gravity);
01631
01632 T = Jacobian.i().t()*JointSpaceForceVct(Gravity);
01633
01634 T.Release();
01635 return T;
01636 }
01646 ReturnMatrix Stewart::Find_h(const Real Gravity)
01647 {
01648 ColumnVector _ddq(6);
01649 _ddq = 0.0;
01650 set_ddq(_ddq);
01651 return Torque(Gravity);
01652 }
01653
01663 ReturnMatrix Stewart::Find_M()
01664 {
01665 Matrix M(6,6);
01666 ColumnVector _ddq(6), _dq(6), tmpdq(6);
01667
01668 tmpdq = dq;
01669 _dq = 0.0;
01670
01671 set_dq(_dq);
01672
01673 for (int i = 1; i < 7; i++)
01674 {
01675 _ddq = 0.0;
01676 _ddq(i) = 1.0;
01677 set_ddq(_ddq);
01678 M.Column(i) = Torque (0.0);
01679 }
01680 set_dq(tmpdq);
01681
01682 M.Release();
01683 return M;
01684 }
01685
01701 ReturnMatrix Stewart::ForwardDyn(const ColumnVector T, const Real Gravity)
01702 {
01703 ColumnVector _ddq;
01704
01705 _ddq = Find_M().i()*(T - Find_h(Gravity));
01706
01707 _ddq.Release();
01708 return _ddq;
01709 }
01710
01736 void Stewart::Find_Mc_Nc_Gc(Matrix & Mc, Matrix & Nc, Matrix & Gc)
01737 {
01738 Matrix G, Ka(6,6), Ma(6,6), Va(6,6), dJacobian(6,6);
01739 dJacobian = jacobian_dot();
01740
01741 Ka = p/(2*M_PI*n)*IdentityMatrix(6);
01742 Ma = (2*M_PI/(n*p))*(Js + n*n*Jm)*IdentityMatrix(6);
01743 Va = (2*M_PI/(n*p))*(bs + n*n*bm)*IdentityMatrix(6);
01744
01745 Mc = Ka*Jacobian.t() * Find_M() + Ma*Jacobian.i();
01746
01747 Nc = Ka*Jacobian.t()*Find_h(0.0) + (Va*Jacobian.i()+Ma*dJacobian)*dq;
01748
01749 ColumnVector _dq(6), _tmpdq(6);
01750 _dq = 0.0;
01751 _tmpdq = dq;
01752 set_dq(_dq);
01753
01754 Gc = Ka *Jacobian.t()*Find_h();
01755 set_dq(_tmpdq);
01756 }
01757
01792 ReturnMatrix Stewart::ForwardDyn_AD(const ColumnVector Command, const Real t)
01793 {
01794 Matrix _ddq;
01795 Matrix Nc,Gc,Mc, tmp1,tmp2;
01796
01797 Find_Mc_Nc_Gc(Mc,Nc,Gc);
01798
01799 tmp1 = (Command - (Jacobian.i()*dq*(2*M_PI/p)*Kb));
01800 tmp2 = (IdentityMatrix(6)*Kt/L*exp(-R*t/L))*tmp1;
01801
01802 _ddq = Mc.i()*(tmp2 - Nc - Gc);
01803
01804 _ddq.Release();
01805 return _ddq;
01806 }
01807
01808 #ifdef use_namespace
01809 }
01810 #endif
01811