ROBOOP, A Robotics Object Oriented Package in C++
delta_t.cpp
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 1996-2004 Richard Gourdeau
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 
20 Report problems and direct all questions to:
21 
22 Richard Gourdeau, Professeur
23 Departement de genie electrique
24 Ecole Polytechnique de Montreal
25 C.P. 6079, Succ. Centre-Ville
26 Montreal, Quebec, H3C 3A7
27 
28 email: richard.gourdeau@polymtl.ca
29 
30 -------------------------------------------------------------------------------
31 Revision_history:
32 
33 2003/29/04: Etienne Lachance
34  -Fix Robot::delta_torque.
35  -Added mRobot/mRobot_min_para::delta_torque.
36 
37 2004/05/14: Etienne Lachance
38  -Replaced vec_x_prod by CrossProduct.
39 
40 2004/07/01: Ethan Tira-Thompson
41  -Added support for newmat's use_namespace #define, using ROBOOP namespace
42 
43 2004/07/02: Etienne Lachance
44  -Added Doxygen comments.
45 -------------------------------------------------------------------------------
46 */
47 
53 #include "robot.h"
54 
55 #ifdef use_namespace
56 namespace ROBOOP {
57  using namespace NEWMAT;
58 #endif
59 
60 
61 void Robot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
62  const ColumnVector & qpp, const ColumnVector & dq,
63  const ColumnVector & dqp, const ColumnVector & dqpp,
64  ColumnVector & ltorque, ColumnVector & dtorque)
152 {
153  int i;
154  Matrix Rt, temp;
155  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
156  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
157  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
158  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
159  if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
160  if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
161  ltorque = ColumnVector(dof);
162  dtorque = ColumnVector(dof);
163  set_q(q);
164 
165  vp[0] = gravity;
166  ColumnVector z0(3);
167  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
168  Matrix Q(3,3);
169  Q = 0.0;
170  Q(1,2) = -1.0;
171  Q(2,1) = 1.0;
172  for(i = 1; i <= dof; i++)
173  {
174  Rt = links[i].R.t();
175  p[i] = ColumnVector(3);
176  p[i](1) = links[i].get_a();
177  p[i](2) = links[i].get_d() * Rt(2,3);
178  p[i](3) = links[i].get_d() * Rt(3,3);
179  if(links[i].get_joint_type() != 0)
180  {
181  dp[i] = ColumnVector(3);
182  dp[i](1) = 0.0;
183  dp[i](2) = Rt(2,3);
184  dp[i](3) = Rt(3,3);
185  }
186  if(links[i].get_joint_type() == 0)
187  {
188  w[i] = Rt*(w[i-1] + z0*qp(i));
189  dw[i] = Rt*(dw[i-1] + z0*dqp(i)
190  - Q*(w[i-1] + z0*qp(i))*dq(i));
191  wp[i] = Rt*(wp[i-1] + z0*qpp(i)
192  + CrossProduct(w[i-1],z0*qp(i)));
193  dwp[i] = Rt*(dwp[i-1] + z0*dqpp(i)
194  + CrossProduct(dw[i-1],z0*qp(i))
195  + CrossProduct(w[i-1],z0*dqp(i))
196  - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i)))
197  *dq(i));
198  vp[i] = CrossProduct(wp[i],p[i])
199  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
200  + Rt*(vp[i-1]);
201  dvp[i] = CrossProduct(dwp[i],p[i])
202  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
203  + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
204  + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
205  }
206  else
207  {
208  w[i] = Rt*w[i-1];
209  dw[i] = Rt*dw[i-1];
210  wp[i] = Rt*wp[i-1];
211  dwp[i] = Rt*dwp[i-1];
212  vp[i] = Rt*(vp[i-1] + z0*qpp(i)
213  + 2.0*CrossProduct(w[i-1],z0*qp(i)))
214  + CrossProduct(wp[i],p[i])
215  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
216  dvp[i] = Rt*(dvp[i-1] + z0*dqpp(i)
217  + 2.0*(CrossProduct(dw[i-1],z0*qp(i))
218  + CrossProduct(w[i-1],z0*dqp(i))))
219  + CrossProduct(dwp[i],p[i])
220  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
221  + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
222  + (CrossProduct(wp[i],dp[i])
223  + CrossProduct(w[i],CrossProduct(w[i],dp[i])))
224  *dq(i);
225  }
226  a[i] = CrossProduct(wp[i],links[i].r)
227  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
228  + vp[i];
229  da[i] = CrossProduct(dwp[i],links[i].r)
230  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
231  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
232  + dvp[i];
233  }
234 
235  for(i = dof; i >= 1; i--)
236  {
237  F[i] = a[i] * links[i].m;
238  dF[i] = da[i] * links[i].m;
239  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
240  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
241  + CrossProduct(w[i],links[i].I*dw[i]);
242  if(i == dof)
243  {
244  f[i] = F[i];
245  df[i] = dF[i];
246  n[i] = CrossProduct(p[i],f[i])
247  + CrossProduct(links[i].r,F[i]) + N[i];
248  dn[i] = CrossProduct(p[i],df[i])
249  + CrossProduct(links[i].r,dF[i]) + dN[i];
250  if(links[i].get_joint_type() != 0)
251  dn[i] += CrossProduct(dp[i],f[i])*dq(i);
252  }
253  else
254  {
255  f[i] = links[i+1].R*f[i+1] + F[i];
256  df[i] = links[i+1].R*df[i+1] + dF[i];
257  if(links[i].get_joint_type() == 0)
258  df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
259  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
260  + CrossProduct(links[i].r,F[i]) + N[i];
261  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
262  + CrossProduct(links[i].r,dF[i]) + dN[i];
263  if(links[i].get_joint_type() == 0)
264  dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
265  else
266  dn[i] += CrossProduct(dp[i],f[i])*dq(i);
267  }
268 
269  if(links[i].get_joint_type() == 0)
270  {
271  temp = ((z0.t()*links[i].R)*n[i]);
272  ltorque(i) = temp(1,1);
273  temp = ((z0.t()*links[i].R)*dn[i]);
274  dtorque(i) = temp(1,1);
275  }
276  else
277  {
278  temp = ((z0.t()*links[i].R)*f[i]);
279  ltorque(i) = temp(1,1);
280  temp = ((z0.t()*links[i].R)*df[i]);
281  dtorque(i) = temp(1,1);
282  }
283  }
284 }
285 
286 
287 void mRobot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
288  const ColumnVector & qpp, const ColumnVector & dq,
289  const ColumnVector & dqp, const ColumnVector & dqpp,
290  ColumnVector & ltorque, ColumnVector & dtorque)
380 {
381  int i;
382  Matrix Rt, temp;
383  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
384  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
385  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
386  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
387  if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
388  if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
389  ltorque = ColumnVector(dof);
390  dtorque = ColumnVector(dof);
391  set_q(q);
392 
393  vp[0] = gravity;
394  ColumnVector z0(3);
395  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
396  Matrix Q(3,3);
397  Q = 0.0;
398  Q(1,2) = -1.0;
399  Q(2,1) = 1.0;
400  for(i = 1; i <= dof; i++)
401  {
402  Rt = links[i].R.t();
403  p[i] = links[i].p;
404  if(links[i].get_joint_type() != 0)
405  {
406  dp[i] = ColumnVector(3);
407  dp[i](1) = 0.0;
408  dp[i](2) = Rt(2,3);
409  dp[i](3) = Rt(3,3);
410  }
411  if(links[i].get_joint_type() == 0)
412  {
413  w[i] = Rt*w[i-1] + z0*qp(i);
414  dw[i] = Rt*dw[i-1] + z0*dqp(i)
415  - Q*Rt*w[i-1]*dq(i);
416  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
417  + z0*qpp(i);
418  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
419  + CrossProduct(Rt*w[i-1],z0*dqp(i))
420  - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
421  + z0*dqpp(i);
422  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
423  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
424  + vp[i-1]);
425  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
426  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
427  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
428  - Q*Rt*(CrossProduct(wp[i-1],p[i])
429  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
430  }
431  else
432  {
433  w[i] = Rt*w[i-1];
434  dw[i] = Rt*dw[i-1];
435  wp[i] = Rt*wp[i-1];
436  dwp[i] = Rt*dwp[i-1];
437  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
438  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
439  + z0*qpp(i) + 2.0*CrossProduct(w[i],z0*qp(i));
440 
441  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
442  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
443  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
444  + (CrossProduct(wp[i-1],dp[i])
445  + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
446  + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
447  + z0*dqpp(i);
448  }
449  a[i] = CrossProduct(wp[i],links[i].r)
450  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
451  + vp[i];
452  da[i] = CrossProduct(dwp[i],links[i].r)
453  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
454  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
455  + dvp[i];
456  }
457 
458  for(i = dof; i >= 1; i--) {
459  F[i] = a[i] * links[i].m;
460  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
461  dF[i] = da[i] * links[i].m;
462  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
463  + CrossProduct(w[i],links[i].I*dw[i]);
464 
465  if(i == dof)
466  {
467  f[i] = F[i];
468  df[i] = dF[i];
469  n[i] = CrossProduct(links[i].r,F[i]) + N[i];
470  dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
471  }
472  else
473  {
474  f[i] = links[i+1].R*f[i+1] + F[i];
475  df[i] = links[i+1].R*df[i+1] + dF[i];
476  if(links[i+1].get_joint_type() == 0)
477  df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
478 
479  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
480  + CrossProduct(links[i].r,F[i]) + N[i];
481  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
482  + CrossProduct(links[i].r,dF[i]) + dN[i];
483  if(links[i+1].get_joint_type() == 0)
484  dn[i] += (links[i+1].R*Q*n[i+1] +
485  CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
486  else
487  dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
488  }
489 
490  if(links[i].get_joint_type() == 0)
491  {
492  temp = z0.t()*n[i];
493  ltorque(i) = temp(1,1);
494  temp = z0.t()*dn[i];
495  dtorque(i) = temp(1,1);
496  }
497  else
498  {
499  temp = z0.t()*f[i];
500  ltorque(i) = temp(1,1);
501  temp = z0.t()*df[i];
502  dtorque(i) = temp(1,1);
503  }
504  }
505 }
506 
507 void mRobot_min_para::delta_torque(const ColumnVector & q, const ColumnVector & qp,
508  const ColumnVector & qpp, const ColumnVector & dq,
509  const ColumnVector & dqp, const ColumnVector & dqpp,
510  ColumnVector & ltorque, ColumnVector & dtorque)
516 {
517  int i;
518  Matrix Rt, temp;
519  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
520  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
521  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
522  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
523  if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
524  if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
525  ltorque = ColumnVector(dof);
526  dtorque = ColumnVector(dof);
527  set_q(q);
528 
529  vp[0] = gravity;
530  ColumnVector z0(3);
531  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
532  Matrix Q(3,3);
533  Q = 0.0;
534  Q(1,2) = -1.0;
535  Q(2,1) = 1.0;
536  for(i = 1; i <= dof; i++)
537  {
538  Rt = links[i].R.t();
539  p[i] = links[i].p;
540  if(links[i].get_joint_type() != 0)
541  {
542  dp[i] = ColumnVector(3);
543  dp[i](1) = 0.0;
544  dp[i](2) = Rt(2,3);
545  dp[i](3) = Rt(3,3);
546  }
547  if(links[i].get_joint_type() == 0)
548  {
549  w[i] = Rt*w[i-1] + z0*qp(i);
550  dw[i] = Rt*dw[i-1] + z0*dqp(i)
551  - Q*Rt*w[i-1]*dq(i);
552  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
553  + z0*qpp(i);
554  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
555  + CrossProduct(Rt*w[i-1],z0*dqp(i))
556  - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
557  + z0*dqpp(i);
558  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
559  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
560  + vp[i-1]);
561  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
562  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
563  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
564  - Q*Rt*(CrossProduct(wp[i-1],p[i])
565  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
566  }
567  else
568  {
569  w[i] = Rt*w[i-1];
570  dw[i] = Rt*dw[i-1];
571  wp[i] = Rt*wp[i-1];
572  dwp[i] = Rt*dwp[i-1];
573  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
574  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
575  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
576  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
577  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
578  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
579  + (CrossProduct(wp[i-1],dp[i])
580  + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
581  + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
582  + z0*dqpp(i);
583  }
584  }
585 
586  for(i = dof; i >= 1; i--) {
587  F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
588  CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
589  dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
590  + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
591  + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
592  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
593  - CrossProduct(vp[i], links[i].mc);
594  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
595  + CrossProduct(w[i],links[i].I*dw[i])
596  - CrossProduct(dvp[i],links[i].mc);
597 
598  if(i == dof)
599  {
600  f[i] = F[i];
601  df[i] = dF[i];
602  n[i] = N[i];
603  dn[i] = dN[i];
604  }
605  else
606  {
607  f[i] = links[i+1].R*f[i+1] + F[i];
608  df[i] = links[i+1].R*df[i+1] + dF[i];
609  if(links[i+1].get_joint_type() == 0)
610  df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
611 
612  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
613  + N[i];
614  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
615  + dN[i];
616  if(links[i+1].get_joint_type() == 0)
617  dn[i] += (links[i+1].R*Q*n[i+1] +
618  CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
619  else
620  dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
621  }
622 
623  if(links[i].get_joint_type() == 0)
624  {
625  temp = z0.t()*n[i];
626  ltorque(i) = temp(1,1);
627  temp = z0.t()*dn[i];
628  dtorque(i) = temp(1,1);
629  }
630  else
631  {
632  temp = z0.t()*f[i];
633  ltorque(i) = temp(1,1);
634  temp = z0.t()*df[i];
635  dtorque(i) = temp(1,1);
636  }
637  }
638 }
639 
640 #ifdef use_namespace
641 }
642 #endif