ROBOOP, A Robotics Object Oriented Package in C++
clik.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 Revision_history:
24 
25 2004/07/01: Etienne Lachance
26  -Added doxygen documentation.
27 
28 2004/07/01: Ethan Tira-Thompson
29  -Added support for newmat's use_namespace #define, using ROBOOP namespace.
30 */
31 
37 #include "quaternion.h"
38 #include "clik.h"
39 
40 #ifdef use_namespace
41 namespace ROBOOP {
42  using namespace NEWMAT;
43 #endif
44 
50 Clik::Clik(const Robot & robot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
51  const Real eps_, const Real lambda_max_, const Real dt_):
52  dt(dt_),
53  eps(eps_),
54  lambda_max(lambda_max_),
55  robot(robot_)
56 {
57  robot_type = CLICK_DH;
58  // Initialize with same joint position (and rates) has the robot.
59  q = robot.get_q();
60  qp = robot.get_qp();
61  qp_prev = qp;
62  Kpep = ColumnVector(3); Kpep = 0;
63  Koe0Quat = ColumnVector(3); Koe0Quat = 0;
64  v = ColumnVector(6); v = 0;
65 
66  if(Kp_.Nrows()==3)
67  Kp = Kp_;
68  else
69  {
70  Kp = DiagonalMatrix(3); Kp = 0.0;
71  cerr << "Clik::Clik-->Robot, Kp if not 3x3, set gain to 0." << endl;
72  }
73  if(Ko_.Nrows()==3)
74  Ko = Ko_;
75  else
76  {
77  Ko = DiagonalMatrix(3); Ko = 0.0;
78  cerr << "Clik::Clik-->Robot, Ko if not 3x3, set gain to 0." << endl;
79  }
80 }
81 
82 
88 Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
89  const Real eps_, const Real lambda_max_, const Real dt_):
90  dt(dt_),
91  eps(eps_),
92  lambda_max(lambda_max_),
93  mrobot(mrobot_)
94 {
95  robot_type = CLICK_mDH;
96  // Initialize with same joint position (and rates) has the robot.
97  q = mrobot.get_q();
98  qp = mrobot.get_qp();
99  qp_prev = qp;
100  Kpep = ColumnVector(3); Kpep = 0;
101  Koe0Quat = ColumnVector(3); Koe0Quat = 0;
102  v = ColumnVector(6); v = 0;
103 
104  if(Kp_.Nrows()==3)
105  Kp = Kp_;
106  else
107  {
108  Kp = DiagonalMatrix(3); Kp = 0.0;
109  cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
110  }
111  if(Ko_.Nrows()==3)
112  Ko = Ko_;
113  else
114  {
115  Ko = DiagonalMatrix(3); Ko = 0.0;
116  cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
117  }
118 }
119 
120 
126 Clik::Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_,
127  const DiagonalMatrix & Ko_, const Real eps_, const Real lambda_max_,
128  const Real dt_):
129  dt(dt_),
130  eps(eps_),
131  lambda_max(lambda_max_),
132  mrobot_min_para(mrobot_min_para_)
133 {
134  robot_type = CLICK_mDH_min_para;
135  // Initialize with same joint position (and rates) has the robot.
136  q = mrobot_min_para.get_q();
137  qp = mrobot_min_para.get_qp();
138  qp_prev = qp;
139  Kpep = ColumnVector(3); Kpep = 0;
140  Koe0Quat = ColumnVector(3); Koe0Quat = 0;
141  v = ColumnVector(6); v = 0;
142 
143  if(Kp_.Nrows()==3)
144  Kp = Kp_;
145  else
146  {
147  Kp = DiagonalMatrix(3); Kp = 0.0;
148  cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
149  }
150  if(Ko_.Nrows()==3)
151  Ko = Ko_;
152  else
153  {
154  Ko = DiagonalMatrix(3); Ko = 0.0;
155  cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
156  }
157 }
158 
159 
160 Clik::Clik(const Clik & x)
162 {
163  robot_type = x.robot_type;
164  switch(robot_type)
165  {
166  case CLICK_DH:
167  robot = x.robot;
168  break;
169  case CLICK_mDH:
170  mrobot = x.mrobot;
171  break;
172  case CLICK_mDH_min_para:
173  mrobot_min_para = x.mrobot_min_para;
174  break;
175  }
176  eps = x.eps;
177  lambda_max = x.lambda_max;
178  dt = x.dt;
179  q = x.q;
180  qp = x.qp;
181  qp_prev = x.qp_prev;
182  Kpep = x.Kpep;
183  Koe0Quat = x.Koe0Quat;
184  Kp = x.Kp;
185  Ko = x.Ko;
186  v = x.v;
187 }
188 
189 Clik & Clik::operator=(const Clik & x)
191 {
192  robot_type = x.robot_type;
193  switch(robot_type)
194  {
195  case CLICK_DH:
196  robot = x.robot;
197  break;
198  case CLICK_mDH:
199  mrobot = x.mrobot;
200  break;
201  case CLICK_mDH_min_para:
202  mrobot_min_para = x.mrobot_min_para;
203  break;
204  }
205  eps = x.eps;
206  lambda_max = x.lambda_max;
207  dt = x.dt;
208  q = x.q;
209  qp = x.qp;
210  qp_prev = x.qp_prev;
211  Kpep = x.Kpep;
212  Koe0Quat = x.Koe0Quat;
213  Kp = x.Kp;
214  Ko = x.Ko;
215  v = x.v;
216 
217  return *this;
218 }
219 
220 
221 int Clik::endeff_pos_ori_err(const ColumnVector & pd, const ColumnVector & pdd,
222  const Quaternion & qqqd, const ColumnVector & wd)
230 {
231  ColumnVector p;
232  Matrix R;
233 
234  switch(robot_type)
235  {
236  case CLICK_DH:
237  robot.set_q(q);
238  robot.kine(R, p); // In base frame
239  break;
240  case CLICK_mDH:
241  mrobot.set_q(q);
242  mrobot.kine(R, p);
243  break;
244  case CLICK_mDH_min_para:
245  mrobot_min_para.set_q(q);
246  mrobot_min_para.kine(R, p);
247  break;
248  }
249  Kpep = Kp*(pd - p);
250  Quaternion qq(R);
251 
252  Quaternion qqd;
253 
254  if(qq.dot_prod(qqqd) < 0)
255  qqd = qqqd*(-1);
256  else
257  qqd = qqqd;
258 
259  // quaternion error on vectoriel part. We used equation 42 [4] instead equation 23 [1].
260  Koe0Quat = Ko*(qq.s()*qqd.v() - qqd.s()*qq.v() + x_prod_matrix(qq.v())*qqd.v());
261 
262  return 0;
263 }
264 
265 
266 void Clik::q_qdot(const Quaternion & qd, const ColumnVector & pd,
267  const ColumnVector & pdd, const ColumnVector & wd,
268  ColumnVector & q_, ColumnVector & qp_)
278 {
279  v.SubMatrix(1,3,1,1) = pdd + Kpep;
280  v.SubMatrix(4,6,1,1) = wd + Koe0Quat;
281 
282  switch(robot_type)
283  {
284  case CLICK_DH:
285  robot.set_q(q);
286  qp = robot.jacobian_DLS_inv(eps, lambda_max)*v;
287  break;
288  case CLICK_mDH:
289  mrobot.set_q(q);
290  qp = mrobot.jacobian_DLS_inv(eps, lambda_max)*v;
291  break;
292  case CLICK_mDH_min_para:
293  mrobot_min_para.set_q(q);
294  qp = mrobot_min_para.jacobian_DLS_inv(eps, lambda_max)*v;
295  break;
296  }
297 
298  q = q + Integ_Trap(qp, qp_prev, dt);
299  endeff_pos_ori_err(pd, pdd, qd, wd);
300 
301  q_ = q;
302  qp_ = qp;
303 }
304 
305 #ifdef use_namespace
306 }
307 #endif