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 
33 static const char rcsid[] = "$Id: clik.cpp,v 1.6 2006/05/16 16:11:15 gourdeau Exp $";
34 
40 #include "quaternion.h"
41 #include "clik.h"
42 
43 #ifdef use_namespace
44 namespace ROBOOP {
45  using namespace NEWMAT;
46 #endif
47 
53 Clik::Clik(const Robot & robot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
54  const Real eps_, const Real lambda_max_, const Real dt_):
55  dt(dt_),
56  eps(eps_),
57  lambda_max(lambda_max_),
58  robot(robot_)
59 {
61  // Initialize with same joint position (and rates) has the robot.
62  q = robot.get_q();
63  qp = robot.get_qp();
64  qp_prev = qp;
65  Kpep = ColumnVector(3); Kpep = 0;
66  Koe0Quat = ColumnVector(3); Koe0Quat = 0;
67  v = ColumnVector(6); v = 0;
68 
69  if(Kp_.Nrows()==3)
70  Kp = Kp_;
71  else
72  {
73  Kp = DiagonalMatrix(3); Kp = 0.0;
74  cerr << "Clik::Clik-->Robot, Kp if not 3x3, set gain to 0." << endl;
75  }
76  if(Ko_.Nrows()==3)
77  Ko = Ko_;
78  else
79  {
80  Ko = DiagonalMatrix(3); Ko = 0.0;
81  cerr << "Clik::Clik-->Robot, Ko if not 3x3, set gain to 0." << endl;
82  }
83 }
84 
85 
91 Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
92  const Real eps_, const Real lambda_max_, const Real dt_):
93  dt(dt_),
94  eps(eps_),
95  lambda_max(lambda_max_),
96  mrobot(mrobot_)
97 {
99  // Initialize with same joint position (and rates) has the robot.
100  q = mrobot.get_q();
101  qp = mrobot.get_qp();
102  qp_prev = qp;
103  Kpep = ColumnVector(3); Kpep = 0;
104  Koe0Quat = ColumnVector(3); Koe0Quat = 0;
105  v = ColumnVector(6); v = 0;
106 
107  if(Kp_.Nrows()==3)
108  Kp = Kp_;
109  else
110  {
111  Kp = DiagonalMatrix(3); Kp = 0.0;
112  cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
113  }
114  if(Ko_.Nrows()==3)
115  Ko = Ko_;
116  else
117  {
118  Ko = DiagonalMatrix(3); Ko = 0.0;
119  cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
120  }
121 }
122 
123 
129 Clik::Clik(const mRobot_min_para & mrobot_min_para_, const DiagonalMatrix & Kp_,
130  const DiagonalMatrix & Ko_, const Real eps_, const Real lambda_max_,
131  const Real dt_):
132  dt(dt_),
133  eps(eps_),
134  lambda_max(lambda_max_),
135  mrobot_min_para(mrobot_min_para_)
136 {
138  // Initialize with same joint position (and rates) has the robot.
139  q = mrobot_min_para.get_q();
141  qp_prev = qp;
142  Kpep = ColumnVector(3); Kpep = 0;
143  Koe0Quat = ColumnVector(3); Koe0Quat = 0;
144  v = ColumnVector(6); v = 0;
145 
146  if(Kp_.Nrows()==3)
147  Kp = Kp_;
148  else
149  {
150  Kp = DiagonalMatrix(3); Kp = 0.0;
151  cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
152  }
153  if(Ko_.Nrows()==3)
154  Ko = Ko_;
155  else
156  {
157  Ko = DiagonalMatrix(3); Ko = 0.0;
158  cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
159  }
160 }
161 
162 
163 Clik::Clik(const Clik & x)
165 {
167  switch(robot_type)
168  {
169  case CLICK_DH:
170  robot = x.robot;
171  break;
172  case CLICK_mDH:
173  mrobot = x.mrobot;
174  break;
175  case CLICK_mDH_min_para:
177  break;
178  }
179  eps = x.eps;
181  dt = x.dt;
182  q = x.q;
183  qp = x.qp;
184  qp_prev = x.qp_prev;
185  Kpep = x.Kpep;
186  Koe0Quat = x.Koe0Quat;
187  Kp = x.Kp;
188  Ko = x.Ko;
189  v = x.v;
190 }
191 
194 {
196  switch(robot_type)
197  {
198  case CLICK_DH:
199  robot = x.robot;
200  break;
201  case CLICK_mDH:
202  mrobot = x.mrobot;
203  break;
204  case CLICK_mDH_min_para:
206  break;
207  }
208  eps = x.eps;
210  dt = x.dt;
211  q = x.q;
212  qp = x.qp;
213  qp_prev = x.qp_prev;
214  Kpep = x.Kpep;
215  Koe0Quat = x.Koe0Quat;
216  Kp = x.Kp;
217  Ko = x.Ko;
218  v = x.v;
219 
220  return *this;
221 }
222 
223 
225  const Quaternion & qqqd, const ColumnVector & wd)
233 {
234  ColumnVector p;
235  Matrix R;
236 
237  switch(robot_type)
238  {
239  case CLICK_DH:
240  robot.set_q(q);
241  robot.kine(R, p); // In base frame
242  break;
243  case CLICK_mDH:
244  mrobot.set_q(q);
245  mrobot.kine(R, p);
246  break;
247  case CLICK_mDH_min_para:
249  mrobot_min_para.kine(R, p);
250  break;
251  }
252  Kpep = Kp*(pd - p);
253  Quaternion qq(R);
254 
255  Quaternion qqd;
256 
257  if(qq.dot_prod(qqqd) < 0)
258  qqd = qqqd*(-1);
259  else
260  qqd = qqqd;
261 
262  // quaternion error on vectoriel part. We used equation 42 [4] instead equation 23 [1].
263  Koe0Quat = Ko*(qq.s()*qqd.v() - qqd.s()*qq.v() + x_prod_matrix(qq.v())*qqd.v());
264 
265  return 0;
266 }
267 
268 
269 void Clik::q_qdot(const Quaternion & qd, const ColumnVector & pd,
270  const ColumnVector & pdd, const ColumnVector & wd,
271  ColumnVector & q_, ColumnVector & qp_)
281 {
282  v.SubMatrix(1,3,1,1) = pdd + Kpep;
283  v.SubMatrix(4,6,1,1) = wd + Koe0Quat;
284 
285  switch(robot_type)
286  {
287  case CLICK_DH:
288  robot.set_q(q);
290  break;
291  case CLICK_mDH:
292  mrobot.set_q(q);
294  break;
295  case CLICK_mDH_min_para:
298  break;
299  }
300 
301  q = q + Integ_Trap(qp, qp_prev, dt);
302  endeff_pos_ori_err(pd, pdd, qd, wd);
303 
304  q_ = q;
305  qp_ = qp;
306 }
307 
308 #ifdef use_namespace
309 }
310 #endif
Quaternion class definition.
Definition: quaternion.h:92
Real eps
Range of singular region in Jacobian DLS inverse.
Definition: clik.h:108
Robot robot
Robot instance.
Definition: clik.h:112
#define CLICK_mDH
Using Clik under modified DH notation.
Definition: clik.h:81
ReturnMatrix v() const
Return vector part.
Definition: quaternion.h:121
mRobot mrobot
mRobot instance.
Definition: clik.h:113
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
DH notation robot class.
Definition: robot.h:340
Handle Closed Loop Inverse Kinematics scheme.
Definition: clik.h:87
Clik()
Definition: clik.h:89
Clik & operator=(const Clik &x)
Overload = operator.
Definition: clik.cpp:192
DiagonalMatrix Ko
Orientation error gain.
Definition: clik.h:115
DiagonalMatrix Kp
Position error gain.
Definition: clik.h:115
ReturnMatrix x_prod_matrix(const ColumnVector &x)
Cross product matrix.
Definition: utils.cpp:88
ColumnVector q
Clik joint position.
Definition: clik.h:118
double Real
Definition: include.h:307
#define CLICK_mDH_min_para
Using Clik under modified DH notation with minimum intertial parameters.
Definition: clik.h:83
int Nrows() const
Definition: newmat.h:494
ColumnVector Kpep
Kp times position error.
Definition: clik.h:118
Real s() const
Return scalar part.
Definition: quaternion.h:119
static const char rcsid[]
RCS/CVS version.
Definition: clik.cpp:33
Header file for Clik class definitions.
Real lambda_max
Damping factor in Jacobian DLS inverse.
Definition: clik.h:108
int endeff_pos_ori_err(const ColumnVector &pd, const ColumnVector &pddot, const Quaternion &qd, const ColumnVector &wd)
Obtain end effector position and orientation error.
Definition: clik.cpp:224
void set_q(const ColumnVector &q)
Set the joint position vector.
Definition: robot.cpp:1137
ColumnVector qp
Clik joint velocity.
Definition: clik.h:118
ColumnVector v
Quaternion vector part.
Definition: clik.h:118
Modified DH notation robot class.
Definition: robot.h:389
#define CLICK_DH
Using Clik under DH notation.
Definition: clik.h:79
Real dt
Time frame.
Definition: clik.h:108
The usual rectangular matrix.
Definition: newmat.h:625
ColumnVector qp_prev
Clik previous joint velocity.
Definition: clik.h:118
ColumnVector Koe0Quat
Ko times orientation error (quaternion vector part).
Definition: clik.h:118
Modified DH notation and minimal inertial parameters robot class.
Definition: robot.h:437
mRobot_min_para mrobot_min_para
mRobot_min_para instance.
Definition: clik.h:114
Quaternion class.
Diagonal matrix.
Definition: newmat.h:896
short robot_type
Robot type used.
Definition: clik.h:111
void kine(Matrix &Rot, ColumnVector &pos) const
Direct kinematics at end effector.
Definition: kinemat.cpp:92
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
Definition: newmat.h:2146
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
Column vector.
Definition: newmat.h:1008
void q_qdot(const Quaternion &qd, const ColumnVector &pd, const ColumnVector &pddot, const ColumnVector &wd, ColumnVector &q, ColumnVector &qp)
Obtain joints position and velocity.
Definition: clik.cpp:269
Real get_q(const int i) const
Definition: robot.h:235
Robot robot
Definition: demo.cpp:227


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