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
23 Professeur Agrege
24 Departement de genie electrique
25 Ecole Polytechnique de Montreal
26 C.P. 6079, Succ. Centre-Ville
27 Montreal, Quebec, H3C 3A7
28 
29 email: richard.gourdeau@polymtl.ca
30 
31 -------------------------------------------------------------------------------
32 Revision_history:
33 
34 2003/29/04: Etienne Lachance
35  -Fix Robot::delta_torque.
36  -Added mRobot/mRobot_min_para::delta_torque.
37 
38 2004/05/14: Etienne Lachance
39  -Replaced vec_x_prod by CrossProduct.
40 
41 2004/07/01: Ethan Tira-Thompson
42  -Added support for newmat's use_namespace #define, using ROBOOP namespace
43 
44 2004/07/02: Etienne Lachance
45  -Added Doxygen comments.
46 -------------------------------------------------------------------------------
47 */
48 
54 static const char rcsid[] = "$Id: delta_t.cpp,v 1.17 2005/07/01 16:11:45 gourdeau Exp $";
56 
57 #include "robot.h"
58 
59 #ifdef use_namespace
60 namespace ROBOOP {
61  using namespace NEWMAT;
62 #endif
63 
64 
65 void Robot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
66  const ColumnVector & qpp, const ColumnVector & dq,
67  const ColumnVector & dqp, const ColumnVector & dqpp,
68  ColumnVector & ltorque, ColumnVector & dtorque)
156 {
157  int i;
158  Matrix Rt, temp;
159  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
160  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
161  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
162  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
163  if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
164  if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
165  ltorque = ColumnVector(dof);
166  dtorque = ColumnVector(dof);
167  set_q(q);
168 
169  vp[0] = gravity;
170  ColumnVector z0(3);
171  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
172  Matrix Q(3,3);
173  Q = 0.0;
174  Q(1,2) = -1.0;
175  Q(2,1) = 1.0;
176  for(i = 1; i <= dof; i++)
177  {
178  Rt = links[i].R.t();
179  p[i] = ColumnVector(3);
180  p[i](1) = links[i].get_a();
181  p[i](2) = links[i].get_d() * Rt(2,3);
182  p[i](3) = links[i].get_d() * Rt(3,3);
183  if(links[i].get_joint_type() != 0)
184  {
185  dp[i] = ColumnVector(3);
186  dp[i](1) = 0.0;
187  dp[i](2) = Rt(2,3);
188  dp[i](3) = Rt(3,3);
189  }
190  if(links[i].get_joint_type() == 0)
191  {
192  w[i] = Rt*(w[i-1] + z0*qp(i));
193  dw[i] = Rt*(dw[i-1] + z0*dqp(i)
194  - Q*(w[i-1] + z0*qp(i))*dq(i));
195  wp[i] = Rt*(wp[i-1] + z0*qpp(i)
196  + CrossProduct(w[i-1],z0*qp(i)));
197  dwp[i] = Rt*(dwp[i-1] + z0*dqpp(i)
198  + CrossProduct(dw[i-1],z0*qp(i))
199  + CrossProduct(w[i-1],z0*dqp(i))
200  - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i)))
201  *dq(i));
202  vp[i] = CrossProduct(wp[i],p[i])
203  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
204  + Rt*(vp[i-1]);
205  dvp[i] = CrossProduct(dwp[i],p[i])
206  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
207  + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
208  + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
209  }
210  else
211  {
212  w[i] = Rt*w[i-1];
213  dw[i] = Rt*dw[i-1];
214  wp[i] = Rt*wp[i-1];
215  dwp[i] = Rt*dwp[i-1];
216  vp[i] = Rt*(vp[i-1] + z0*qpp(i)
217  + 2.0*CrossProduct(w[i-1],z0*qp(i)))
218  + CrossProduct(wp[i],p[i])
219  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
220  dvp[i] = Rt*(dvp[i-1] + z0*dqpp(i)
221  + 2.0*(CrossProduct(dw[i-1],z0*qp(i))
222  + CrossProduct(w[i-1],z0*dqp(i))))
223  + CrossProduct(dwp[i],p[i])
224  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
225  + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
226  + (CrossProduct(wp[i],dp[i])
227  + CrossProduct(w[i],CrossProduct(w[i],dp[i])))
228  *dq(i);
229  }
230  a[i] = CrossProduct(wp[i],links[i].r)
231  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
232  + vp[i];
233  da[i] = CrossProduct(dwp[i],links[i].r)
234  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
235  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
236  + dvp[i];
237  }
238 
239  for(i = dof; i >= 1; i--)
240  {
241  F[i] = a[i] * links[i].m;
242  dF[i] = da[i] * links[i].m;
243  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
244  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
245  + CrossProduct(w[i],links[i].I*dw[i]);
246  if(i == dof)
247  {
248  f[i] = F[i];
249  df[i] = dF[i];
250  n[i] = CrossProduct(p[i],f[i])
251  + CrossProduct(links[i].r,F[i]) + N[i];
252  dn[i] = CrossProduct(p[i],df[i])
253  + CrossProduct(links[i].r,dF[i]) + dN[i];
254  if(links[i].get_joint_type() != 0)
255  dn[i] += CrossProduct(dp[i],f[i])*dq(i);
256  }
257  else
258  {
259  f[i] = links[i+1].R*f[i+1] + F[i];
260  df[i] = links[i+1].R*df[i+1] + dF[i];
261  if(links[i].get_joint_type() == 0)
262  df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
263  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
264  + CrossProduct(links[i].r,F[i]) + N[i];
265  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
266  + CrossProduct(links[i].r,dF[i]) + dN[i];
267  if(links[i].get_joint_type() == 0)
268  dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
269  else
270  dn[i] += CrossProduct(dp[i],f[i])*dq(i);
271  }
272 
273  if(links[i].get_joint_type() == 0)
274  {
275  temp = ((z0.t()*links[i].R)*n[i]);
276  ltorque(i) = temp(1,1);
277  temp = ((z0.t()*links[i].R)*dn[i]);
278  dtorque(i) = temp(1,1);
279  }
280  else
281  {
282  temp = ((z0.t()*links[i].R)*f[i]);
283  ltorque(i) = temp(1,1);
284  temp = ((z0.t()*links[i].R)*df[i]);
285  dtorque(i) = temp(1,1);
286  }
287  }
288 }
289 
290 
291 void mRobot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
292  const ColumnVector & qpp, const ColumnVector & dq,
293  const ColumnVector & dqp, const ColumnVector & dqpp,
294  ColumnVector & ltorque, ColumnVector & dtorque)
384 {
385  int i;
386  Matrix Rt, temp;
387  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
388  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
389  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
390  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
391  if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
392  if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
393  ltorque = ColumnVector(dof);
394  dtorque = ColumnVector(dof);
395  set_q(q);
396 
397  vp[0] = gravity;
398  ColumnVector z0(3);
399  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
400  Matrix Q(3,3);
401  Q = 0.0;
402  Q(1,2) = -1.0;
403  Q(2,1) = 1.0;
404  for(i = 1; i <= dof; i++)
405  {
406  Rt = links[i].R.t();
407  p[i] = links[i].p;
408  if(links[i].get_joint_type() != 0)
409  {
410  dp[i] = ColumnVector(3);
411  dp[i](1) = 0.0;
412  dp[i](2) = Rt(2,3);
413  dp[i](3) = Rt(3,3);
414  }
415  if(links[i].get_joint_type() == 0)
416  {
417  w[i] = Rt*w[i-1] + z0*qp(i);
418  dw[i] = Rt*dw[i-1] + z0*dqp(i)
419  - Q*Rt*w[i-1]*dq(i);
420  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
421  + z0*qpp(i);
422  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
423  + CrossProduct(Rt*w[i-1],z0*dqp(i))
424  - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
425  + z0*dqpp(i);
426  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
427  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
428  + vp[i-1]);
429  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
430  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
431  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
432  - Q*Rt*(CrossProduct(wp[i-1],p[i])
433  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
434  }
435  else
436  {
437  w[i] = Rt*w[i-1];
438  dw[i] = Rt*dw[i-1];
439  wp[i] = Rt*wp[i-1];
440  dwp[i] = Rt*dwp[i-1];
441  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
442  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
443  + z0*qpp(i) + 2.0*CrossProduct(w[i],z0*qp(i));
444 
445  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
446  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
447  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
448  + (CrossProduct(wp[i-1],dp[i])
449  + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
450  + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
451  + z0*dqpp(i);
452  }
453  a[i] = CrossProduct(wp[i],links[i].r)
454  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
455  + vp[i];
456  da[i] = CrossProduct(dwp[i],links[i].r)
457  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
458  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
459  + dvp[i];
460  }
461 
462  for(i = dof; i >= 1; i--) {
463  F[i] = a[i] * links[i].m;
464  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
465  dF[i] = da[i] * links[i].m;
466  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
467  + CrossProduct(w[i],links[i].I*dw[i]);
468 
469  if(i == dof)
470  {
471  f[i] = F[i];
472  df[i] = dF[i];
473  n[i] = CrossProduct(links[i].r,F[i]) + N[i];
474  dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
475  }
476  else
477  {
478  f[i] = links[i+1].R*f[i+1] + F[i];
479  df[i] = links[i+1].R*df[i+1] + dF[i];
480  if(links[i+1].get_joint_type() == 0)
481  df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
482 
483  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
484  + CrossProduct(links[i].r,F[i]) + N[i];
485  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
486  + CrossProduct(links[i].r,dF[i]) + dN[i];
487  if(links[i+1].get_joint_type() == 0)
488  dn[i] += (links[i+1].R*Q*n[i+1] +
489  CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
490  else
491  dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
492  }
493 
494  if(links[i].get_joint_type() == 0)
495  {
496  temp = z0.t()*n[i];
497  ltorque(i) = temp(1,1);
498  temp = z0.t()*dn[i];
499  dtorque(i) = temp(1,1);
500  }
501  else
502  {
503  temp = z0.t()*f[i];
504  ltorque(i) = temp(1,1);
505  temp = z0.t()*df[i];
506  dtorque(i) = temp(1,1);
507  }
508  }
509 }
510 
511 void mRobot_min_para::delta_torque(const ColumnVector & q, const ColumnVector & qp,
512  const ColumnVector & qpp, const ColumnVector & dq,
513  const ColumnVector & dqp, const ColumnVector & dqpp,
514  ColumnVector & ltorque, ColumnVector & dtorque)
520 {
521  int i;
522  Matrix Rt, temp;
523  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
524  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
525  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
526  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
527  if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
528  if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
529  ltorque = ColumnVector(dof);
530  dtorque = ColumnVector(dof);
531  set_q(q);
532 
533  vp[0] = gravity;
534  ColumnVector z0(3);
535  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
536  Matrix Q(3,3);
537  Q = 0.0;
538  Q(1,2) = -1.0;
539  Q(2,1) = 1.0;
540  for(i = 1; i <= dof; i++)
541  {
542  Rt = links[i].R.t();
543  p[i] = links[i].p;
544  if(links[i].get_joint_type() != 0)
545  {
546  dp[i] = ColumnVector(3);
547  dp[i](1) = 0.0;
548  dp[i](2) = Rt(2,3);
549  dp[i](3) = Rt(3,3);
550  }
551  if(links[i].get_joint_type() == 0)
552  {
553  w[i] = Rt*w[i-1] + z0*qp(i);
554  dw[i] = Rt*dw[i-1] + z0*dqp(i)
555  - Q*Rt*w[i-1]*dq(i);
556  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
557  + z0*qpp(i);
558  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
559  + CrossProduct(Rt*w[i-1],z0*dqp(i))
560  - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
561  + z0*dqpp(i);
562  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
563  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
564  + vp[i-1]);
565  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
566  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
567  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
568  - Q*Rt*(CrossProduct(wp[i-1],p[i])
569  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
570  }
571  else
572  {
573  w[i] = Rt*w[i-1];
574  dw[i] = Rt*dw[i-1];
575  wp[i] = Rt*wp[i-1];
576  dwp[i] = Rt*dwp[i-1];
577  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
578  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
579  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
580  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
581  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
582  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
583  + (CrossProduct(wp[i-1],dp[i])
584  + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
585  + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
586  + z0*dqpp(i);
587  }
588  }
589 
590  for(i = dof; i >= 1; i--) {
591  F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
592  CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
593  dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
594  + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
595  + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
596  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
597  - CrossProduct(vp[i], links[i].mc);
598  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
599  + CrossProduct(w[i],links[i].I*dw[i])
600  - CrossProduct(dvp[i],links[i].mc);
601 
602  if(i == dof)
603  {
604  f[i] = F[i];
605  df[i] = dF[i];
606  n[i] = N[i];
607  dn[i] = dN[i];
608  }
609  else
610  {
611  f[i] = links[i+1].R*f[i+1] + F[i];
612  df[i] = links[i+1].R*df[i+1] + dF[i];
613  if(links[i+1].get_joint_type() == 0)
614  df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
615 
616  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
617  + N[i];
618  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
619  + dN[i];
620  if(links[i+1].get_joint_type() == 0)
621  dn[i] += (links[i+1].R*Q*n[i+1] +
622  CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
623  else
624  dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
625  }
626 
627  if(links[i].get_joint_type() == 0)
628  {
629  temp = z0.t()*n[i];
630  ltorque(i) = temp(1,1);
631  temp = z0.t()*dn[i];
632  dtorque(i) = temp(1,1);
633  }
634  else
635  {
636  temp = z0.t()*f[i];
637  ltorque(i) = temp(1,1);
638  temp = z0.t()*df[i];
639  dtorque(i) = temp(1,1);
640  }
641  }
642 }
643 
644 #ifdef use_namespace
645 }
646 #endif
rcsid
static const char rcsid[]
RCS/CVS version.
Definition: delta_t.cpp:55
robot.h
Robots class definitions.
mRobot_min_para::delta_torque
virtual void delta_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, const ColumnVector &dqp, const ColumnVector &dqpp, ColumnVector &torque, ColumnVector &dtorque)
Delta torque dynamics.
Definition: delta_t.cpp:511
mRobot::delta_torque
virtual void delta_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, const ColumnVector &dqp, const ColumnVector &dqpp, ColumnVector &torque, ColumnVector &dtorque)
Delta torque dynamics.
Definition: delta_t.cpp:291
GeneralMatrix::Nrows
int Nrows() const
Definition: newmat.h:494
BaseMatrix::t
TransposedMatrix t() const
Definition: newmat6.cpp:320
CrossProduct
Matrix CrossProduct(const Matrix &A, const Matrix &B)
Definition: newmat.h:2062
Matrix
The usual rectangular matrix.
Definition: newmat.h:625
a
FloatVector FloatVector * a
Definition: include/libKinematics.h:78
GeneralMatrix::Ncols
int Ncols() const
Definition: newmat.h:495
Robot::delta_torque
virtual void delta_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, const ColumnVector &dqp, const ColumnVector &dqpp, ColumnVector &ltorque, ColumnVector &dtorque)
Delta torque dynamics.
Definition: delta_t.cpp:65
ColumnVector
Column vector.
Definition: newmat.h:1008

kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16