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