ROBOOP, A Robotics Object Oriented Package in C++
stewart.cpp
Go to the documentation of this file.
1 /*
2 Copyright (C) 2004 Samuel Bélanger
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: samuel.belanger@polymtl.ca or richard.gourdeau@polymtl.ca
22 */
23 
24 
30 #include "config.h"
31 #include "stewart.h"
32 
33 #ifdef use_namespace
34 namespace ROBOOP {
35 #endif
36 
37 
42 LinkStewart::LinkStewart ()
43 {
44  b = ColumnVector(3);
45  b = 0.0;
46  ap = ColumnVector(3);
47  ap = 0.0;
48 
49  I1aa = 0.0;
50  I1nn = 0.0;
51  I2aa = 0.0;
52  I2nn = 0.0;
53  m1 = 0.0;
54  m2 = 0.0;
55  Lenght1 = 0.0;
56  Lenght2 = 0.0;
57 
58  ColumnVector ZeroValue(3);
59  ZeroValue<<0.0<<0.0<<0.0;
60 
61  UnitV = ZeroValue;
62  aPos = ZeroValue;
63  Vu = ZeroValue;
64  Vc = ZeroValue;
65  Vv = ZeroValue;
66  da = ZeroValue;
67  dda = ZeroValue;
68  LOmega = ZeroValue;
69  LAlpha = ZeroValue;
70  ACM1 = ZeroValue;
71  N = ZeroValue;
72  gravity = ZeroValue;
73  L = 0.0;
74 }
75 
84 LinkStewart::LinkStewart (const ColumnVector & InitLink, const Matrix wRp, const ColumnVector q)
85 {
86  b = InitLink.Rows(1,3);
87  ap = InitLink.Rows(4,6);
88  I1aa = InitLink(7);
89  I1nn = InitLink(8);
90  I2aa = InitLink(9);
91  I2nn = InitLink(10);
92  m1 = InitLink(11);
93  m2 = InitLink(12);
94  Lenght1 = InitLink(13);
95  Lenght2 = InitLink(14);
96 
97  ColumnVector ZeroValue(3);
98  ZeroValue<<0.0<<0.0<<0.0;
99 
100  UnitV = ZeroValue;
101  aPos = ZeroValue;
102  Vc = ZeroValue;
103  Vv = ZeroValue;
104  da = ZeroValue;
105  dda = ZeroValue;
106  LOmega = ZeroValue;
107  LAlpha = ZeroValue;
108  ACM1 = ZeroValue;
109  N = ZeroValue;
110  gravity = ZeroValue;
111  L = 0.0;
112 
113  aPos = Find_a(wRp,q);
114  L = Find_Lenght();
115  UnitV = Find_UnitV();
116  Vu = Find_VctU();
117 }
118 
123 LinkStewart::LinkStewart(const LinkStewart & x)
124 {
125  b = x.b;
126  ap = x.ap;
127  I1aa = x.I1aa;
128  I1nn = x.I1nn;
129  I2aa = x.I2aa;
130  I2nn = x.I2nn;
131  m1 = x.m1;
132  m2 = x.m2;
133  Lenght1 = x.Lenght1;
134  Lenght2 = x.Lenght2;
135 
136  UnitV = x.UnitV;
137  aPos = x.aPos;
138  Vu = x.Vu;
139  Vc = x.Vc;
140  Vv = x.Vv;
141  da = x.da;
142  dda = x.dda;
143  LOmega = x.LOmega;
144  LAlpha = x.LAlpha;
145  ACM1 = x.ACM1;
146  N = x.N;
147  gravity = x.gravity;
148  L = x.L;
149 }
150 
151 LinkStewart::~LinkStewart()
153 {
154 }
155 
156 const LinkStewart & LinkStewart::operator = (const LinkStewart& x)
157 {
159  b = x.b;
160  ap = x.ap;
161  I1aa = x.I1aa;
162  I1nn = x.I1nn;
163  I2aa = x.I2aa;
164  I2nn = x.I2nn;
165  m1 = x.m1;
166  m2 = x.m2;
167  Lenght1 = x.Lenght1;
168  Lenght2 = x.Lenght2;
169 
170  UnitV = x.UnitV;
171  aPos = x.aPos;
172  Vu = x.Vu;
173  Vc = x.Vc;
174  Vv = x.Vv;
175  da = x.da;
176  dda = x.dda;
177  LOmega = x.LOmega;
178  LAlpha = x.LAlpha;
179  ACM1 = x.ACM1;
180  N = x.N;
181  gravity = x.gravity;
182  L = x.L;
183  return *this;
184 }
185 
186 void LinkStewart::set_b(const ColumnVector Newb)
188 {
189  if(Newb.Nrows() == 3)
190  b = Newb;
191  else
192  cerr<< "LinkStewart::set_b: wrong size in input vector."<< endl;
193 }
194 
195 void LinkStewart::set_ap(const ColumnVector NewAp)
197 {
198  if(NewAp.Nrows()== 3)
199  ap = NewAp;
200  else
201  cerr<< "LinkStewart::set_Ap: wrong size in input vector."<< endl;
202 }
203 
204 void LinkStewart::set_I1aa(const Real NewI1aa)
205 
206 {
207  I1aa = NewI1aa;
208 }
209 
210 void LinkStewart::set_I1nn(const Real NewI1nn)
211 
212 {
213  I1nn = NewI1nn;
214 }
215 
216 void LinkStewart::set_I2aa(const Real NewI2aa)
217 
218 {
219  I2aa = NewI2aa;
220 }
221 
222 void LinkStewart::set_I2nn(const Real NewI2nn)
223 
224 {
225  I2nn = NewI2nn;
226 }
227 
228 void LinkStewart::set_m1(const Real Newm1)
229 
230 {
231  m1 = Newm1;
232 }
233 
234 void LinkStewart::set_m2(const Real Newm2)
235 
236 {
237  m2 = Newm2;
238 }
239 
240 void LinkStewart::set_Lenght1(const Real NewLenght1)
242 {
243  Lenght1 = NewLenght1;
244 }
245 
246 void LinkStewart::set_Lenght2(const Real NewLenght2)
248 {
249  Lenght2 = NewLenght2;
250 }
251 
252 ReturnMatrix LinkStewart::get_ap() const
254 {
255  return ap;
256 }
257 
258 ReturnMatrix LinkStewart::get_b() const
260 {
261  return b;
262 }
263 
264 Real LinkStewart::get_I1aa() const
266 {
267  return I1aa;
268 }
269 
270 Real LinkStewart::get_I1nn() const
272 {
273  return I1nn;
274 }
275 
276 Real LinkStewart::get_I2aa() const
278 {
279  return I2aa;
280 }
281 
282 Real LinkStewart::get_I2nn() const
284 {
285  return I2nn;
286 }
287 
288 Real LinkStewart::get_m1() const
290 {
291  return m1;
292 }
293 
294 Real LinkStewart::get_m2() const
296 {
297  return m2;
298 }
299 
300 Real LinkStewart::get_Lenght1() const
302 {
303  return Lenght1;
304 }
305 
306 Real LinkStewart::get_Lenght2() const
308 {
309  return Lenght2;
310 }
311 
312 void LinkStewart::LTransform(const Matrix wRp, const ColumnVector q)
318 {
319  aPos = Find_a(wRp,q);
320  L = Find_Lenght();
321  UnitV = Find_UnitV();
322  Vv = Find_VctV();
323  Vc = Find_VctC();
324 }
325 
326 void LinkStewart::d_LTransform(const ColumnVector dq, const ColumnVector Omega,
327  const Real dl, const Real ddl)
335 {
336  Matrix AngularKin;
337 
338  da = Find_da(dq,Omega);
339  AngularKin = Find_AngularKin(dl, ddl);
340  LOmega = AngularKin.Column(1);
341  LAlpha = AngularKin.Column(2);
342 }
343 
344 void LinkStewart::dd_LTransform(const ColumnVector ddq, const ColumnVector Omega,
345  const ColumnVector Alpha, const Real dl, const Real ddl)
354 {
355  Matrix AngularKin;
356 
357  dda = Find_dda(ddq,Omega,Alpha);
358  AngularKin = Find_AngularKin(dl, ddl);
359  LOmega = AngularKin.Column(1);
360  LAlpha = AngularKin.Column(2);
361 }
362 
363 void LinkStewart::tau_LTransform(const Real dl, const Real ddl,const Real Gravity)
370 {
371  ACM1 = Find_ACM1(dl,ddl);
372  N = Find_N(Gravity);
373 }
374 
375 ReturnMatrix LinkStewart::Find_a(const Matrix wRp, const ColumnVector q)
393 {
394  ColumnVector a;
395  a = q.Rows(1,3) + wRp*ap;
396  a.Release();
397  return a;
398 }
399 
413 ReturnMatrix LinkStewart::Find_UnitV()
414 {
415  Matrix Tmp (1,3);
416  Tmp = (aPos - b)/L;
417  Tmp.Release();
418  return Tmp;
419 }
420 
435 ReturnMatrix LinkStewart::Find_da(const ColumnVector dq, const ColumnVector Omega)
436 {
437  ColumnVector da;
438 
439  da = dq.Rows(1,3) + CrossProduct(Omega,aPos);
440 
441  da.Release();
442  return da;
443 }
444 
461 ReturnMatrix LinkStewart::Find_dda(const ColumnVector ddq,const ColumnVector Omega,const ColumnVector Alpha)
462 {
463  ColumnVector dda;
464  dda = ddq.Rows(1,3) + CrossProduct(Alpha,aPos) +
465  CrossProduct(Omega,CrossProduct(Omega,aPos));
466 
467  dda.Release();
468  return dda;
469 }
470 
483 Real LinkStewart::Find_Lenght()
484 {
485  return sqrt(DotProduct((aPos - b),(aPos - b)));
486 }
487 
500 ReturnMatrix LinkStewart::Find_VctU()
501 {
502  ColumnVector u(3);
503 
504  u(1) = -UnitV(1)/sqrt(UnitV(1)*UnitV(1) + UnitV(3)*UnitV(3));
505  u(2) = 0.0;
506  u(3) = -UnitV(3)/sqrt(UnitV(1)*UnitV(1) + UnitV(3)*UnitV(3));
507 
508  u.Release();
509  return u;
510 }
511 
524 ReturnMatrix LinkStewart::Find_VctV()
525 {
526  ColumnVector v;
527  v = CrossProduct(Vu,UnitV)/(CrossProduct(Vu,UnitV).NormFrobenius());
528 
529  v.Release();
530  return v;
531 }
532 
545 ReturnMatrix LinkStewart::Find_VctC()
546 {
547  return CrossProduct(Vu, Vv);
548 }
549 
582 ReturnMatrix LinkStewart::Find_AngularKin(const Real dl, const Real ddl)
583 {
584  Matrix Temp(3,2);
585  ColumnVector tmp_dda(3), omega(3), alpha(3);
586  Real wu, wv, au, av;
587 
588  wu = DotProduct(-(da-dl*UnitV),Vv/(DotProduct(L*UnitV,Vc)));
589  wv = DotProduct((da-dl*UnitV),Vu/(DotProduct(L*UnitV,Vc)));
590 
591  omega = wu*Vu + wv*Vv;
592 
593  tmp_dda = dda - wu*wv*L*CrossProduct(Vc,UnitV)-ddl*UnitV-2*dl*CrossProduct(omega,UnitV)
594  -L*CrossProduct(omega,CrossProduct(omega,UnitV));
595 
596  au = DotProduct(-tmp_dda,Vv/(L*DotProduct(UnitV,Vc)));
597  av = DotProduct(tmp_dda,Vu/(L*DotProduct(UnitV,Vc)));
598 
599  alpha = au*Vu + av*Vv + wu*wv*Vc;
600 
601  Temp.Column(1) = omega; Temp.Column(2) = alpha;
602 
603  Temp.Release(); return Temp;
604 }
605 
638 ReturnMatrix LinkStewart::Find_N(const Real Gravity)
639 {
640  ColumnVector Accel2(3);
641  Matrix Fn(3,1), N_N(3,1), I1(3,3), I2(3,3);
642 
643  gravity = 0;
644  gravity(2) = -Gravity;
645 
646  Accel2 = Lenght2*CrossProduct(LOmega,CrossProduct(LOmega, UnitV))
647  + Lenght2*CrossProduct(LAlpha,UnitV);
648 
649  IdentityMatrix Identity(3);
650 
651  I1 = I1aa*UnitV*UnitV.t() + I1nn*(Identity - UnitV*UnitV.t());
652  I2 = I2aa*UnitV*UnitV.t() + I2nn*(Identity - UnitV*UnitV.t());
653 
654  N_N = -m1*(L-Lenght1)*CrossProduct(UnitV, gravity) - m2*Lenght2*CrossProduct(UnitV, gravity) +
655  (I1+I2)*LAlpha - (I1+I2)*CrossProduct(LOmega,LOmega)
656  + m1*(L-Lenght1)*CrossProduct(UnitV,ACM1)
657  +m2*Lenght2*CrossProduct(UnitV,Accel2);
658 
659  N_N.Release();
660  return N_N;
661 }
662 
663 ReturnMatrix LinkStewart::Moment()
677 {
678  Matrix M(3,1);
679  M = DotProduct(N, UnitV)/DotProduct(Vc, UnitV);
680  M.Release();
681  return M;
682 }
683 
684 ReturnMatrix LinkStewart::NormalForce()
699 {
700  Matrix Fn;
701  Fn = (CrossProduct(N, UnitV) - CrossProduct(Moment(), UnitV))/L;
702  Fn.Release();
703  return Fn;
704 }
705 
724 ReturnMatrix LinkStewart::AxialForce(const Matrix J1, const ColumnVector C,
725  const int Index)
726 {
727  Matrix Fa;
728  ColumnVector tmp;
729  tmp = (J1.t()*C);
730  Fa = tmp(Index)*UnitV;
731 
732  Fa.Release();
733  return Fa;
734 }
735 
756 Real LinkStewart::ActuationForce(const Matrix J1, const ColumnVector C,
757  const int Index, const Real Gravity)
758 {
759  Real f, fa;
760 
761  ColumnVector Fa(3);
762  gravity = 0;
763  gravity(2) = -Gravity;
764 
765  Fa = AxialForce(J1, C, Index);
766 
767  if (Fa(1) != 0)
768  fa = Fa(1)/UnitV(1);
769  else if (Fa(2) != 0)
770  fa = Fa(2)/UnitV(2);
771  else if (Fa(3) != 0)
772  fa = Fa(3)/UnitV(3);
773  else
774  fa = 0;
775 
776  f = m1*DotProduct(ACM1,UnitV) - fa - m1*DotProduct(gravity, UnitV);
777 
778  return f;
779 }
799 ReturnMatrix LinkStewart::Find_ACM1(const Real dl, const Real ddl)
800 {
801  ColumnVector Accel1;
802 
803  Accel1 = (L-Lenght1)*CrossProduct(LOmega, CrossProduct(LOmega, UnitV)) +
804  (L-Lenght1)*CrossProduct(LAlpha,UnitV) +
805  CrossProduct(2*LOmega,dl*UnitV)+ddl*UnitV;
806 
807  Accel1.Release();
808  return Accel1;
809 }
810 
811 
816 Stewart::Stewart()
817 {
818  q = ColumnVector(6);
819  q = 0.0;
820  dq = ColumnVector(6);
821  dq = 0.0;
822  ddq = ColumnVector(6);
823  ddq = 0.0;
824  pR = ColumnVector(3);
825  pR = 0.0;
826  gravity = pR;
827  pIp = Matrix(3,3);
828  pIp = 0.0;
829  mp = 0.0;
830  p = 0.0;
831  n = 0.0;
832  Js = 0.0;
833  Jm = 0.0;
834  bs = 0.0;
835  bm = 0.0;
836  p = 0.0;
837  n = 0.0;
838  Kb = 0.0;
839  L = 0.0;
840  R = 0.0;
841  Kt = 0.0;
842 }
843 
850 Stewart::Stewart(const Matrix InitPlatt, bool Joint)
851 {
852  ColumnVector InitLink (14);
853  Matrix Inertia (3,3);
854 
855  pR = InitPlatt.SubMatrix(7,7,1,3).t();
856 
857  Inertia = 0.0;
858  Inertia(1,1) = InitPlatt(7,4);
859  Inertia(2,2) = InitPlatt(7,5);
860  Inertia(3,3) = InitPlatt(7,6);
861  pIp = Inertia;
862  mp = InitPlatt(7,7);
863  Js = InitPlatt(7,8);
864  Jm = InitPlatt(7,9);
865  bs = InitPlatt(7,10);
866  bm = InitPlatt(7,11);
867  p = InitPlatt(7,12);
868  n = InitPlatt(7,13);
869  Kb = InitPlatt(7,14);
870  L = InitPlatt(7,15);
871  R = InitPlatt(7,16);
872  Kt = InitPlatt(7,17);
873 
874  UJointAtBase = Joint;
875 
876  q = ColumnVector(6); q <<0.0 <<0.0 <<1.0 <<0.0 <<M_PI/2 <<0.0;
877  dq = ColumnVector(6); dq =0.0;
878  ddq = ColumnVector(6); ddq =0.0;
879 
880  wRp = Find_wRp();
881 
882  gravity = ColumnVector(3);
883  gravity = 0;
884 
885  for (int i =0; i<6; i++)
886  {
887  InitLink = InitPlatt.Row(i+1).t();
888  Links[i] = LinkStewart (InitLink,wRp,q);
889  }
890 }
891 
898 Stewart::Stewart(const string & FileName, const string & PlatFormName)
899 {
900  pR = ColumnVector(3);
901  pR = 0.0;
902  gravity = pR;
903  pIp = Matrix(3,3);
904  pIp = 0.0;
905  mp = 0.0;
906 
907  ColumnVector InitLink(14);
908 
909  Config platData;
910  ifstream inconffile(FileName.c_str(), std::ios::in);
911  if (platData.read_conf(inconffile))
912  {
913  cerr << "Stewart::Stewart: can not read input config file" << endl;
914  }
915 
916  platData.select(PlatFormName, "mp", mp);
917  platData.select(PlatFormName, "Joint", UJointAtBase);
918  platData.select(PlatFormName, "pRx", pR(1));
919  platData.select(PlatFormName, "pRy", pR(2));
920  platData.select(PlatFormName, "pRz", pR(3));
921  platData.select(PlatFormName, "Ixx", pIp(1,1));
922  platData.select(PlatFormName, "Iyy", pIp(2,2));
923  platData.select(PlatFormName, "Izz", pIp(3,3));
924  platData.select(PlatFormName, "Js", Js);
925  platData.select(PlatFormName, "Jm", Jm);
926  platData.select(PlatFormName, "bs", bs);
927  platData.select(PlatFormName, "bm", bm);
928  platData.select(PlatFormName, "p", p);
929  platData.select(PlatFormName, "n", n);
930  platData.select(PlatFormName, "Kb", Kb);
931  platData.select(PlatFormName, "L", L);
932  platData.select(PlatFormName, "R", R);
933  platData.select(PlatFormName, "Kt", Kt);
934 
935  q = ColumnVector(6); q <<0.0 <<0.0 <<1.0 <<0.0 <<M_PI/2 <<0.0;
936  dq = ColumnVector(6); dq =0.0;
937  ddq = ColumnVector(6); ddq =0.0;
938 
939  wRp = Find_wRp();
940 
941  for(int j = 1; j <= 6; j++)
942  {
943  string platformName_link;
944  ostringstream ostr;
945  ostr << PlatFormName << "_LINK" << j;
946  platformName_link = ostr.str();
947 
948  platData.select(platformName_link, "bx", InitLink(1));
949  platData.select(platformName_link, "by", InitLink(2));
950  platData.select(platformName_link, "bz", InitLink(3));
951  platData.select(platformName_link, "ax", InitLink(4));
952  platData.select(platformName_link, "ay", InitLink(5));
953  platData.select(platformName_link, "az", InitLink(6));
954  platData.select(platformName_link, "Iaa1", InitLink(7));
955  platData.select(platformName_link, "Inn1", InitLink(8));
956  platData.select(platformName_link, "Iaa2", InitLink(9));
957  platData.select(platformName_link, "Inn2", InitLink(10));
958  platData.select(platformName_link, "m1", InitLink(11));
959  platData.select(platformName_link, "m2", InitLink(12));
960  platData.select(platformName_link, "L1", InitLink(13));
961  platData.select(platformName_link, "L2", InitLink(14));
962 
963  Links[j-1] = LinkStewart(InitLink,wRp,q);
964  }
965 }
966 
971 Stewart::Stewart(const Stewart & x)
972 {
973  for(int i=0;i<6;i++)
974  Links[i] = x.Links[i];
975  UJointAtBase = x.UJointAtBase;
976  q = x.q;
977  dq = x.dq;
978  ddq = x.ddq;
979  pR = x.pR;
980  pIp = x.pIp;
981  mp = x.mp;
982  p = x.p;
983  n = x.n;
984  Js = x.Js;
985  Jm = x.Jm;
986  bs = x.bs;
987  bm = x.bm;
988  Kb = x.Kb;
989  L = x.L;
990  R = x.R;
991  Kt = x.Kt;
992  gravity = x.gravity;
993 }
994 
995 
996 Stewart::~Stewart()
998 {
999 }
1000 
1001 const Stewart & Stewart::operator = (const Stewart& x)
1002 {
1004  for (int i=0; i<6; i++)
1005  Links[i] = x.Links[i];
1006  UJointAtBase = x.UJointAtBase;
1007  q = x.q;
1008  dq = x.dq;
1009  ddq = x.ddq;
1010  pR = x.pR;
1011  pIp = x.pIp;
1012  mp = x.mp;
1013  p = x.p;
1014  n = x.n;
1015  Js = x.Js;
1016  Jm = x.Jm;
1017  bs = x.bs;
1018  bm = x.bm;
1019  Kb = x.Kb;
1020  L = x.L;
1021  R = x.R;
1022  Kt = x.Kt;
1023  gravity = x.gravity;
1024  return *this;
1025 }
1026 
1027 void Stewart::set_Joint(const bool Joint)
1028 
1029 {
1030  UJointAtBase = Joint;
1031 }
1032 
1033 void Stewart::set_q(const ColumnVector _q)
1035 {
1036  if(_q.Nrows()== 6)
1037  {
1038  q = _q;
1039  Transform();
1040  }
1041  else
1042  cerr<< "Stewart::set_q: wrong size in input vector."<< endl;
1043 }
1044 
1045 void Stewart::set_dq(const ColumnVector dq_)
1047 {
1048  if(dq_.Nrows()== 6)
1049  {
1050  dq = dq_;
1051 
1052  Omega = Find_Omega();
1053  dl = Find_dl();
1054  ddl = Find_ddl();
1055 
1056  for(int i =0; i<6; i++)
1057  Links[i].d_LTransform(dq,Omega, dl(i+1),ddl(i+1));
1058  }
1059  else
1060  cerr<< "Stewart::set_dq: wrong size in input vector."<< endl;
1061 }
1062 
1063 void Stewart::set_ddq(const ColumnVector _ddq)
1065 {
1066  if(_ddq.Nrows()== 6)
1067  {
1068  ddq = _ddq;
1069 
1070  Omega = Find_Omega();
1071  Alpha = Find_Alpha();
1072  ddl = Find_ddl();
1073  for(int i =0; i<6; i++)
1074  Links[i].dd_LTransform(ddq,Omega,Alpha, dl(i+1),ddl(i+1));
1075  }
1076  else
1077  cerr<< "Stewart::set_ddq: wrong size in input vector."<< endl;
1078 }
1079 
1080 void Stewart::set_pR(const ColumnVector _pR)
1082 {
1083  if(_pR.Nrows()== 3)
1084  pR = _pR;
1085  else
1086  cerr<< "Stewart::set_pR: wrong size in input vector."<< endl;
1087 }
1088 
1089 void Stewart::set_pIp(const Matrix _pIp)
1091 {
1092  if((_pIp.Nrows()== 3)&&(_pIp.Ncols() == 3))
1093  pIp = _pIp;
1094  else
1095  cerr<< "Stewart::set_pIp: wrong size in input vector."<< endl;
1096 }
1097 
1098 void Stewart::set_mp (const Real _mp)
1100 {
1101  mp = _mp;
1102 }
1103 
1104 bool Stewart::get_Joint () const
1106 {
1107  return UJointAtBase;
1108 }
1109 
1110 ReturnMatrix Stewart::get_q () const
1112 {
1113  return q;
1114 }
1115 
1116 ReturnMatrix Stewart::get_dq () const
1118 {
1119  return dq;
1120 }
1121 
1122 ReturnMatrix Stewart::get_ddq () const
1124 {
1125  return ddq;
1126 }
1127 
1128 ReturnMatrix Stewart::get_pR () const
1130 {
1131  return pR;
1132 }
1133 
1134 ReturnMatrix Stewart::get_pIp () const
1136 {
1137  return pIp;
1138 }
1139 
1140 Real Stewart::get_mp() const
1142 {
1143  return mp;
1144 }
1145 
1157 void Stewart::Transform()
1158 {
1159  wRp = Find_wRp();
1160 
1161  for(int i=0; i<6; i++)
1162  Links[i].LTransform(wRp, q);
1163 
1164  IJ1 = Find_InvJacob1();
1165  IJ2 = Find_InvJacob2();
1166  Jacobian = jacobian();
1167 }
1168 
1183 ReturnMatrix Stewart::Find_wRp ()
1184 {
1185  Matrix _wRp(3,3);
1186 
1187  _wRp(1,1) = cos(q(6))*cos(q(4)) - cos(q(5))*sin(q(4))*sin(q(6));
1188  _wRp(1,2) = -sin(q(6))*cos(q(4)) - cos(q(5))*sin(q(4))*cos(q(6));
1189  _wRp(1,3) = sin(q(5))*sin(q(4));
1190  _wRp(2,1) = sin(q(6))*cos(q(4)) + cos(q(5))*sin(q(4))*cos(q(6));
1191  _wRp(2,2) = -sin(q(6))*sin(q(4)) + cos(q(6))*cos(q(4))*cos(q(5));
1192  _wRp(2,3) = -sin(q(5))*cos(q(4));
1193  _wRp(3,1) = sin(q(6))*sin(q(5));
1194  _wRp(3,2) = sin(q(5))*cos(q(6));
1195  _wRp(3,3) = cos(q(5));
1196 
1197  _wRp.Release();
1198  return _wRp;
1199 }
1200 
1220 ReturnMatrix Stewart::Find_Omega()
1221 {
1222  ColumnVector w(3);
1223 
1224  w(1) = cos(q(4))*dq(5) + sin(q(4))*cos(q(5))*dq(6);
1225  w(2) = sin(q(4))*dq(5) - cos(q(4))*sin(q(5))*dq(6);
1226  w(3) = dq(4) + cos(q(5))*dq(6);
1227 
1228  w.Release();
1229  return w;
1230 }
1231 
1258 ReturnMatrix Stewart::Find_Alpha()
1259 {
1260  Matrix A, Temp(3,3),Temp2(3,3);
1261 
1262  Temp = 0.0; Temp(3,1) = 1; Temp(1,2) = cos(q(4)); Temp(2,2) = sin(q(4));
1263  Temp(1,3) = sin(q(4))*cos(q(5)); Temp(2,3)=-cos(q(4))*sin(q(5)); Temp(3,3) = cos(q(5));
1264 
1265  Temp2 = 0.0; Temp2(1,2) = -dq(4)*sin(q(4)); Temp2(2,2) = dq(4)*cos(q(4));
1266  Temp2(1,3) = dq(4)*cos(q(4))*sin(q(5))+dq(5)*sin(q(4))*cos(q(5));
1267  Temp2(2,3) = dq(4)*sin(q(4))*sin(q(5))-dq(5)*cos(q(4))*cos(q(5));
1268  Temp2(3,3) = -dq(5)*sin(q(5));
1269 
1270  A = Temp*ddq.Rows(4,6) + Temp2*dq.Rows(4,6);
1271 
1272  A.Release();
1273  return A;
1274 }
1275 
1287 ReturnMatrix Stewart::jacobian()
1288 {
1289  Matrix _Jacobi;
1290 
1291  _Jacobi = (IJ1*IJ2).i();
1292 
1293  _Jacobi.Release();
1294  return _Jacobi;
1295 }
1296 
1313 ReturnMatrix Stewart::Find_InvJacob1()
1314 {
1315  Matrix tmp_Jacobi1 (6,6);
1316 
1317  for(int i = 0; i<6; i++)
1318  tmp_Jacobi1.Row(i+1) = Links[i].UnitV.t() | CrossProduct(wRp*Links[i].ap,Links[i].UnitV).t();
1319 
1320  tmp_Jacobi1.Release();
1321  return tmp_Jacobi1;
1322 }
1323 
1341 ReturnMatrix Stewart::Find_InvJacob2()
1342 {
1343  Matrix tmp_Jacobi2;
1344 
1345  tmp_Jacobi2 = IdentityMatrix(6);
1346  tmp_Jacobi2(4,4) = 0;
1347  tmp_Jacobi2(6,4) = 1;
1348  tmp_Jacobi2(4,5) = cos(q(4));
1349  tmp_Jacobi2(5,5) = sin(q(4));
1350  tmp_Jacobi2(4,6) = sin(q(4))*sin(q(5));
1351  tmp_Jacobi2(5,6) = -cos(q(4))*sin(q(5));
1352  tmp_Jacobi2(6,6) = cos(q(5));
1353 
1354  tmp_Jacobi2.Release();
1355  return tmp_Jacobi2;
1356 }
1386 ReturnMatrix Stewart::jacobian_dot()
1387 {
1388  Matrix tmp_dJ2(6,6), tmp_dn(6,3), tmp_sol(6,3), tmp_dJ1(6,6), tmp_dJ(6,6);
1389  ColumnVector VctNorm, a;
1390 
1391  tmp_dJ2 = 0.0;
1392  tmp_dJ2(4,5) = -dq(4)*sin(q(4));
1393  tmp_dJ2(5,5) = dq(4)*cos(q(4));
1394  tmp_dJ2(4,6) = dq(4)*cos(q(4))*sin(q(5))+dq(5)*sin(q(4))*cos(q(5));
1395  tmp_dJ2(5,6) = dq(4)*sin(q(4))*sin(q(5))-dq(5)*cos(q(4))*cos(q(5));
1396  tmp_dJ2(6,6) = -dq(5)*sin(q(5));
1397 
1398  for (int i = 0; i<6; i++)
1399  {
1400  tmp_dn.Row(i+1) = ((Links[i].UnitV*Links[i].L -
1401  dl(i+1)*Links[i].UnitV)/Links[i].L).t();
1402  tmp_sol.Row(i+1) = (CrossProduct(CrossProduct(Omega,Links[i].aPos.t()),
1403  Links[i].UnitV.t()) +
1404  (CrossProduct(Links[i].aPos,tmp_dn.Row(i+1)))).t();
1405  }
1406 
1407  for (int j = 1; j < 7; j++)
1408  for(int k = 1; k < 4; k++)
1409  {
1410  tmp_dJ1(j,k) = tmp_dn(j,k);
1411  tmp_dJ1(j,k+3) = tmp_sol(j,k);
1412  }
1413 
1414  tmp_dJ = tmp_dJ1*IJ2 + IJ1*tmp_dJ2;
1415 
1416  tmp_dJ.Release();
1417  return tmp_dJ;
1418 }
1426 ReturnMatrix Stewart::InvPosKine()
1427 {
1428  ColumnVector Vct_L(6);
1429 
1430  for (int i = 1; i < 7; i++)
1431  Vct_L(i) = Links[i-1].L;
1432 
1433  Vct_L.Release(); return Vct_L;
1434 }
1435 
1448 ReturnMatrix Stewart::Find_dl()
1449 {
1450  ColumnVector tmp_dl;
1451  tmp_dl = Jacobian.i()*dq;
1452 
1453  tmp_dl.Release();
1454  return tmp_dl;
1455 }
1456 
1469 ReturnMatrix Stewart::Find_ddl()
1470 {
1471  ColumnVector tmp_ddl;
1472  tmp_ddl = Jacobian.i() * ddq + jacobian_dot() * dq;
1473  tmp_ddl.Release();
1474  return tmp_ddl;
1475 }
1476 
1509 ReturnMatrix Stewart::Find_C(const Real Gravity)
1510 {
1511  Matrix C(6,1), I(3,3);
1512  ColumnVector Sum(3), Sum2(3), Sum3(3), ddxg(3), LNormalForce(3);
1513 
1514  gravity = 0;
1515  gravity(2) = -Gravity;
1516 
1517  ddxg = ddq.Rows(1,3) + CrossProduct(Alpha,wRp*pR) + CrossProduct(Omega,CrossProduct(Omega,wRp*pR));
1518  I = wRp*pIp*(wRp.t());
1519 
1520  Sum = 0.0;
1521  Sum2 = 0.0;
1522  Sum3 = 0.0;
1523  for (int i=0; i<6; i++)
1524  {
1525  LNormalForce = Links[i].NormalForce();
1526  Sum = Sum + LNormalForce;
1527  Sum2 = Sum2 + CrossProduct(Links[i].aPos,LNormalForce);
1528  if(!UJointAtBase)
1529  {
1530  Sum3 = Sum3 +Links[i].Moment();
1531  }
1532  }
1533 
1534  C.Rows(1,3) = mp*gravity - mp*ddxg - Sum;
1535  C.Rows(4,6) = mp*CrossProduct(wRp*pR, gravity) - mp*CrossProduct(wRp*pR,ddxg)-I*Alpha
1536  + I*CrossProduct(Omega,Omega)- Sum2 - Sum3;
1537 
1538  C.Release();
1539  return C;
1540 }
1541 
1564 ReturnMatrix Stewart::ForwardKine(const ColumnVector guess_q, const ColumnVector l_given,
1565  const Real tolerance)
1566 {
1567  ColumnVector next_q, tmp_long(6);
1568  Real Diff = 1;
1569 
1570  q = guess_q;
1571  while (Diff>tolerance)
1572  {
1573  for(int i=0; i<6; i++)
1574  tmp_long(i+1) = Links[i].L - l_given(i+1);
1575 
1576  next_q = q - Jacobian*(tmp_long);
1577  Diff = (next_q - q).MaximumAbsoluteValue();
1578 
1579  set_q(next_q);
1580  }
1581  next_q.Release();
1582  return next_q;
1583 }
1584 
1592 ReturnMatrix Stewart::JointSpaceForceVct(const Real Gravity)
1593 {
1594  Matrix F(6,1), C, IJ1(6,6);
1595 
1596  IJ1 = Find_InvJacob1();
1597  C = Find_C(Gravity);
1598 
1599  for (int i =0; i<6; i++)
1600  {
1601  F(i+1,1) = Links[i].ActuationForce(IJ1, C, i+1, Gravity);
1602  }
1603 
1604  F.Release();
1605  return F;
1606 }
1607 
1622 ReturnMatrix Stewart::Torque(const Real Gravity)
1623 {
1624  Matrix T;
1625 
1626  for(int i=0;i<6;i++)
1627  Links[i].tau_LTransform(dl(i+1), ddl(i+1), Gravity);
1628 
1629  T = Jacobian.i().t()*JointSpaceForceVct(Gravity);
1630 
1631  T.Release();
1632  return T;
1633 }
1643 ReturnMatrix Stewart::Find_h(const Real Gravity)
1644 {
1645  ColumnVector _ddq(6);
1646  _ddq = 0.0;
1647  set_ddq(_ddq);
1648  return Torque(Gravity);
1649 }
1650 
1660 ReturnMatrix Stewart::Find_M()
1661 {
1662  Matrix M(6,6);
1663  ColumnVector _ddq(6), _dq(6), tmpdq(6);
1664 
1665  tmpdq = dq;
1666  _dq = 0.0;
1667 
1668  set_dq(_dq);
1669 
1670  for (int i = 1; i < 7; i++)
1671  {
1672  _ddq = 0.0;
1673  _ddq(i) = 1.0;
1674  set_ddq(_ddq);
1675  M.Column(i) = Torque (0.0);
1676  }
1677  set_dq(tmpdq);
1678 
1679  M.Release();
1680  return M;
1681 }
1682 
1698 ReturnMatrix Stewart::ForwardDyn(const ColumnVector T, const Real Gravity)
1699 {
1700  ColumnVector _ddq;
1701 
1702  _ddq = Find_M().i()*(T - Find_h(Gravity));
1703 
1704  _ddq.Release();
1705  return _ddq;
1706 }
1707 
1733 void Stewart::Find_Mc_Nc_Gc(Matrix & Mc, Matrix & Nc, Matrix & Gc)
1734 {
1735  Matrix G, Ka(6,6), Ma(6,6), Va(6,6), dJacobian(6,6);
1736  dJacobian = jacobian_dot();
1737 
1738  Ka = p/(2*M_PI*n)*IdentityMatrix(6);
1739  Ma = (2*M_PI/(n*p))*(Js + n*n*Jm)*IdentityMatrix(6);
1740  Va = (2*M_PI/(n*p))*(bs + n*n*bm)*IdentityMatrix(6);
1741 
1742  Mc = Ka*Jacobian.t() * Find_M() + Ma*Jacobian.i();
1743 
1744  Nc = Ka*Jacobian.t()*Find_h(0.0) + (Va*Jacobian.i()+Ma*dJacobian)*dq;
1745 
1746  ColumnVector _dq(6), _tmpdq(6);
1747  _dq = 0.0;
1748  _tmpdq = dq;
1749  set_dq(_dq);
1750 
1751  Gc = Ka *Jacobian.t()*Find_h();
1752  set_dq(_tmpdq);
1753 }
1754 
1789 ReturnMatrix Stewart::ForwardDyn_AD(const ColumnVector Command, const Real t)
1790 {
1791  Matrix _ddq;
1792  Matrix Nc,Gc,Mc, tmp1,tmp2;
1793 
1794  Find_Mc_Nc_Gc(Mc,Nc,Gc);
1795 
1796  tmp1 = (Command - (Jacobian.i()*dq*(2*M_PI/p)*Kb));
1797  tmp2 = (IdentityMatrix(6)*Kt/L*exp(-R*t/L))*tmp1;
1798 
1799  _ddq = Mc.i()*(tmp2 - Nc - Gc);
1800 
1801  _ddq.Release();
1802  return _ddq;
1803 }
1804 
1805 #ifdef use_namespace
1806 }
1807 #endif
1808