controller.cpp
Go to the documentation of this file.
1 /*
2 Copyright (C) 2002-2004 Etienne Lachance
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
22 
23 -------------------------------------------------------------------------------
24 Revision_history:
25 
26 2005/06/10: Etienne Lachance
27  -The desired joint acceleration was missing in the computed torque method.
28 
29 2005/11/06: Etienne Lachance
30  - No need to provide a copy constructor and the assignment operator
31  (operator=) for Impedance, Resolved_acc, Computed_torque_method and
32  Proportional_Derivative classes. Instead we use the one provide by the
33  compiler.
34 -------------------------------------------------------------------------------
35 */
36 
42 static const char rcsid[] = "$Id: controller.cpp,v 1.3 2005/11/15 19:06:13 gourdeau Exp $";
44 
45 
46 #include "controller.h"
47 
48 #ifdef use_namespace
49 namespace ROBOOP {
50  using namespace NEWMAT;
51 #endif
52 
53 
56 {
57  pc = ColumnVector(3); pc = 0;
58  pcp = pc;
59  pcpp = pc;
60  pcpp_prev = pc;
61  qc = Quaternion();
62  qcp = qc;
63  qcp_prev = qc;
64  quat = qc;
65  wc = pc;
66  wcp = pc;
67 }
68 
70  const DiagonalMatrix & Dp_, const DiagonalMatrix & Kp_,
71  const DiagonalMatrix & Mo_, const DiagonalMatrix & Do_,
72  const DiagonalMatrix & Ko_)
74 {
75  set_Mp(Mp_);
76  set_Dp(Dp_);
77  set_Kp(Kp_);
78  set_Mo(Mo_);
79  set_Do(Do_);
80  set_Ko(Ko_);
81 
82  pc = ColumnVector(3); pc = 0;
83  pcp = pc;
84  pcp_prev = pc;
85  pcpp = pc;
86  pcpp_prev = pc;
87  qc = Quaternion();
88  qcp = qc;
89  qcp_prev = qc;
90  quat = qc;
91  wc = pc;
92  wcp = pc;
93  wcp_prev = pc;
94 
95  Matrix Rot;
96  robot.kine(Rot, pc);
97  qc = Quaternion(Rot);
98 }
99 
105 {
106  if(Mp_.Nrows() != 3)
107  {
108  Mp = DiagonalMatrix(3); Mp = 1;
109  cerr << "Impedance::set_Mp: wrong size for input matrix Mp" << endl;
110  return WRONG_SIZE;
111  }
112 
113  Mp = Mp_;
114  return 0;
115 }
116 
117 short Impedance::set_Mp(const Real Mp_i, const short i)
122 {
123  if( (i < 0) || (i > 3) )
124  {
125  cerr << "Impedance::set_Mp: index i out of bound" << endl;
126  return WRONG_SIZE;
127  }
128 
129  Mp(i) = Mp_i;
130  return 0;
131 }
132 
138 {
139  if(Dp_.Nrows() != 3)
140  {
141  Dp = DiagonalMatrix(3); Dp = 1;
142  cerr << "Impedance::set_Dp: input matrix Dp of wrong size" << endl;
143  return WRONG_SIZE;
144  }
145 
146  Dp = Dp_;
147  return 0;
148 }
149 
150 short Impedance::set_Dp(const Real Dp_i, const short i)
155 {
156  if( (i < 0) || (i > 3) )
157  {
158  cerr << "Impedance::set_Dp: index i out of bound" << endl;
159  return WRONG_SIZE;
160  }
161 
162  Dp(i) = Dp_i;
163  return 0;
164 }
165 
171 {
172  if(Kp_.Nrows() != 3)
173  {
174  Kp = DiagonalMatrix(3); Kp = 1;
175  cerr << "Impedance::set_Kp: wrong size for input matrix Kp" << endl;
176  return WRONG_SIZE;
177  }
178 
179  Kp = Kp_;
180  return 0;
181 }
182 
183 short Impedance::set_Kp(const Real Kp_i, const short i)
188 {
189  if( (i < 0) || (i > 3) )
190  {
191  cerr << "Impedance::set_Mp: index i out of bound" << endl;
192  return WRONG_SIZE;
193  }
194 
195  Kp(i) = Kp_i;
196  return 0;
197 }
198 
204 {
205  if(Mo_.Nrows() != 3)
206  {
207  Mo = DiagonalMatrix(3); Mo = 1;
208  cerr << "Impedance::set_Mo: wrong size input matrix Mo" << endl;
209  return WRONG_SIZE;
210  }
211 
212  Mo = Mo_;
213  return 0;
214 }
215 
216 short Impedance::set_Mo(const Real Mo_i, const short i)
221 {
222  if( (i < 0) || (i > 3) )
223  {
224  cerr << "Impedance::set_Mo: index i out of bound" << endl;
225  return WRONG_SIZE;
226  }
227 
228  Mo(i) = Mo_i;
229  return 0;
230 }
231 
237 {
238  if( Do_.Nrows() != 3)
239  {
240  Do = DiagonalMatrix(3); Do = 1;
241  cerr << "Impedance::set_Do: wrong size input matrix Do" << endl;
242  return WRONG_SIZE;
243  }
244 
245  Do = Do_;
246  return 0;
247 }
248 
249 short Impedance::set_Do(const Real Do_i, const short i)
254 {
255  if( (i < 0) || (i > 3) )
256  {
257  cerr << "Impedance::set_Do: index i out of bound" << endl;
258  return WRONG_SIZE;
259  }
260 
261  Do(i) = Do_i;
262  return 0;
263 }
264 
270 {
271  if(Ko_.Nrows() != 3)
272  {
273  Ko = DiagonalMatrix(3); Ko = 1;
274  cerr << "Impedance::set_Ko: wrong size for input matrix Ko" << endl;
275  return WRONG_SIZE;
276  }
277 
278  Ko = Ko_;
279  return 0;
280 }
281 
282 short Impedance::set_Ko(const Real Ko_i, const short i)
287 {
288  if( (i < 0) || (i > 3) )
289  {
290  cerr << "Impedance::set_Ko: index i out of bound" << endl;
291  return WRONG_SIZE;
292  }
293 
294  Ko(i) = Ko_i;
295  return 0;
296 }
297 
298 short Impedance::control(const ColumnVector & pdpp, const ColumnVector & pdp,
299  const ColumnVector & pd, const ColumnVector & wdp,
300  const ColumnVector & wd, const Quaternion & qd,
301  const ColumnVector & f, const ColumnVector & n,
302  const Real dt)
328 {
329  if(pdpp.Nrows() !=3)
330  {
331  cerr << "Impedance::control: wrong size for input vector pdpp" << endl;
332  return WRONG_SIZE;
333  }
334  if(pdp.Nrows() !=3)
335  {
336  cerr << "Impedance::control: wrong size for input vector pdp" << endl;
337  return WRONG_SIZE;
338  }
339  if(pd.Nrows() != 3)
340  {
341  cerr << "Impedance::control: wrong size for input vector pd" << endl;
342  return WRONG_SIZE;
343  }
344  if(wdp.Nrows() !=3)
345  {
346  cerr << "Impedance::control: wrong size for input vector wdp" << endl;
347  return WRONG_SIZE;
348  }
349  if(wd.Nrows() !=3)
350  {
351  cerr << "Impedance::control: wrong size for input vector wd" << endl;
352  return WRONG_SIZE;
353  }
354  if(f.Nrows() !=3)
355  {
356  cerr << "Impedance::control: wrong size for input vector f" << endl;
357  return WRONG_SIZE;
358  }
359  if(n.Nrows() !=3)
360  {
361  cerr << "Impedance::control: wrong size for input vector f" << endl;
362  return WRONG_SIZE;
363  }
364 
365  static bool first=true;
366  if(first)
367  {
368  qc = qd;
369  qcp = qc.dot(wc, BASE_FRAME);
370  qcp_prev = qcp;
371  wc = wd;
372  wcp = wdp;
373  first = false;
374  }
375  if(qc.dot_prod(qd) < 0)
376  quat = qd*(-1);
377  else
378  quat = qd;
379 
380  qcd = quat * qc.i();
381 
382  // Solving pcpp, pcp, pc with the translational impedance
383  pcd = pc - pd;
384  pcdp = pcp - pdp;
385  // pcpp = pdpp + Mp.i()*(f - Dp*pcdp - Kp*pcd - 2*qcd.s()*Km.t()*qcd.v()); // (21)
386  pcpp = pdpp + Mp.i()*(f - Dp*pcdp - Kp*pcd);
387  pcp = pcp_prev + Integ_Trap(pcpp, pcpp_prev, dt);
388  pc = pc + Integ_Trap(pcp, pcp_prev, dt);
389 
390  // Solving wcp, wc, qc with the rotational impedance
391  wcd = wc - wd;
392  Ko_prime = 2*qcd.E(BASE_FRAME)*Ko; //(23)
393  // Km_prime = (qcd.s()*qcd.E(BASE_FRAME) - qcd.v()*qcd.v().t())*Km; // (24)
394  // wcp = wdp + Mo.i()*(n - Do*wcd - Ko_prime*qcd.v() - Km_prime*pcd); // (22)
395  wcp = wdp + Mo.i()*(n - Do*wcd - Ko_prime*qcd.v()); // (22)
396  wc = wc + Integ_Trap(wcp, wcp_prev, dt);
397  qcp = qc.dot(wc, BASE_FRAME);
398  Integ_quat(qcp, qcp_prev, qc, dt);
399 
400  return 0;
401 }
402 
403 // ------------------------------------------------------------------------------------------------------
404 // Position control based on resolved acceleration.
405 // ------------------------------------------------------------------------------------------------------
406 
409 {
410  Kvp = Kpp = Kvo = Kpo = 0;
411  zero3 = ColumnVector(3); zero3 = 0;
412  p = zero3;
413  pp = zero3;
414 
415  if(dof>0)
416  {
417  qp = ColumnVector(dof); qp = 0;
418  qpp = qp;
419  }
420 
421  quat_v_error = zero3;
422  a = ColumnVector(6); a = 0;
423  quat = Quaternion();
424 }
425 
427  const Real Kvp_,
428  const Real Kpp_,
429  const Real Kvo_,
430  const Real Kpo_)
432 {
433  set_Kvp(Kvp_);
434  set_Kpp(Kpp_);
435  set_Kvo(Kvo_);
436  set_Kpo(Kpo_);
437  zero3 = ColumnVector(3); zero3 = 0;
438  qp = ColumnVector(robot.get_dof()); qp = 0;
439  qpp = qp;
440  a = ColumnVector(6); a = 0;
441  p = zero3;
442  pp = zero3;
443  quat_v_error = zero3;
444  quat = Quaternion();
445 }
446 
447 void Resolved_acc::set_Kvp(const Real Kvp_)
449 {
450  Kvp = Kvp_;
451 }
452 
453 void Resolved_acc::set_Kpp(const Real Kpp_)
455 {
456  Kpp = Kpp_;
457 }
458 
459 void Resolved_acc::set_Kvo(const Real Kvo_)
461 {
462  Kvo = Kvo_;
463 }
464 
465 void Resolved_acc::set_Kpo(const Real Kpo_)
467 {
468  Kpo = Kpo_;
469 }
470 
472  const ColumnVector & pdp, const ColumnVector & pd,
473  const ColumnVector & wdp, const ColumnVector & wd,
474  const Quaternion & quatd, const short link_pc,
475  const Real dt)
487 {
488  robot.kine_pd(Rot, p, pp, link_pc);
489 
490  Quaternion quate(Rot); // end effector orientation
491  if(quate.dot_prod(quatd) < 0)
492  quat = quatd*(-1);
493  else
494  quat = quatd;
495 
496  quat_v_error = quate.s()*quat.v() - quat.s()*quate.v() +
497  x_prod_matrix(quate.v())*quat.v();
498 
499  a.SubMatrix(1,3,1,1) = pdpp + Kvp*(pdp-pp) + Kpp*(pd-p);
500  a.SubMatrix(4,6,1,1) = wdp + Kvo*(wd-Rot*robot.w[robot.get_dof()]) +
501  Kpo*quat_v_error;
502  qp = robot.get_qp();
503  // (eps, lamda_max)
504  qpp = robot.jacobian_DLS_inv(0.015, 0.2)*(a - robot.jacobian_dot()*qp);
505  return robot.torque(robot.get_q(),qp, qpp, zero3, zero3);
506 }
507 
508 
509 // ------------------------------------------------------------------------------
510 // Position control based on Computed Torque Method
511 // ------------------------------------------------------------------------------
512 
515 {
516  dof = dof_;
517  qpp = ColumnVector(dof); qpp = 0;
518  q = qpp;
519  qp = qpp;
520  zero3 = ColumnVector(3); zero3 = 0;
521 }
522 
524  const DiagonalMatrix & Kp,
525  const DiagonalMatrix & Kd)
527 {
528  dof = robot.get_dof();
529  set_Kd(Kd);
530  set_Kp(Kp);
531  qpp = ColumnVector(dof); qpp = 0;
532  q = qpp;
533  qp = qpp;
534  zero3 = ColumnVector(3); zero3 = 0;
535 }
536 
538  const ColumnVector & qd,
539  const ColumnVector & qpd,
540  const ColumnVector & qppd )
542 {
543  if(qd.Nrows() != dof)
544  {
545  ColumnVector tau(dof); tau = 0;
546  cerr << "Computed_torque_methode::torque_cmd: wrong size for input vector qd" << endl;
547  tau.Release();
548  return tau;
549  }
550  if(qpd.Nrows() != dof)
551  {
552  ColumnVector tau(dof); tau = 0;
553  cerr << "Computed_torque_methode::torque_cmd: wrong size for input vector qpd" << endl;
554  tau.Release();
555  return tau;
556  }
557  if(qppd.Nrows() != dof)
558  {
559  ColumnVector tau(dof); tau = 0;
560  cerr << "Computed_torque_methode::torque_cmd: wrong size for input vector qppd" << endl;
561  tau.Release();
562  return tau;
563  }
564 
565  q = robot.get_q();
566  qp = robot.get_qp();
567  qpp = Kp*(qd-q) + Kd*(qpd-qp) + qppd;
568  return robot.torque(q, qp, qpp, zero3, zero3);
569 }
570 
576 {
577  if(Kd_.Nrows() != dof)
578  {
579  Kd = DiagonalMatrix(dof); Kd = 0;
580  cerr << "Computed_torque_method::set_kd: wrong size for input matrix Kd." << endl;
581  return WRONG_SIZE;
582  }
583 
584  Kd = Kd_;
585  return 0;
586 }
587 
593 {
594  if(Kp_.Nrows() != dof)
595  {
596  Kp = DiagonalMatrix(dof); Kp = 0;
597  cerr << "Computed_torque_method::set_kp: wrong size for input matrix Kp." << endl;
598  return WRONG_SIZE;
599  }
600 
601  Kp = Kp_;
602  return 0;
603 }
604 
605 // ------------------------------------------------------------------------------
606 // Position control based on Proportional_Derivative (PD)
607 // ------------------------------------------------------------------------------
608 
611 {
612  dof = dof_;
613  q = ColumnVector(dof); q = 0;
614  qp = q;
615  zero3 = ColumnVector(3); zero3 = 0;
616 }
617 
619  const DiagonalMatrix & Kp,
620  const DiagonalMatrix & Kd)
622 {
623  dof = robot.get_dof();
624  set_Kp(Kp);
625  set_Kd(Kd);
626  q = ColumnVector(dof); q = 0;
627  qp = q;
628  tau = ColumnVector(dof); tau = 0;
629  zero3 = ColumnVector(3); zero3 = 0;
630 }
631 
633  const ColumnVector & qd,
634  const ColumnVector & qpd)
636 {
637  if(qd.Nrows() != dof)
638  {
639  tau = 0;
640  cerr << "Proportional_Derivative::torque_cmd: wrong size for input vector qd" << endl;
641  return tau;
642  }
643  if(qpd.Nrows() != dof)
644  {
645  tau = 0;
646  cerr << "Proportional_Derivative::torque_cmd: wrong size for input vector qpd" << endl;
647  return tau;
648  }
649 
650  q = robot.get_q();
651  qp = robot.get_qp();
652  tau = Kp*(qd-q) + Kd*(qpd-qp);
653 
654  return tau;
655 }
656 
662 {
663  if(Kd_.Nrows() != dof)
664  {
665  Kd = DiagonalMatrix(dof); Kd = 0;
666  cerr << "Proportional_Derivative::set_kd: wrong size for input matrix Kd." << endl;
667  return WRONG_SIZE;
668  }
669 
670  Kd = Kd_;
671  return 0;
672 }
673 
679 {
680  if(Kp_.Nrows() != dof)
681  {
682  Kp = DiagonalMatrix(dof); Kp = 0;
683  cerr << "Computed_torque_method::set_kp: wrong size for input matrix Kp." << endl;
684  return WRONG_SIZE;
685  }
686 
687  Kp = Kp_;
688  return 0;
689 }
690 
691 #ifdef use_namespace
692 }
693 #endif
Quaternion class definition.
Definition: quaternion.h:92
short set_Do(const DiagonalMatrix &Do_)
Assign the rotational impedance damping matrix .
Definition: controller.cpp:232
Resolved_acc(const short dof=1)
Constructor.
Definition: controller.cpp:407
ReturnMatrix v() const
Return vector part.
Definition: quaternion.h:121
void Release()
Definition: newmat.h:514
ReturnMatrix Integ_Trap(const ColumnVector &present, ColumnVector &past, const Real dt)
Trapezoidal integration.
Definition: utils.cpp:165
ReturnMatrix get_qp(void) const
Return the joint velocity vector.
Definition: robot.cpp:1014
short set_Kp(const DiagonalMatrix &Kp_)
Assign the translational impedance stifness matrix .
Definition: controller.cpp:166
int get_dof() const
Return dof.
Definition: robot.h:240
ReturnMatrix torque_cmd(Robot_basic &robot, const ColumnVector &pdpp, const ColumnVector &pdp, const ColumnVector &pd, const ColumnVector &wdp, const ColumnVector &wd, const Quaternion &qd, const short link_pc, const Real dt)
Output torque.
Definition: controller.cpp:471
#define WRONG_SIZE
Return value when input vectors or matrix don&#39;t have the right size.
Definition: controller.h:61
ReturnMatrix x_prod_matrix(const ColumnVector &x)
Cross product matrix.
Definition: utils.cpp:88
ReturnMatrix torque_cmd(Robot_basic &robot, const ColumnVector &qd, const ColumnVector &qpd, const ColumnVector &qppd)
Output torque.
Definition: controller.cpp:537
void set_Kvp(const Real Kvp)
Assign the gain .
Definition: controller.cpp:447
double Real
Definition: include.h:307
short set_Kp(const DiagonalMatrix &Kp)
Assign the position error gain matrix .
Definition: controller.cpp:588
ReturnMatrix torque_cmd(Robot_basic &robot, const ColumnVector &qd, const ColumnVector &qpd)
Output torque.
Definition: controller.cpp:632
int Nrows() const
Definition: newmat.h:494
short Integ_quat(Quaternion &dquat_present, Quaternion &dquat_past, Quaternion &quat, const Real dt)
Trapezoidal quaternion integration.
Definition: quaternion.cpp:585
Real s() const
Return scalar part.
Definition: quaternion.h:119
static const char rcsid[]
RCS/CVS version.
Definition: controller.cpp:43
void set_Kpo(const Real Kpo)
Assign the gain .
Definition: controller.cpp:465
short set_Kd(const DiagonalMatrix &Kd)
Assign the velocity error gain matrix .
Definition: controller.cpp:571
short set_Mp(const DiagonalMatrix &Mp_)
Assign the translational impedance inertia matrix .
Definition: controller.cpp:100
virtual ReturnMatrix jacobian_dot(const int ref=0) const =0
short set_Mo(const DiagonalMatrix &Mo_)
Assign the rotational impedance inertia matrix .
Definition: controller.cpp:199
Virtual base robot class.
Definition: robot.h:216
short set_Kp(const DiagonalMatrix &Kp)
Assign the position error gain matrix .
Definition: controller.cpp:674
#define BASE_FRAME
Definition: quaternion.h:84
short set_Kd(const DiagonalMatrix &Kd)
Assign the velocity error gain matrix .
Definition: controller.cpp:657
The usual rectangular matrix.
Definition: newmat.h:625
Quaternion dot(const ColumnVector &w, const short sign) const
Quaternion time derivative.
Definition: quaternion.cpp:388
InvertedMatrix i() const
Definition: newmat6.cpp:329
Header file for controller class definitions.
void set_Kvo(const Real Kvo)
Assign the gain .
Definition: controller.cpp:459
FloatVector FloatVector * a
Real dot_prod(const Quaternion &q) const
Quaternion dot product.
Definition: quaternion.cpp:445
Impedance()
Constructor.
Definition: controller.cpp:54
Diagonal matrix.
Definition: newmat.h:896
ReturnMatrix kine_pd(const int ref=0) const
Direct kinematics with velocity.
Definition: kinemat.cpp:142
void kine(Matrix &Rot, ColumnVector &pos) const
Direct kinematics at end effector.
Definition: kinemat.cpp:92
Quaternion i() const
Quaternion inverse. where and are the quaternion conjugate and the quaternion norm respectively...
Definition: quaternion.cpp:323
ColumnVector * w
Definition: robot.h:313
ReturnMatrix jacobian_DLS_inv(const double eps, const double lambda_max, const int ref=0) const
Inverse Jacobian based on damped least squares inverse.
Definition: kinemat.cpp:169
short control(const ColumnVector &pdpp, const ColumnVector &pdp, const ColumnVector &pd, const ColumnVector &wdp, const ColumnVector &wd, const Quaternion &qd, const ColumnVector &f, const ColumnVector &n, const Real dt)
Generation of a compliance trajectory.
Definition: controller.cpp:298
virtual ReturnMatrix torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)=0
Column vector.
Definition: newmat.h:1008
Computed_torque_method(const short dof=1)
Constructor.
Definition: controller.cpp:513
void set_Kpp(const Real Kpp)
Assign the gain .
Definition: controller.cpp:453
Proportional_Derivative(const short dof=1)
Constructor.
Definition: controller.cpp:609
short set_Dp(const DiagonalMatrix &Dp_)
Assign the translational impedance damping matrix .
Definition: controller.cpp:133
Real get_q(const int i) const
Definition: robot.h:235
short set_Ko(const DiagonalMatrix &Ko_)
Assign the rotational impedance stifness matrix .
Definition: controller.cpp:265
Robot robot
Definition: demo.cpp:227


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