ROBOOP, A Robotics Object Oriented Package in C++
robot.h
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  -Removed class mlink. DH and modified DH parameters are now included in link.
34  -Created virtual class Robot_basic which is now the base class of Robot and
35  mRobot.
36  -Removed classes RobotMotor and mRobotMotor. Motors effect are now included
37  in classes Robot and mRobot. Code using the old motor class whould need to
38  be change by changing RobotMotor by Robot and mRobotMotor by mRobot.
39  -Added classes mRobot_min_para (modified DH parameters) and IO_matrix_file.
40  -Created a new torque member function that allowed to have load on last link
41  (Robot_basic, Robot, mRobot, mRobot_min_para).
42  -Added the following member functions in class Robot_basic:
43  void kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & posdot)const;
44  void kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & posdot, int j)const;
45  ReturnMatrix kine_pd(void)const;
46  ReturnMatrix kine_pd(int j)const;
47  These functions are like the kine(), but with speed calculation.
48  -Added labels theta_min and theta_max in class Link.
49 
50 2003/04/29: Etienne Lachance
51  -All gnugrah.cpp definitions are now in gnugraph.h.
52  -The following ColumnVector are now part of class Robot_basic: *dw, *dwp, *dvp, *da,
53  *df, *dn, *dF, *dN, *dp.
54  -Added functons Robot_basic::jacobian_DLS_inv.
55  -Added z0 in Robot_basic.
56  -Fix kine_pd function.
57 
58 2003/08/22: Etienne Lachance
59  -Added parameter converge in member function inv_kin.
60 
61 2004/01/23: Etienne Lachance
62  -Added member function G() (gravity torque), and H() (corriolis torque)
63  on all robot class.
64  -Commun definitions are now in include file utils.h.
65  -Added const in non reference argument for all functions prototypes.
66 
67 2004/03/01: Etienne Lachance
68  -Added non member function perturb_robot.
69 
70 2004/03/21: Etienne Lachance
71  -Added the following member functions: get_theta_min, get_theta_max,
72  get_mc, get_r, get_p, get_m, get_Im, get_Gr, get_B, get_Cf, get_I,
73  set_m, set_mc, set_r, set_Im, set_B, set_Cf, set_I.
74 
75 2004/04/26: Etienne Lachance and Vincent Drolet
76  -Added member functions inv_kin_rhino and inv_kin_puma.
77 
78 2004/05/21: Etienne Lachance
79  -Added Doxygen comments.
80 
81 2004/07/01: Ethan Tira-Thompson
82  -Added support for newmat's use_namespace #define, using ROBOOP namespace
83 
84 2004/07/02: Etienne Lachance
85  -Added joint_offset variable in class Link and Link member functions
86  get_joint_offset. Idea proposed by Ethan Tira-Thompson.
87 
88 2004/07/16: Ethan Tira-Thompson
89  -Added Link::immobile flag and accessor functions
90  -Added get_available_q* functions
91  -inv_kin functions are now all virtual
92  -Added parameters to jacobian and inv_kin functions to work with frames
93  other than the end effector
94 
95 2004/10/02: Etienne Lachance
96  -Added Schlling for analytic inverse kinematic.
97 
98 2005/11/06: Etienne Lachance
99  - No need to provide a copy constructor and the assignment operator
100  (operator=) for Link class. Instead we use the one provide by the
101  compiler.
102  -No need to provide an assignment operator for Robot, mRobot and
103  mRobot_min_para classes.
104 -------------------------------------------------------------------------------
105 */
106 
107 #ifndef __cplusplus
108 #error Must use C++ for the type Robot
109 #endif
110 #ifndef ROBOT_H
111 #define ROBOT_H
112 
118 #include "utils.h"
119 
120 #ifdef use_namespace
121 namespace ROBOOP {
122  using namespace NEWMAT;
123 #endif
124 
125 
133 class Link
134 {
135  friend class Robot_basic;
136  friend class Robot;
137  friend class mRobot;
138  friend class mRobot_min_para;
139 
140 public:
141  Link(const int jt = 0, const Real it = 0.0, const Real id = 0.0,
142  const Real ia = 0.0, const Real ial = 0.0, const Real theta_min = -M_PI/2,
143  const Real theta_max = M_PI/2, const Real it_off = 0.0, const Real mass = 1.0,
144  const Real cmx = 0.0, const Real cmy = 0.0, const Real cmz = 0.0,
145  const Real ixx = 0.0, const Real ixy = 0.0, const Real ixz = 0.0,
146  const Real iyy = 0.0, const Real iyz = 0.0, const Real izz = 0.0,
147  const Real iIm = 0.0, const Real iGr = 0.0, const Real iB = 0.0,
148  const Real iCf = 0.0, const bool dh = true, const bool min_inertial_para = false,
149  const bool immobile = false);
150  ~Link(){}
151  void transform(const Real q);
152  bool get_DH(void) const {return DH; }
153  int get_joint_type(void) const { return joint_type; }
154  Real get_theta(void) const { return theta; }
155  Real get_d(void) const { return d; }
156  Real get_a(void) const { return a; }
157  Real get_alpha(void) const { return alpha; }
158  Real get_q(void) const;
159  Real get_theta_min(void) const { return theta_min; }
160  Real get_theta_max(void) const { return theta_max; }
161  Real get_joint_offset(void) const { return joint_offset; }
162  ReturnMatrix get_mc(void) { return mc; }
163  ReturnMatrix get_r(void) { return r; }
164  ReturnMatrix get_p(void) const { return p; }
165  Real get_m(void) const { return m; }
166  Real get_Im(void) const { return Im; }
167  Real get_Gr(void) const { return Gr; }
168  Real get_B(void) const { return B; }
169  Real get_Cf(void) const { return Cf; }
170  ReturnMatrix get_I(void) const { return I; }
171  bool get_immobile(void) const { return immobile; }
172  void set_m(const Real m_) { m = m_; }
173  void set_mc(const ColumnVector & mc_);
174  void set_r(const ColumnVector & r_);
175  void set_Im(const Real Im_) { Im = Im_; }
176  void set_B(const Real B_) { B = B_; }
177  void set_Cf(const Real Cf_) { Cf = Cf_; }
178  void set_I(const Matrix & I);
179  void set_immobile(bool im) { immobile=im; }
180 
181  Matrix R;
182  Real qp,
183  qpp;
184 
185 private:
186  int joint_type;
187  Real theta,
188  d,
189  a,
190  alpha,
191  theta_min,
192  theta_max,
193  joint_offset;
194  bool DH,
195  min_para;
196  ColumnVector r,
197  p;
198  Real m,
199  Im,
200  Gr,
201  B,
202  Cf;
203  ColumnVector mc;
204  Matrix I;
205  bool immobile;
206 };
207 
212 class Robot_basic {
213  friend class Robot;
214  friend class mRobot;
215  friend class mRobot_min_para;
216  friend class Robotgl;
217  friend class mRobotgl;
218 public:
219  Robot_basic(const int ndof = 1, const bool dh_parameter = false,
220  const bool min_inertial_para = false);
221  Robot_basic(const Matrix & initrobot_motor, const bool dh_parameter = false,
222  const bool min_inertial_para = false);
223  Robot_basic(const Matrix & initrobot, const Matrix & initmotor,
224  const bool dh_parameter = false, const bool min_inertial_para = false);
225  Robot_basic(const std::string & filename, const std::string & robotName,
226  const bool dh_parameter = false, const bool min_inertial_para = false);
227  Robot_basic(const Robot_basic & x);
228  virtual ~Robot_basic();
229  Robot_basic & operator=(const Robot_basic & x);
230 
231  Real get_q(const int i) const {
232  if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
233  return links[i].get_q();
234  }
235  bool get_DH()const { return links[1].DH; }
236  int get_dof()const { return dof; }
237  int get_available_dof()const { return get_available_dof(dof); }
238  int get_available_dof(const int endlink)const;
239  int get_fix()const { return fix; }
240  ReturnMatrix get_q(void)const;
241  ReturnMatrix get_qp(void)const;
242  ReturnMatrix get_qpp(void)const;
243  ReturnMatrix get_available_q(void)const { return get_available_q(dof); }
244  ReturnMatrix get_available_qp(void)const { return get_available_qp(dof); }
245  ReturnMatrix get_available_qpp(void)const { return get_available_qpp(dof); }
246  ReturnMatrix get_available_q(const int endlink)const;
247  ReturnMatrix get_available_qp(const int endlink)const;
248  ReturnMatrix get_available_qpp(const int endlink)const;
249  void set_q(const ColumnVector & q);
250  void set_q(const Matrix & q);
251  void set_q(const Real q, const int i) {
252  if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
253  links[i].transform(q);
254  }
255  void set_qp(const ColumnVector & qp);
256  void set_qpp(const ColumnVector & qpp);
257  void kine(Matrix & Rot, ColumnVector & pos)const;
258  void kine(Matrix & Rot, ColumnVector & pos, const int j)const;
259  ReturnMatrix kine(void)const;
260  ReturnMatrix kine(const int j)const;
261  ReturnMatrix kine_pd(const int ref=0)const;
262  virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
263  ColumnVector & pos_dot, const int ref)const=0;
264  virtual void robotType_inv_kin() = 0;
265  virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
267  ReturnMatrix inv_kin(const Matrix & Tobj, const int mj,
268  bool & converge) {return inv_kin(Tobj,mj,dof,converge);}
269  virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
270  virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge) = 0;
271  virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge) = 0;
272  virtual ReturnMatrix inv_kin_schilling(const Matrix & Tobj, bool & converge) = 0;
273  virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); }
274  virtual ReturnMatrix jacobian(const int endlink, const int ref)const = 0;
275  virtual ReturnMatrix jacobian_dot(const int ref=0)const = 0;
276  ReturnMatrix jacobian_DLS_inv(const double eps, const double lambda_max, const int ref=0)const;
277  virtual void dTdqi(Matrix & dRot, ColumnVector & dp, const int i) = 0;
278  virtual ReturnMatrix dTdqi(const int i) = 0;
279  ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
280  const ColumnVector & tau);
281  ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
282  const ColumnVector & tau, const ColumnVector & Fext,
283  const ColumnVector & Next);
284  ReturnMatrix inertia(const ColumnVector & q);
285  virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp) = 0;
286  virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
287  const ColumnVector & qpp) = 0;
288  virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
289  const ColumnVector & qpp,
290  const ColumnVector & Fext_,
291  const ColumnVector & Next_) = 0;
292  virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
293  const ColumnVector & qpp, const ColumnVector & dq,
294  const ColumnVector & dqp, const ColumnVector & dqpp,
295  ColumnVector & torque, ColumnVector & dtorque) =0;
296  virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
297  const ColumnVector & qpp, const ColumnVector & dq,
298  ColumnVector & torque, ColumnVector & dtorque) =0;
299  virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
300  const ColumnVector & dqp, ColumnVector & torque,
301  ColumnVector & dtorque) =0;
302  ReturnMatrix dtau_dq(const ColumnVector & q, const ColumnVector & qp,
303  const ColumnVector & qpp);
304  ReturnMatrix dtau_dqp(const ColumnVector & q, const ColumnVector & qp);
305  virtual ReturnMatrix G() = 0;
306  virtual ReturnMatrix C(const ColumnVector & qp) = 0;
307  void error(const std::string & msg1) const;
308 
309  ColumnVector *w, *wp, *vp, *a, *f, *f_nv, *n, *n_nv, *F, *N, *p, *pp,
310  *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp,
311  z0,
312  gravity;
313  Matrix *R;
314  Link *links;
315 
316 private:
317  void cleanUpPointers();
318 
320  enum EnumRobotType
321  {
322  DEFAULT = 0,
323  RHINO = 1,
324  PUMA = 2,
325  SCHILLING = 3
326  };
327  EnumRobotType robotType;
328  int dof,
329  fix;
330 };
331 
336 class Robot : public Robot_basic
337 {
338 public:
339  Robot(const int ndof=1);
340  Robot(const Matrix & initrobot);
341  Robot(const Matrix & initrobot, const Matrix & initmotor);
342  Robot(const Robot & x);
343  Robot(const std::string & filename, const std::string & robotName);
344  virtual ~Robot(){}
345  virtual void robotType_inv_kin();
346  virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
347  ColumnVector & pos_dot, const int ref)const;
348  ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
349  virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
350  virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
351  virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
352  virtual ReturnMatrix inv_kin_schilling(const Matrix & Tobj, bool & converge);
353  virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); }
354  virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
355  virtual ReturnMatrix jacobian_dot(const int ref=0)const;
356  virtual void dTdqi(Matrix & dRot, ColumnVector & dp, const int i);
357  virtual ReturnMatrix dTdqi(const int i);
358  virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
359  const ColumnVector & qpp);
360  virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
361  const ColumnVector & qpp,
362  const ColumnVector & Fext_,
363  const ColumnVector & Next_);
364  virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
365  virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
366  const ColumnVector & qpp, const ColumnVector & dq,
367  const ColumnVector & dqp, const ColumnVector & dqpp,
368  ColumnVector & ltorque, ColumnVector & dtorque);
369  virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
370  const ColumnVector & qpp, const ColumnVector & dq,
371  ColumnVector & torque, ColumnVector & dtorque);
372  virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
373  const ColumnVector & dqp, ColumnVector & torque,
374  ColumnVector & dtorque);
375  virtual ReturnMatrix G();
376  virtual ReturnMatrix C(const ColumnVector & qp);
377 };
378 
379 // ---------- R O B O T M O D I F I E D DH N O T A T I O N --------------
380 
385 class mRobot : public Robot_basic {
386 public:
387  mRobot(const int ndof=1);
388  mRobot(const Matrix & initrobot_motor);
389  mRobot(const Matrix & initrobot, const Matrix & initmotor);
390  mRobot(const mRobot & x);
391  mRobot(const std::string & filename, const std::string & robotName);
392  virtual ~mRobot(){}
393  virtual void robotType_inv_kin();
394  ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
395  virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
396  virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
397  virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
398  virtual ReturnMatrix inv_kin_schilling(const Matrix & Tobj, bool & converge);
399  virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
400  ColumnVector & pos_dot, const int ref)const;
401  virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); }
402  virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
403  virtual ReturnMatrix jacobian_dot(const int ref=0)const;
404  virtual void dTdqi(Matrix & dRot, ColumnVector & dp, const int i);
405  virtual ReturnMatrix dTdqi(const int i);
406  virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
407  const ColumnVector & qpp);
408  virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
409  const ColumnVector & qpp,
410  const ColumnVector & Fext_,
411  const ColumnVector & Next_);
412  virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
413  virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
414  const ColumnVector & qpp, const ColumnVector & dq,
415  const ColumnVector & dqp, const ColumnVector & dqpp,
416  ColumnVector & torque, ColumnVector & dtorque);
417  virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
418  const ColumnVector & qpp, const ColumnVector & dq,
419  ColumnVector & torque, ColumnVector & dtorque);
420  virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
421  const ColumnVector & dqp, ColumnVector & torque,
422  ColumnVector & dtorque);
423  virtual ReturnMatrix G();
424  virtual ReturnMatrix C(const ColumnVector & qp);
425 };
426 
427 // --- R O B O T DH M O D I F I E D, M I N I M U M P A R A M E T E R S ---
428 
433 class mRobot_min_para : public Robot_basic
434 {
435 public:
436  mRobot_min_para(const int ndof=1);
437  mRobot_min_para(const Matrix & dhinit);
438  mRobot_min_para(const Matrix & initrobot, const Matrix & initmotor);
439  mRobot_min_para(const mRobot_min_para & x);
440  mRobot_min_para(const std::string & filename, const std::string & robotName);
441  virtual ~mRobot_min_para(){}
442  virtual void robotType_inv_kin();
443  ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
444  virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
445  virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
446  virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
447  virtual ReturnMatrix inv_kin_schilling(const Matrix & Tobj, bool & converge);
448  virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
449  ColumnVector & pos_dot, const int ref=0)const;
450  virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); }
451  virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
452  virtual ReturnMatrix jacobian_dot(const int ref=0)const;
453  virtual void dTdqi(Matrix & dRot, ColumnVector & dp, const int i);
454  virtual ReturnMatrix dTdqi(const int i);
455  virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
456  const ColumnVector & qpp);
457  virtual ReturnMatrix torque(const ColumnVector & q,
458  const ColumnVector & qp,
459  const ColumnVector & qpp,
460  const ColumnVector & Fext_,
461  const ColumnVector & Next_);
462  virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
463  virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
464  const ColumnVector & qpp, const ColumnVector & dq,
465  const ColumnVector & dqp, const ColumnVector & dqpp,
466  ColumnVector & torque, ColumnVector & dtorque);
467  virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
468  const ColumnVector & qpp, const ColumnVector & dq,
469  ColumnVector & torque, ColumnVector & dtorque);
470  virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
471  const ColumnVector & dqp, ColumnVector & torque,
472  ColumnVector & dtorque);
473  virtual ReturnMatrix G();
474  virtual ReturnMatrix C(const ColumnVector & qp);
475 };
476 
477 void perturb_robot(Robot_basic & robot, const double f = 0.1);
478 
479 bool Rhino_DH(const Robot_basic & robot);
480 bool Puma_DH(const Robot_basic & robot);
481 bool Schilling_DH(const Robot_basic & robot);
482 
483 bool Rhino_mDH(const Robot_basic & robot);
484 bool Puma_mDH(const Robot_basic & robot);
485 bool Schilling_mDH(const Robot_basic & robot);
486 
487 #ifdef use_namespace
488 }
489 #endif
490 
491 #endif
492