stewart.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2004  Samuel Bélanger
00003 
00004 This library is free software; you can redistribute it and/or modify
00005 it under the terms of the GNU Lesser General Public License as
00006 published by the Free Software Foundation; either version 2.1 of the
00007 License, or (at your option) any later version.
00008 
00009 This library is distributed in the hope that it will be useful,
00010 but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 GNU Lesser General Public License for more details.
00013 
00014 You should have received a copy of the GNU Lesser General Public
00015 License along with this library; if not, write to the Free Software
00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017 
00018 
00019 Report problems and direct all questions to:
00020 
00021 email: samuel.belanger@polymtl.ca or richard.gourdeau@polymtl.ca
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 


kni
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:07