dynamics.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 Revision_history:
32 
33 2003/02/03: Etienne Lachance
34  -Member function inertia and acceleration are now part of class Robot_basic.
35  -Added torque member funtions to allowed to had load on last link.
36  -Changed variable "n" and "f" for "n_nv" and "f_nv" in torque_novelocity.
37  -Corrected calculation of wp, vp and n in mRobot::torque and
38  mRobot::torque_novelocity.
39  -Removed all member functions related to classes RobotMotor and mRobotMotor.
40  -Added motor effect in torque function (see ltorque).
41  -Added function call set_qp() and set_qpp in Robot::torque and mRobot::torque.
42 
43 2003/04/29: Etienne Lachance
44  -Corrected vp calculation for prismatic case in mRobot/mRobot_min_para::torque,
45  mRobot_min_para::torque_novelocity.
46  -Added functions Robot_basic::acceleration(const ColumnVector & q,const ColumnVector & qp,
47  const ColumnVector & tau)
48 2003/11/18: Etienne Lachance
49  -Added member function G() (gravity torque) and C() (Coriolis and centrifugal).
50 
51 2004/05/14: Etienne Lachance
52  -Replaced vec_x_prod by CrossProduct.
53 
54 2004/05/21: Etienne Lachance
55  -Added doxygen comments.
56 
57 2004/07/01: Ethan Tira-Thompson
58  -Added support for newmat's use_namespace #define, using ROBOOP namespace
59 
60 2004/07/13: Ethan Tira-Thompson
61  -Re-added the namespace closing brace at the bottom
62 
63 2006/05/19: Richard Gourdeau
64  -Fixed Gear ratio bug for viscous friction (reported by Carmine Lia)
65 -------------------------------------------------------------------------------
66 */
72 static const char rcsid[] = "$Id: dynamics.cpp,v 1.34 2006/05/19 18:32:30 gourdeau Exp $";
74 
75 #include "robot.h"
76 
77 #ifdef use_namespace
78 namespace ROBOOP {
79  using namespace NEWMAT;
80 #endif
81 
82 
85 {
86  Matrix M(dof,dof);
87  ColumnVector torque(dof);
88  set_q(q);
89  for(int i = 1; i <= dof; i++) {
90  for(int j = 1; j <= dof; j++) {
91  torque(j) = (i == j ? 1.0 : 0.0);
92  }
93  torque = torque_novelocity(torque);
94  M.Column(i) = torque;
95  }
96  M.Release(); return M;
97 }
98 
99 
101  const ColumnVector & qp,
102  const ColumnVector & tau_cmd)
104 {
105  ColumnVector qpp(dof);
106  qpp = 0.0;
107  qpp = inertia(q).i()*(tau_cmd-torque(q,qp,qpp));
108  qpp.Release();
109  return qpp;
110 }
111 
113  const ColumnVector & tau_cmd, const ColumnVector & Fext,
114  const ColumnVector & Next)
127 {
128  ColumnVector qpp(dof);
129  qpp = 0.0;
130  qpp = inertia(q).i()*(tau_cmd-torque(q, qp, qpp, Fext, Next));
131  qpp.Release();
132  return qpp;
133 }
134 
136  const ColumnVector & qpp)
141 {
142  ColumnVector Fext(3), Next(3);
143  Fext = 0;
144  Next = 0;
145  return torque(q, qp, qpp, Fext, Next);
146 }
147 
149  const ColumnVector & qpp, const ColumnVector & Fext,
150  const ColumnVector & Next)
215 {
216  int i;
217  ColumnVector ltorque(dof);
218  Matrix Rt, temp;
219  if(q.Nrows() != dof) error("q has wrong dimension");
220  if(qp.Nrows() != dof) error("qp has wrong dimension");
221  if(qpp.Nrows() != dof) error("qpp has wrong dimension");
222  set_q(q);
223  set_qp(qp);
224 
225  vp[0] = gravity;
226  for(i = 1; i <= dof; i++) {
227  Rt = links[i].R.t();
228  if(links[i].get_joint_type() == 0) {
229  w[i] = Rt*(w[i-1] + z0*qp(i));
230  wp[i] = Rt*(wp[i-1] + z0*qpp(i)
231  + CrossProduct(w[i-1],z0*qp(i)));
232  vp[i] = CrossProduct(wp[i],p[i])
233  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
234  + Rt*(vp[i-1]);
235  } else {
236  w[i] = Rt*w[i-1];
237  wp[i] = Rt*wp[i-1];
238  vp[i] = Rt*(vp[i-1] + z0*qpp(i))
239  + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
240  + CrossProduct(wp[i],p[i])
241  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
242  }
243  a[i] = CrossProduct(wp[i],links[i].r)
244  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
245  + vp[i];
246  }
247 
248  for(i = dof; i >= 1; i--) {
249  F[i] = a[i] * links[i].m;
250  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
251  if(i == dof) {
252  f[i] = F[i] + Fext;
253  n[i] = CrossProduct(p[i],f[i])
254  + CrossProduct(links[i].r,F[i]) + N[i] + Next;
255  } else {
256  f[i] = links[i+1].R*f[i+1] + F[i];
257  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
258  + CrossProduct(links[i].r,F[i]) + N[i];
259  }
260  if(links[i].get_joint_type() == 0)
261  temp = ((z0.t()*links[i].R)*n[i]);
262  else
263  temp = ((z0.t()*links[i].R)*f[i]);
264  ltorque(i) = temp(1,1)
265  + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
266  + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
267  }
268 
269  ltorque.Release(); return ltorque;
270 }
271 
277 {
278  int i;
279  ColumnVector ltorque(dof);
280  Matrix Rt, temp;
281  if(qpp.Nrows() != dof) error("qpp has wrong dimension");
282 
283  vp[0] = 0.0;
284  for(i = 1; i <= dof; i++) {
285  Rt = links[i].R.t();
286  if(links[i].get_joint_type() == 0) {
287  wp[i] = Rt*(wp[i-1] + z0*qpp(i));
288  vp[i] = CrossProduct(wp[i],p[i])
289  + Rt*(vp[i-1]);
290  } else {
291  wp[i] = Rt*wp[i-1];
292  vp[i] = Rt*(vp[i-1] + z0*qpp(i))
293  + CrossProduct(wp[i],p[i]);
294  }
295  a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
296  }
297 
298  for(i = dof; i >= 1; i--) {
299  F[i] = a[i] * links[i].m;
300  N[i] = links[i].I*wp[i];
301  if(i == dof) {
302  f_nv[i] = F[i];
303  n_nv[i] = CrossProduct(p[i],f_nv[i])
304  + CrossProduct(links[i].r,F[i]) + N[i];
305  } else {
306  f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
307  n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i],f_nv[i])
308  + CrossProduct(links[i].r,F[i]) + N[i];
309  }
310  if(links[i].get_joint_type() == 0)
311  temp = ((z0.t()*links[i].R)*n_nv[i]);
312  else
313  temp = ((z0.t()*links[i].R)*f_nv[i]);
314  ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
315 
316  }
317 
318  ltorque.Release(); return ltorque;
319 }
320 
323 {
324  int i;
325  ColumnVector ltorque(dof);
326  Matrix Rt, temp;
327 
328  vp[0] = gravity;
329  for(i = 1; i <= dof; i++) {
330  Rt = links[i].R.t();
331  if(links[i].get_joint_type() == 0)
332  vp[i] = Rt*(vp[i-1]);
333  else
334  vp[i] = Rt*vp[i-1];
335 
336  a[i] = vp[i];
337  }
338 
339  for(i = dof; i >= 1; i--) {
340  F[i] = a[i] * links[i].m;
341  if(i == dof) {
342  f[i] = F[i];
343  n[i] = CrossProduct(p[i],f[i])
344  + CrossProduct(links[i].r,F[i]);
345  } else {
346  f[i] = links[i+1].R*f[i+1] + F[i];
347  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
348  + CrossProduct(links[i].r,F[i]);
349  }
350  if(links[i].get_joint_type() == 0)
351  temp = ((z0.t()*links[i].R)*n[i]);
352  else
353  temp = ((z0.t()*links[i].R)*f[i]);
354  ltorque(i) = temp(1,1);
355  }
356 
357  ltorque.Release(); return ltorque;
358 }
359 
362 {
363  int i;
364  ColumnVector ltorque(dof);
365  Matrix Rt, temp;
366  if(qp.Nrows() != dof) error("qp has wrong dimension");
367 
368  vp[0]=0;
369  for(i = 1; i <= dof; i++) {
370  Rt = links[i].R.t();
371  if(links[i].get_joint_type() == 0) {
372  w[i] = Rt*(w[i-1] + z0*qp(i));
373  wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
374  vp[i] = CrossProduct(wp[i],p[i])
375  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
376  + Rt*(vp[i-1]);
377  } else {
378  w[i] = Rt*w[i-1];
379  wp[i] = Rt*wp[i-1];
380  vp[i] = Rt*vp[i-1] + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
381  + CrossProduct(wp[i],p[i])
382  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
383  }
384  a[i] = CrossProduct(wp[i],links[i].r)
385  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
386  + vp[i];
387  }
388 
389  for(i = dof; i >= 1; i--) {
390  F[i] = a[i] * links[i].m;
391  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
392  if(i == dof) {
393  f[i] = F[i];
394  n[i] = CrossProduct(p[i],f[i])
395  + CrossProduct(links[i].r,F[i]) + N[i];
396  } else {
397  f[i] = links[i+1].R*f[i+1] + F[i];
398  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
399  + CrossProduct(links[i].r,F[i]) + N[i];
400  }
401  if(links[i].get_joint_type() == 0)
402  temp = ((z0.t()*links[i].R)*n[i]);
403  else
404  temp = ((z0.t()*links[i].R)*f[i]);
405  ltorque(i) = temp(1,1)
406  + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
407  }
408 
409  ltorque.Release(); return ltorque;
410 }
411 
413  const ColumnVector & qpp)
415 {
416  ColumnVector Fext(3), Next(3);
417  Fext = 0;
418  Next = 0;
419  return torque(q, qp, qpp, Fext, Next);
420 }
421 
423  const ColumnVector & qpp, const ColumnVector & Fext_,
424  const ColumnVector & Next_)
487 {
488  int i;
489  ColumnVector ltorque(dof);
490  Matrix Rt, temp;
491  if(q.Nrows() != dof) error("q has wrong dimension");
492  if(qp.Nrows() != dof) error("qp has wrong dimension");
493  if(qpp.Nrows() != dof) error("qpp has wrong dimension");
494  set_q(q);
495  set_qp(qp);
496 
497  vp[0] = gravity;
498  for(i = 1; i <= dof; i++) {
499  Rt = links[i].R.t();
500  if(links[i].get_joint_type() == 0) {
501  w[i] = Rt*w[i-1] + z0*qp(i);
502  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
503  + z0*qpp(i);
504  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
505  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
506  + vp[i-1]);
507  } else {
508  w[i] = Rt*w[i-1];
509  wp[i] = Rt*wp[i-1];
510  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
511  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
512  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
513  }
514  a[i] = CrossProduct(wp[i],links[i].r)
515  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
516  + vp[i];
517  }
518 
519  // Load on last link
520  ColumnVector Fext(3), Next(3);
521  if(fix) // Last link is fix
522  {
523  Fext = links[dof+fix].R*Fext_;
524  Next = links[dof+fix].R*Next_;
525  }
526  else
527  {
528  Fext = Fext_;
529  Next = Next_;
530  }
531 
532  for(i = dof; i >= 1; i--) {
533  F[i] = a[i] * links[i].m;
534  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
535  if(i == dof) {
536  f[i] = F[i] + Fext;
537  n[i] = CrossProduct(links[i].r,F[i]) + N[i] + Next;
538  } else {
539  f[i] = links[i+1].R*f[i+1] + F[i];
540  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
541  + CrossProduct(links[i].r,F[i]) + N[i];
542  }
543  if(links[i].get_joint_type() == 0)
544  temp = (z0.t()*n[i]);
545  else
546  temp = (z0.t()*f[i]);
547  ltorque(i) = temp(1,1)
548  + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
549  + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
550  }
551 
552  ltorque.Release(); return ltorque;
553 }
554 
557 {
558  int i;
559  ColumnVector ltorque(dof);
560  Matrix Rt, temp;
561  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
562 
563  vp[0] = 0.0;
564  for(i = 1; i <= dof; i++) {
565  Rt = links[i].R.t();
566  if(links[i].get_joint_type() == 0) {
567  wp[i] = Rt*wp[i-1] + z0*qpp(i);
568  vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
569  } else {
570  wp[i] = Rt*wp[i-1];
571  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
572  + z0*qpp(i);
573  }
574  a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
575  }
576 
577  for(i = dof; i >= 1; i--) {
578  F[i] = a[i] * links[i].m;
579  N[i] = links[i].I*wp[i];
580  if(i == dof) {
581  f_nv[i] = F[i];
582  n_nv[i] = CrossProduct(links[i].r,F[i]) + N[i];
583  } else {
584  f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
585  n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i+1],links[i+1].R*f_nv[i+1])
586  + CrossProduct(links[i].r,F[i]) + N[i];
587  }
588 
589  if(links[i].get_joint_type() == 0)
590  temp = (z0.t()*n_nv[i]);
591  else
592  temp = (z0.t()*f_nv[i]);
593  ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
594  }
595 
596  ltorque.Release(); return ltorque;
597 }
598 
601 {
602  int i;
603  ColumnVector ltorque(dof);
604  Matrix Rt, temp;
605 
606  vp[0] = gravity;
607  for(i = 1; i <= dof; i++) {
608  Rt = links[i].R.t();
609  if(links[i].get_joint_type() == 0)
610  vp[i] = Rt*vp[i-1];
611  else
612  vp[i] = Rt*vp[i-1];
613  a[i] = vp[i];
614  }
615 
616  for(i = dof; i >= 1; i--) {
617  F[i] = a[i] * links[i].m;
618  if(i == dof) {
619  f[i] = F[i];
620  n[i] = CrossProduct(links[i].r,F[i]);
621  } else {
622  f[i] = links[i+1].R*f[i+1] + F[i];
623  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
624  + CrossProduct(links[i].r,F[i]);
625  }
626  if(links[i].get_joint_type() == 0)
627  temp = (z0.t()*n[i]);
628  else
629  temp = (z0.t()*f[i]);
630  ltorque(i) = temp(1,1);
631  }
632 
633  ltorque.Release(); return ltorque;
634 }
635 
638 {
639  int i;
640  ColumnVector ltorque(dof);
641  Matrix Rt, temp;
642  if(qp.Nrows() != dof) error("qp has wrong dimension");
643 
644  vp[0] = 0;
645  for(i = 1; i <= dof; i++) {
646  Rt = links[i].R.t();
647  if(links[i].get_joint_type() == 0) {
648  w[i] = Rt*w[i-1] + z0*qp(i);
649  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i));
650  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
651  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
652  + vp[i-1]);
653  } else {
654  w[i] = Rt*w[i-1];
655  wp[i] = Rt*wp[i-1];
656  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
657  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
658  + 2.0*CrossProduct(w[i],z0*qp(i));
659  }
660  a[i] = CrossProduct(wp[i],links[i].r)
661  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
662  + vp[i];
663  }
664 
665  for(i = dof; i >= 1; i--) {
666  F[i] = a[i] * links[i].m;
667  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
668  if(i == dof) {
669  f[i] = F[i];
670  n[i] = CrossProduct(links[i].r,F[i]) + N[i];
671  } else {
672  f[i] = links[i+1].R*f[i+1] + F[i];
673  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
674  + CrossProduct(links[i].r,F[i]) + N[i];
675  }
676  if(links[i].get_joint_type() == 0)
677  temp = (z0.t()*n[i]);
678  else
679  temp = (z0.t()*f[i]);
680  ltorque(i) = temp(1,1)
681  + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
682  }
683 
684  ltorque.Release(); return ltorque;
685 }
686 
688  const ColumnVector & qpp)
690 {
691  ColumnVector Fext(3), Next(3);
692  Fext = 0;
693  Next = 0;
694  return torque(q, qp, qpp, Fext, Next);
695 }
696 
698  const ColumnVector & qpp, const ColumnVector & Fext_,
699  const ColumnVector & Next_)
708 {
709  int i;
710  ColumnVector ltorque(dof);
711  Matrix Rt, temp;
712  if(q.Nrows() != dof) error("q has wrong dimension");
713  if(qp.Nrows() != dof) error("qp has wrong dimension");
714  if(qpp.Nrows() != dof) error("qpp has wrong dimension");
715  set_q(q);
716  set_qp(qp);
717 
718  vp[0] = gravity;
719  for(i = 1; i <= dof; i++) {
720  Rt = links[i].R.t();
721  if(links[i].get_joint_type() == 0)
722  {
723  w[i] = Rt*w[i-1] + z0*qp(i);
724  wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)))
725  + z0*qpp(i);
726  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
727  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
728  + vp[i-1]);
729  }
730  else
731  {
732  w[i] = Rt*w[i-1];
733  wp[i] = Rt*wp[i-1];
734  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
735  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
736  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
737  }
738  }
739 
740  ColumnVector Fext(3), Next(3);
741  if(fix)
742  {
743  Fext = links[dof+fix].R*Fext_;
744  Next = links[dof+fix].R*Next_;
745  }
746  else
747  {
748  Fext = Fext_;
749  Next = Next_;
750  }
751 
752  for(i = dof; i >= 1; i--)
753  {
754  F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
755  CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
756  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
757  CrossProduct(-vp[i], links[i].mc);
758  if(i == dof)
759  {
760  f[i] = F[i] + Fext;
761  n[i] = N[i] + Next;
762  }
763  else
764  {
765  f[i] = links[i+1].R*f[i+1] + F[i];
766  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
767  }
768 
769  if(links[i].get_joint_type() == 0)
770  temp = (z0.t()*n[i]);
771  else
772  temp = (z0.t()*f[i]);
773  ltorque(i) = temp(1,1)
774  + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
775  + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
776  }
777 
778  ltorque.Release(); return ltorque;
779 }
780 
781 
784 {
785  int i;
786  ColumnVector ltorque(dof);
787  Matrix Rt, temp;
788  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
789 
790  vp[0] = 0.0;
791  for(i = 1; i <= dof; i++)
792  {
793  Rt = links[i].R.t();
794  if(links[i].get_joint_type() == 0)
795  {
796  wp[i] = Rt*wp[i-1] + z0*qpp(i);
797  vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
798  }
799  else
800  {
801  wp[i] = Rt*wp[i-1];
802  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
803  + z0*qpp(i);
804  }
805  }
806 
807  for(i = dof; i >= 1; i--)
808  {
809  F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc);
810  N[i] = links[i].I*wp[i] + CrossProduct(-vp[i], links[i].mc);
811  if(i == dof)
812  {
813  f_nv[i] = F[i];
814  n_nv[i] = N[i];
815  }
816  else
817  {
818  f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
819  n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i+1],links[i+1].R*f_nv[i+1]) + N[i];
820  }
821 
822  if(links[i].get_joint_type() == 0)
823  temp = (z0.t()*n_nv[i]);
824  else
825  temp = (z0.t()*f_nv[i]);
826  ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
827  }
828 
829  ltorque.Release(); return ltorque;
830 }
831 
834 {
835  int i;
836  ColumnVector ltorque(dof);
837  Matrix Rt, temp;
838 
839  vp[0] = gravity;
840  for(i = 1; i <= dof; i++) {
841  Rt = links[i].R.t();
842  if(links[i].get_joint_type() == 0)
843  vp[i] = Rt*vp[i-1];
844  else
845  vp[i] = Rt*vp[i-1];
846  }
847 
848  for(i = dof; i >= 1; i--)
849  {
850  F[i] = vp[i]*links[i].m;
851  N[i] = CrossProduct(-vp[i], links[i].mc);
852  if(i == dof)
853  {
854  f[i] = F[i];
855  n[i] = N[i];
856  }
857  else
858  {
859  f[i] = links[i+1].R*f[i+1] + F[i];
860  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
861  }
862 
863  if(links[i].get_joint_type() == 0)
864  temp = (z0.t()*n[i]);
865  else
866  temp = (z0.t()*f[i]);
867  ltorque(i) = temp(1,1);
868  }
869 
870  ltorque.Release(); return ltorque;
871 }
872 
875 {
876  int i;
877  ColumnVector ltorque(dof);
878  Matrix Rt, temp;
879  if(qp.Nrows() != dof) error("qp has wrong dimension");
880  set_qp(qp);
881 
882  vp[0] = 0;
883  for(i = 1; i <= dof; i++) {
884  Rt = links[i].R.t();
885  if(links[i].get_joint_type() == 0)
886  {
887  w[i] = Rt*w[i-1] + z0*qp(i);
888  wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
889  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
890  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
891  + vp[i-1]);
892  }
893  else
894  {
895  w[i] = Rt*w[i-1];
896  wp[i] = Rt*wp[i-1];
897  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
898  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
899  + 2.0*CrossProduct(w[i],z0*qp(i));
900  }
901  }
902 
903  for(i = dof; i >= 1; i--)
904  {
905  F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
906  CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
907  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
908  CrossProduct(-vp[i], links[i].mc);
909  if(i == dof)
910  {
911  f[i] = F[i];
912  n[i] = N[i];
913  }
914  else
915  {
916  f[i] = links[i+1].R*f[i+1] + F[i];
917  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
918  }
919 
920  if(links[i].get_joint_type() == 0)
921  temp = (z0.t()*n[i]);
922  else
923  temp = (z0.t()*f[i]);
924  ltorque(i) = temp(1,1)
925  + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
926  }
927 
928  ltorque.Release(); return ltorque;
929 }
930 
931 #ifdef use_namespace
932 }
933 #endif
934 
void Release()
Definition: newmat.h:514
virtual ReturnMatrix torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
Joint torque, without contact force, based on Recursive Newton-Euler formulation. ...
Definition: dynamics.cpp:135
Robots class definitions.
virtual ReturnMatrix G()
Joint torque due to gravity based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:599
static const char rcsid[]
RCS/CVS version.
Definition: dynamics.cpp:73
virtual ReturnMatrix torque_novelocity(const ColumnVector &qpp)
Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:555
int Nrows() const
Definition: newmat.h:494
virtual ReturnMatrix C(const ColumnVector &qp)
Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:636
ReturnMatrix inertia(const ColumnVector &q)
Inertia of the manipulator.
Definition: dynamics.cpp:83
ReturnMatrix acceleration(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau)
Joints acceleration without contact force.
Definition: dynamics.cpp:100
virtual ReturnMatrix torque_novelocity(const ColumnVector &qpp)
Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:272
GetSubMatrix Column(int f) const
Definition: newmat.h:2152
virtual ReturnMatrix C(const ColumnVector &qp)
Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:873
virtual ReturnMatrix torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
Joint torque, without contact force, based on Recursive Newton-Euler formulation. ...
Definition: dynamics.cpp:412
TransposedMatrix t() const
Definition: newmat6.cpp:320
Matrix CrossProduct(const Matrix &A, const Matrix &B)
Definition: newmat.h:2062
The usual rectangular matrix.
Definition: newmat.h:625
InvertedMatrix i() const
Definition: newmat6.cpp:329
virtual ReturnMatrix G()
Joint torque due to gravity based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:832
FloatVector FloatVector * a
int Ncols() const
Definition: newmat.h:495
virtual ReturnMatrix torque_novelocity(const ColumnVector &qpp)
Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:782
virtual ReturnMatrix C(const ColumnVector &qp)
Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:360
Column vector.
Definition: newmat.h:1008
virtual ReturnMatrix torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
Joint torque without contact force based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:687
virtual ReturnMatrix G()
Joint torque due to gravity based on Recursive Newton-Euler formulation.
Definition: dynamics.cpp:321


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