ROBOOP, A Robotics Object Oriented Package in C++
controller.h
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 2004/07/13: Ethan Tira-Thompson
27  -Added support for newmat's use_namespace #define, using ROBOOP namespace
28 
29 2005/06/10: Etienne Lachance
30  -The desired joint acceleration was missing in the computed torque method.
31 
32 2005/11/06: Etienne Lachance
33  - No need to provide a copy constructor and the assignment operator
34  (operator=) for Impedance, Resolved_acc, Computed_torque_method and
35  Proportional_Derivative classes. Instead we use the one provide by the
36  compiler.
37 -------------------------------------------------------------------------------
38 */
39 
40 #ifndef CONTROLLER_H
41 #define CONTROLLER_H
42 
48 #include "robot.h"
49 #include "quaternion.h"
50 
51 #ifdef use_namespace
52 namespace ROBOOP {
53  using namespace NEWMAT;
54 #endif
55 
57 #define WRONG_SIZE -1
58 
87 class Impedance{
88 public:
89  Impedance();
90  Impedance(const Robot_basic & robot, const DiagonalMatrix & Mp_,
91  const DiagonalMatrix & Dp_, const DiagonalMatrix & Kp_,
92  const DiagonalMatrix & Mo_, const DiagonalMatrix & Do_,
93  const DiagonalMatrix & Ko_);
94  short set_Mp(const DiagonalMatrix & Mp_);
95  short set_Mp(const Real MP_i, const short i);
96  short set_Dp(const DiagonalMatrix & Dp_);
97  short set_Dp(const Real Dp_i, const short i);
98  short set_Kp(const DiagonalMatrix & Kp_);
99  short set_Kp(const Real Kp_i, const short i);
100  short set_Mo(const DiagonalMatrix & Mo_);
101  short set_Mo(const Real Mo_i, const short i);
102  short set_Do(const DiagonalMatrix & Do_);
103  short set_Do(const Real Do_i, const short i);
104  short set_Ko(const DiagonalMatrix & Ko_);
105  short set_Ko(const Real Ko_i, const short i);
106  short control(const ColumnVector & pdpp, const ColumnVector & pdp,
107  const ColumnVector & pd, const ColumnVector & wdp,
108  const ColumnVector & wd, const Quaternion & qd,
109  const ColumnVector & f, const ColumnVector & n,
110  const Real dt);
111 
113  qcp,
114  qcp_prev,
115  qcd,
116  quat;
117  ColumnVector pc,
118  pcp,
119  pcpp,
120  pcp_prev,
121  pcpp_prev,
122  pcd,
123  pcdp,
124  wc,
125  wcp,
126  wcp_prev,
127  wcd;
128 private:
129  DiagonalMatrix Mp,
130  Dp,
131  Kp,
132  Mo,
133  Do,
134  Ko;
135  Matrix Ko_prime;
136 };
137 
168 public:
169  Resolved_acc(const short dof = 1);
170  Resolved_acc(const Robot_basic & robot,
171  const Real Kvp, const Real Kpp,
172  const Real Kvo, const Real Kpo);
173  void set_Kvp(const Real Kvp);
174  void set_Kpp(const Real Kpp);
175  void set_Kvo(const Real Kvo);
176  void set_Kpo(const Real Kpo);
177 
178  ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & pdpp,
179  const ColumnVector & pdp, const ColumnVector & pd,
180  const ColumnVector & wdp, const ColumnVector & wd,
181  const Quaternion & qd, const short link_pc,
182  const Real dt);
183 private:
184  double Kvp,
185  Kpp,
186  Kvo,
187  Kpo;
188  Matrix Rot;
189  ColumnVector zero3,
190  qp,
191  qpp,
192  a,
193  p,
194  pp,
195  quat_v_error;
197 };
198 
199 
217 public:
218  Computed_torque_method(const short dof = 1);
219  Computed_torque_method(const Robot_basic & robot,
220  const DiagonalMatrix & Kp, const DiagonalMatrix & Kd);
221  ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
222  const ColumnVector & qpd,
223  const ColumnVector & qppd);
224  short set_Kd(const DiagonalMatrix & Kd);
225  short set_Kp(const DiagonalMatrix & Kp);
226 
227 private:
228  int dof;
229  ColumnVector q,
230  qp,
231  qpp,
232  zero3;
233  DiagonalMatrix Kp,
234  Kd;
235 };
236 
237 
249 public:
250  Proportional_Derivative(const short dof = 1);
251  Proportional_Derivative(const Robot_basic & robot, const DiagonalMatrix & Kp,
252  const DiagonalMatrix & Kd);
253  ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & qd,
254  const ColumnVector & qpd);
255  short set_Kd(const DiagonalMatrix & Kd);
256  short set_Kp(const DiagonalMatrix & Kp);
257 
258 private:
259  int dof;
260  ColumnVector q,
261  qp,
262  qpp,
263  tau,
264  zero3;
265  DiagonalMatrix Kp,
266  Kd;
267 };
268 
269 #ifdef use_namespace
270 }
271 #endif
272 
273 #endif
274