ikBase.cpp
Go to the documentation of this file.
1 /*
2  * Katana Native Interface - A C++ interface to the robot arm Katana.
3  * Copyright (C) 2005 Neuronics AG
4  * Check out the AUTHORS file for detailed contact information.
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  */
20 
21 
22 #include "KNI_InvKin/ikBase.h"
23 #include "libKinematics.h"
24 
25 CikBase::~CikBase() {
26  if(mKinematics != 0) {
27  // mKinematics == 1 and default
28  // RobAnaGuess Kinematics
29  if(_kinematicsIsInitialized) {
30  kin_clean();
31  }
32  }
33 }
34 
35 void CikBase::_initKinematics() {
36 
37  if(mKinematics == 0) {
38  // Analytical Kinematics
39  if( std::string(base->GetGNL()->modelName) == "Katana6M90A_G") {
40  _kinematicsImpl.reset(new KNI::KatanaKinematics6M90G);
41  } else if(std::string(base->GetGNL()->modelName) == "Katana6M90A_F") {
42  _kinematicsImpl.reset(new KNI::KatanaKinematics6M90T);
43  } else if( std::string(base->GetGNL()->modelName) == "Katana6M90B_G") {
44  _kinematicsImpl.reset(new KNI::KatanaKinematics6M90G);
45  } else if(std::string(base->GetGNL()->modelName) == "Katana6M90B_F") {
46  _kinematicsImpl.reset(new KNI::KatanaKinematics6M90T);
47  } else if( std::string(base->GetGNL()->modelName) == "Katana6M90G") {
48  _kinematicsImpl.reset(new KNI::KatanaKinematics6M90G);
49  } else if(std::string(base->GetGNL()->modelName) == "Katana6M90T") {
50  _kinematicsImpl.reset(new KNI::KatanaKinematics6M90T);
51  } else if(std::string(base->GetGNL()->modelName) == "Katana6M180") {
52  _kinematicsImpl.reset(new KNI::KatanaKinematics6M180);
53  } else if(std::string(base->GetGNL()->modelName) == "Katana5M180") {
54  _kinematicsImpl.reset(new KNI::KatanaKinematics5M180);
55  } else {
56  return;
57  }
58 
59  const TKatEFF* eff = base->GetEFF();
60  const TKatMOT* mot = base->GetMOT();
61 
62  KNI::KatanaKinematics::metrics length;
63  for(int i = 0; i < getNumberOfMotors()-2; ++i) {
64  length.push_back( eff->arr_segment[i] );
65  }
66 
67  KNI::KinematicParameters joint;
68  KNI::KatanaKinematics::parameter_container parameters;
69  for(int i = 0; i < getNumberOfMotors(); ++i) {
70  joint.epc = mot->arr[i].GetInitialParameters()->encodersPerCycle;
71  joint.encOffset = mot->arr[i].GetInitialParameters()->encoderOffset;
72  joint.angleOffset = mot->arr[i].GetInitialParameters()->angleOffset;
73  joint.angleStop = mot->arr[i].GetInitialParameters()->angleStop;
74  joint.rotDir = mot->arr[i].GetInitialParameters()->rotationDirection;
75  parameters.push_back(joint);
76  }
77 
78  _kinematicsImpl->init(length, parameters);
79  } else {
80  // mKinematics == 1 and default
81  // RobAnaGuess Kinematics
82  int type = -1;
83  if( std::string(base->GetGNL()->modelName) == "Katana6M90A_G") {
84  type = 1;
85  } else if(std::string(base->GetGNL()->modelName) == "Katana6M90A_F") {
86  type = 0;
87  } else if(std::string(base->GetGNL()->modelName) == "Katana6M90B_G") {
88  type = 4;
89  } else if(std::string(base->GetGNL()->modelName) == "Katana6M90B_F") {
90  type = 3;
91  } else if(std::string(base->GetGNL()->modelName) == "Katana6M90G") {
92  type = 1;
93  } else if(std::string(base->GetGNL()->modelName) == "Katana6M90T") {
94  type = 0;
95  } else if(std::string(base->GetGNL()->modelName) == "Katana6M180") {
96  type = 2;
97  } else {
98  return;
99  }
100 
101  // set type
102  kin_setType(type);
103  int dom = kin_getDOM();
104  // set link length
105  FloatVector links;
106  links.data[0] = (float) (base->GetEFF()->arr_segment[0] / 1000.0);
107  links.data[1] = (float) (base->GetEFF()->arr_segment[1] / 1000.0);
108  links.data[2] = (float) (base->GetEFF()->arr_segment[2] / 1000.0);
109  links.data[3] = (float) (base->GetEFF()->arr_segment[3] / 1000.0);
110  links.length = 4;
111  kin_setLinkLen(&links);
112  // set encoder per cycle
113  IntVector epc;
114  for (int i = 0; i < dom; ++i) {
115  epc.data[i] = base->GetMOT()->arr[i].GetInitialParameters()->encodersPerCycle;
116  }
117  epc.length = dom;
118  kin_setEPC(&epc);
119  // set encoder offset
120  IntVector encOffset;
121  for (int i = 0; i < dom; ++i) {
122  encOffset.data[i] = base->GetMOT()->arr[i].GetInitialParameters()->encoderOffset;
123  }
124  encOffset.length = dom;
125  kin_setEncOff(&encOffset);
126  // set rotation direction
127  IntVector rotDir;
128  for (int i = 0; i < dom; ++i) {
129  if (i < 3) {
130  // invert first three rotation directions (different angle definitions)
131  rotDir.data[i] = -base->GetMOT()->arr[i].GetInitialParameters()->rotationDirection;
132  } else {
133  rotDir.data[i] = base->GetMOT()->arr[i].GetInitialParameters()->rotationDirection;
134  }
135  }
136  rotDir.length = dom;
137  kin_setRotDir(&rotDir);
138  // set angle offset
139  FloatVector angleOffset;
140  for (int i = 0; i < dom; ++i) {
141  angleOffset.data[i] = (float) (base->GetMOT()->arr[i].GetInitialParameters()->angleOffset);
142  }
143  angleOffset.length = dom;
144  FloatVector angleOffset2;
145  kin_K4D2mDHAng(&angleOffset, &angleOffset2);
146  kin_setAngOff(&angleOffset2);
147  // set angle range
148  FloatVector angleRange;
149  for (int i = 0; i < dom; ++i) {
150  angleRange.data[i] = (float) fabs(base->GetMOT()->arr[i].GetInitialParameters()->angleRange);
151  }
152  angleRange.length = dom;
153  kin_setAngRan(&angleRange);
154  // initialize kinematics
155  kin_init();
156  }
157 
158  _kinematicsIsInitialized = true;
159 }
160 
161 void CikBase::getKinematicsVersion(std::vector<int>& version) {
162  if(mKinematics == 0) {
163  // Analytical Kinematics
164  version.clear();
165  version.push_back(0);
166  version.push_back(1);
167  version.push_back(0);
168  } else {
169  // mKinematics == 1 and default
170  // RobAnaGuess Kinematics
171  IntVector v;
172  kin_getVersion(&v);
173  version.clear();
174  for (int i = 0; i < v.length; ++i) {
175  version.push_back(v.data[i]);
176  }
177  }
178 }
179 
180 void CikBase::setTcpOffset(double xoff, double yoff, double zoff, double psioff) {
181 
182  if(mKinematics != 0) {
183  // mKinematics == 1 and default
184  // RobAnaGuess Kinematics
185  FloatVector tcpOff;
186  tcpOff.data[0] = (float) xoff;
187  tcpOff.data[1] = (float) yoff;
188  tcpOff.data[2] = (float) zoff;
189  tcpOff.data[3] = (float) psioff;
190  tcpOff.length = 4;
191  kin_setTcpOff(&tcpOff);
192  }
193 
194 }
195 
196 void CikBase::DKApos(double* position) {
197  getCoordinates(position[0], position[1], position[2], position[3], position[4], position[5]);
198 }
199 
200 void CikBase::IKCalculate(double X, double Y, double Z, double phi, double theta, double psi, std::vector<int>::iterator solution_iter) {
201 
202  if(!_kinematicsIsInitialized)
203  _initKinematics();
204 
205  if(mKinematics == 0) {
206  // Analytical Kinematics
207  std::vector<double> pose(6);
208  pose[0] = X;
209  pose[1] = Y;
210  pose[2] = Z;
211  pose[3] = phi;
212  pose[4] = theta;
213  pose[5] = psi;
214 
215  std::vector<int> actualPosition;
216  base->recvMPS();
217  for(int c = 0; c < getNumberOfMotors(); ++c) {
218  actualPosition.push_back(getMotorEncoders(c, false));
219  }
220 
221  _kinematicsImpl->IK(solution_iter, pose, actualPosition);
222  } else {
223  // mKinematics == 1 and default
224  // RobAnaGuess Kinematics
225  int maxBisection = 3;
226  int nOfMot = getNumberOfMotors();
227 
228  FloatVector pose;
229  pose.data[0] = (float) (X / 1000);
230  pose.data[1] = (float) (Y / 1000);
231  pose.data[2] = (float) (Z / 1000);
232  pose.data[3] = (float) phi;
233  pose.data[4] = (float) theta;
234  pose.data[5] = (float) psi;
235  pose.length = 6;
236 
237  IntVector actualPosition;
238  base->recvMPS();
239  for(int i = 0; i < nOfMot; ++i) {
240  actualPosition.data[i] = getMotorEncoders(i, false);
241  }
242  actualPosition.length = nOfMot;
243  FloatVector prev;
244  kin_enc2rad(&actualPosition, &prev);
245 
246  FloatVector ikangle;
247  kin_IK(&pose, &prev, &ikangle, maxBisection);
248 
249  IntVector ikenc;
250  kin_rad2enc(&ikangle, &ikenc);
251 
252  // copy motor 6 encoder if KNI uses 6 and kinematics uses 5 motors (w/o gripper)
253  if (ikenc.length == actualPosition.length - 1) {
254  ikenc.data[ikenc.length] = actualPosition.data[ikenc.length];
255  ikenc.length = actualPosition.length;
256  }
257 
258  for(int i = 0; i < nOfMot; ++i) {
259  *solution_iter = ikenc.data[i];
260  solution_iter++;
261  }
262  }
263 
264 }
265 
266 void CikBase::IKCalculate(double X, double Y, double Z, double phi, double theta, double psi, std::vector<int>::iterator solution_iter, const std::vector<int>& actualPosition) {
267 
268  if(!_kinematicsIsInitialized)
269  _initKinematics();
270 
271  if(mKinematics == 0) {
272  // Analytical Kinematics
273  std::vector<double> pose(6);
274  pose[0] = X;
275  pose[1] = Y;
276  pose[2] = Z;
277  pose[3] = phi;
278  pose[4] = theta;
279  pose[5] = psi;
280 
281  _kinematicsImpl->IK(solution_iter, pose, actualPosition);
282  } else {
283  // mKinematics == 1 and default
284  // RobAnaGuess Kinematics
285  int maxBisection = 3;
286  int nOfMot = getNumberOfMotors();
287 
288  FloatVector pose;
289  pose.data[0] = (float) (X / 1000);
290  pose.data[1] = (float) (Y / 1000);
291  pose.data[2] = (float) (Z / 1000);
292  pose.data[3] = (float) phi;
293  pose.data[4] = (float) theta;
294  pose.data[5] = (float) psi;
295  pose.length = 6;
296 
297  IntVector actPos;
298  for(int i = 0; i < nOfMot; ++i) {
299  actPos.data[i] = actualPosition.at(i);
300  }
301  actPos.length = nOfMot;
302  FloatVector prev;
303  kin_enc2rad(&actPos, &prev);
304 
305  FloatVector ikangle;
306  int error = kin_IK(&pose, &prev, &ikangle, maxBisection);
307  if (error)
308  throw KNI::NoSolutionException();
309 
310  IntVector ikenc;
311  kin_rad2enc(&ikangle, &ikenc);
312 
313  // copy motor 6 encoder if KNI uses 6 and kinematics uses 5 motors (w/o gripper)
314  if (ikenc.length == actPos.length - 1) {
315  ikenc.data[ikenc.length] = actPos.data[ikenc.length];
316  ikenc.length = actPos.length;
317  }
318 
319  for(int i = 0; i < nOfMot; ++i) {
320  *solution_iter = ikenc.data[i];
321  solution_iter++;
322  }
323  }
324 
325 }
326 
327 void CikBase::IKGoto(double X, double Y, double Z, double Al, double Be, double Ga, bool wait, int tolerance, long timeout) {
328 
329  if(!_kinematicsIsInitialized)
330  _initKinematics();
331 
332  const TKatMOT* mot = base->GetMOT();
333 
334  std::vector<int> solution(getNumberOfMotors());
335  // fills act_pos[] with the current position in degree units
336  std::vector<int> act_pos(getNumberOfMotors());
337  std::vector<int> distance(getNumberOfMotors());
338 
339  base->recvMPS();
340  for (int idx=0; idx<getNumberOfMotors(); ++idx) {
341  act_pos[idx] = mot->arr[idx].GetPVP()->pos;
342  }
343 
344  IKCalculate( X, Y, Z, Al, Be, Ga, solution.begin(), act_pos );
345  moveRobotToEnc( solution.begin(), solution.end(), wait, tolerance, timeout);
346 
347 }
348 
349 void CikBase::getCoordinates(double& x, double& y, double& z, double& phi, double& theta, double& psi, bool refreshEncoders) {
350 
351  if(!_kinematicsIsInitialized)
352  _initKinematics();
353 
354  if(refreshEncoders)
355  base->recvMPS();
356 
357  if(mKinematics == 0) {
358  // Analytical Kinematics
359  std::vector<int> current_encoders(getNumberOfMotors());
360  for (int i=0; i<getNumberOfMotors(); i++) {
361  current_encoders[i] = base->GetMOT()->arr[i].GetPVP()->pos;
362  }
363 
364  std::vector<double> pose(6);
365 
366  _kinematicsImpl->DK(pose, current_encoders);
367 
368  x = pose[0];
369  y = pose[1];
370  z = pose[2];
371  phi = pose[3];
372  theta = pose[4];
373  psi = pose[5];
374  } else {
375  // mKinematics == 1 and default
376  // RobAnaGuess Kinematics
377  int nOfMot = getNumberOfMotors();
378 
379  IntVector actPos;
380  for(int i = 0; i < nOfMot; ++i) {
381  actPos.data[i] = base->GetMOT()->arr[i].GetPVP()->pos;
382  }
383  actPos.length = nOfMot;
384  FloatVector angle;
385  kin_enc2rad(&actPos, &angle);
386 
387  FloatVector pose;
388  kin_DK(&angle, &pose);
389 
390  x = pose.data[0] * 1000;
391  y = pose.data[1] * 1000;
392  z = pose.data[2] * 1000;
393  phi = pose.data[3];
394  theta = pose.data[4];
395  psi = pose.data[5];
396  }
397 }
398 
399 void CikBase::getCoordinatesFromEncoders(std::vector<double>& pos, const std::vector<int>& encs){
400 
401  if(!_kinematicsIsInitialized)
402  _initKinematics();
403 
404  if(mKinematics == 0) {
405  // Analytical Kinematics
406  _kinematicsImpl->DK(pos, encs);
407  } else {
408  // mKinematics == 1 and default
409  int nOfMot = getNumberOfMotors();
410  IntVector actPos;
411  for(int i = 0; i < nOfMot; ++i) {
412  actPos.data[i] = encs.at(i);
413  }
414  actPos.length = nOfMot;
415  FloatVector angle;
416  kin_enc2rad(&actPos, &angle);
417  FloatVector pose;
418  kin_DK(&angle, &pose);
419  pos.clear();
420  pos.push_back(pose.data[0] * 1000);
421  pos.push_back(pose.data[1] * 1000);
422  pos.push_back(pose.data[2] * 1000);
423  pos.push_back(pose.data[3]);
424  pos.push_back(pose.data[4]);
425  pos.push_back(pose.data[5]);
426  }
427 }
428 
429 void CikBase::moveRobotTo(double x, double y, double z, double phi, double theta, double psi, const bool waitUntilReached, const int waitTimeout) {
430  IKGoto(x, y, z, phi, theta, psi, waitUntilReached, 100, waitTimeout);
431 }
432 
433 void CikBase::moveRobotTo(std::vector<double> coordinates, const bool waitUntilReached, const int waitTimeout) {
434  IKGoto(coordinates.at(0), coordinates.at(1), coordinates.at(2), coordinates.at(3), coordinates.at(4), coordinates.at(5), waitUntilReached, 100, waitTimeout);
435 }
CikBase::setTcpOffset
void setTcpOffset(double xoff, double yoff, double zoff, double psioff)
Definition: ikBase.cpp:180
TKatGNL::modelName
char modelName[255]
model name
Definition: kmlBase.h:69
CikBase::_kinematicsImpl
std::unique_ptr< KNI::KatanaKinematics > _kinematicsImpl
Definition: ikBase.h:52
prev
FloatVector * prev
Definition: include/libKinematics.h:355
CKatana::base
CKatBase * base
base katana
Definition: kmlExt.h:67
kin_K4D2mDHAng
int kin_K4D2mDHAng(FloatVector *angleK4D, FloatVector *angleMDH)
Definition: libKinematics.cpp:329
kin_IK
int kin_IK(FloatVector *pose, FloatVector *prev, FloatVector *angle, int maxBisection)
Definition: libKinematics.cpp:411
kin_setAngRan
int kin_setAngRan(FloatVector *angleRange)
Definition: libKinematics.cpp:113
TKatEFF::arr_segment
double arr_segment[4]
length of the Katana segments
Definition: kmlBase.h:114
CikBase::_initKinematics
void _initKinematics()
Definition: ikBase.cpp:35
CikBase::_kinematicsIsInitialized
bool _kinematicsIsInitialized
Definition: ikBase.h:53
kin_init
int kin_init()
Definition: libKinematics.cpp:310
CMotBase::GetPVP
const TMotPVP * GetPVP()
Definition: kmlMotBase.h:246
TMotInit::encoderOffset
int encoderOffset
Definition: kmlMotBase.h:200
kin_getDOM
int kin_getDOM()
Definition: libKinematics.cpp:158
TMotInit::encodersPerCycle
int encodersPerCycle
Definition: kmlMotBase.h:201
kin_setEncOff
int kin_setEncOff(IntVector *encOffset)
Definition: libKinematics.cpp:77
CikBase::~CikBase
~CikBase()
Definition: ikBase.cpp:25
kin_getVersion
int kin_getVersion(IntVector *version)
Definition: libKinematics.cpp:288
kin_setType
int kin_setType(int type)
Definition: libKinematics.cpp:21
IntVector::data
int data[MaxDof]
Definition: include/libKinematics.h:34
TKatMOT::arr
CMotBase * arr
array of motors
Definition: kmlMotBase.h:42
kin_setLinkLen
int kin_setLinkLen(FloatVector *links)
Definition: libKinematics.cpp:45
pose
FloatVector * pose
Definition: include/libKinematics.h:343
CKatBase::GetEFF
TKatEFF * GetEFF()
Get a pointer to the desired structure.
Definition: kmlBase.h:169
KNI::KatanaKinematics::parameter_container
std::vector< KinematicParameters > parameter_container
Definition: KatanaKinematics.h:67
CikBase::getCoordinates
void getCoordinates(double &x, double &y, double &z, double &phi, double &theta, double &psi, bool refreshEncoders=true)
Definition: ikBase.cpp:349
kin_clean
int kin_clean()
Definition: libKinematics.cpp:320
kin_DK
int kin_DK(FloatVector *angle, FloatVector *pose)
Definition: libKinematics.cpp:395
CKatana::getMotorEncoders
int getMotorEncoders(short number, bool refreshEncoders=true) const
Definition: kmlExt.cpp:362
TMotInit::angleStop
double angleStop
Definition: kmlMotBase.h:207
CKatBase::GetMOT
const TKatMOT * GetMOT()
Get a pointer to the desired structure.
Definition: kmlBase.h:165
CKatBase::recvMPS
void recvMPS()
read all motor positions simultaneously
Definition: kmlBase.cpp:247
TKatEFF
Inverse Kinematics structure of the endeffektor.
Definition: kmlBase.h:113
TKatMOT
[MOT] every motor&#39;s attributes
Definition: kmlMotBase.h:40
CKatana::mKinematics
int mKinematics
The kinematics implementation: 0 for Analytical, 1 for RobAnaGuess.
Definition: kmlExt.h:75
CikBase::getKinematicsVersion
void getKinematicsVersion(std::vector< int > &version)
Definition: ikBase.cpp:161
TMotInit::rotationDirection
int rotationDirection
Definition: kmlMotBase.h:204
maxBisection
FloatVector FloatVector int maxBisection
Definition: include/libKinematics.h:356
TMotInit::angleRange
double angleRange
Definition: kmlMotBase.h:203
kin_setEPC
int kin_setEPC(IntVector *epc)
Definition: libKinematics.cpp:65
kin_setRotDir
int kin_setRotDir(IntVector *rotDir)
Definition: libKinematics.cpp:89
CikBase::getCoordinatesFromEncoders
void getCoordinatesFromEncoders(std::vector< double > &pose, const std::vector< int > &encs)
Returns the position of the robot corresponting to the given encoders in cartesian coordinates...
Definition: ikBase.cpp:399
TMotInit::angleOffset
double angleOffset
Definition: kmlMotBase.h:202
TMotPVP::pos
short pos
position
Definition: kmlMotBase.h:164
angle
FloatVector * angle
Definition: include/libKinematics.h:323
kin_setTcpOff
int kin_setTcpOff(FloatVector *tcpOffset)
Definition: libKinematics.cpp:126
CikBase::moveRobotTo
void moveRobotTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached=false, int waitTimeout=TM_ENDLESS)
Definition: ikBase.cpp:429
CMotBase::GetInitialParameters
const TMotInit * GetInitialParameters()
Definition: kmlMotBase.h:250
kin_enc2rad
int kin_enc2rad(IntVector *enc, FloatVector *angle)
Definition: libKinematics.cpp:361
kin_rad2enc
int kin_rad2enc(FloatVector *angle, IntVector *enc)
Definition: libKinematics.cpp:378
kin_setAngOff
int kin_setAngOff(FloatVector *angleOffset)
Definition: libKinematics.cpp:101
CKatana::getNumberOfMotors
short getNumberOfMotors() const
Definition: kmlExt.cpp:357
CikBase::IKCalculate
void IKCalculate(double X, double Y, double Z, double Al, double Be, double Ga, std::vector< int >::iterator solution_iter)
Definition: ikBase.cpp:200
FloatVector::data
float data[MaxDof]
Definition: include/libKinematics.h:40
CikBase::DKApos
void DKApos(double *position)
Definition: ikBase.cpp:196
CKatana::moveRobotToEnc
void moveRobotToEnc(std::vector< int >::const_iterator start, std::vector< int >::const_iterator end, bool waitUntilReached=false, int encTolerance=100, int waitTimeout=0)
Definition: kmlExt.cpp:463
KNI::KatanaKinematics::metrics
std::vector< double > metrics
To store metrics, &#39;aka&#39; the length&#39;s of the different segments of the robot.
Definition: KatanaKinematics.h:77
CKatBase::GetGNL
const TKatGNL * GetGNL()
Get a pointer to the desired structure.
Definition: kmlBase.h:152
CikBase::IKGoto
void IKGoto(double X, double Y, double Z, double Al, double Be, double Ga, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Definition: ikBase.cpp:327

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