libKinematics.cpp
Go to the documentation of this file.
1 /* --------------------------------------------------------------------------------
2  #
3  # libKinematics.cpp
4  # Project : Kinematics
5  # author : jhaller
6  # 24.04.2008
7  #
8  # --------------------------------------------------------------------------------*/
9 
10 
11 #include "libKinematics.h"
12 
13 
14 extern "C"{
15 
17 bool LibInstantiated = false;
18 
20 
21 int kin_setType(int type) {
22  if (LibInstantiated == true) delete _kinematics;
23  _kinematics = new KinematicsLib(type);
24  LibInstantiated = true;
25  return 0;
26 }
27 
29  FloatVector* alpha, int typeNr) {
30  if (LibInstantiated == true) delete _kinematics;
31  _kinematics = new KinematicsLib();
32  LibInstantiated = true;
33  std::vector<double> thetaw, dw, aw, alphaw;
34  for (int i = 0; i < theta->length; ++i) {
35  thetaw.push_back(theta->data[i]);
36  dw.push_back(d->data[i]);
37  aw.push_back(a->data[i]);
38  alphaw.push_back(alpha->data[i]);
39  }
40  int ok = _kinematics->setMDH(thetaw, dw, aw, alphaw, typeNr);
41  int error = (ok < 0) ? -1 : 0;
42  return error;
43 }
44 
46  if (!LibInstantiated)
47  return -1;
48  std::vector<double> fw;
49  for (int i = 0; i < links->length; ++i) {
50  fw.push_back(links->data[i]);
51  }
52  int ok = _kinematics->setLinkLen(fw);
53  int error = (ok < 0) ? -1 : 0;
54  return error;
55 }
56 
57 int kin_setImmob(int immobile) {
58  if (!LibInstantiated)
59  return -1;
60  int ok = _kinematics->setImmob(immobile);
61  int error = (ok < 0) ? -1 : 0;
62  return error;
63 }
64 
65 int kin_setEPC(IntVector* epc) {
66  if (!LibInstantiated)
67  return -1;
68  std::vector<int> iw;
69  for (int i = 0; i < epc->length; ++i) {
70  iw.push_back(epc->data[i]);
71  }
72  int ok = _kinematics->setEPC(iw);
73  int error = (ok < 0) ? -1 : 0;
74  return error;
75 }
76 
77 int kin_setEncOff(IntVector* encOffset) {
78  if (!LibInstantiated)
79  return -1;
80  std::vector<int> iw;
81  for (int i = 0; i < encOffset->length; ++i) {
82  iw.push_back(encOffset->data[i]);
83  }
84  int ok = _kinematics->setEncOff(iw);
85  int error = (ok < 0) ? -1 : 0;
86  return error;
87 }
88 
89 int kin_setRotDir(IntVector* rotDir) {
90  if (!LibInstantiated)
91  return -1;
92  std::vector<int> iw;
93  for (int i = 0; i < rotDir->length; ++i) {
94  iw.push_back(rotDir->data[i]);
95  }
96  int ok = _kinematics->setRotDir(iw);
97  int error = (ok < 0) ? -1 : 0;
98  return error;
99 }
100 
101 int kin_setAngOff(FloatVector* angleOffset) {
102  if (!LibInstantiated)
103  return -1;
104  std::vector<double> fw;
105  for (int i = 0; i < angleOffset->length; ++i) {
106  fw.push_back(angleOffset->data[i]);
107  }
108  int ok = _kinematics->setAngOff(fw);
109  int error = (ok < 0) ? -1 : 0;
110  return error;
111 }
112 
113 int kin_setAngRan(FloatVector* angleRange) {
114  if (!LibInstantiated)
115  return -1;
116  // check vector size
117  std::vector<double> fw;
118  for (int i = 0; i < angleRange->length; ++i) {
119  fw.push_back(angleRange->data[i]);
120  }
121  int ok = _kinematics->setAngRan(fw);
122  int error = (ok < 0) ? -1 : 0;
123  return error;
124 }
125 
126 int kin_setTcpOff(FloatVector* tcpOffset) {
127  if (!LibInstantiated)
128  return -1;
129  std::vector<double> fw;
130  for (int i = 0; i < tcpOffset->length; ++i) {
131  fw.push_back(tcpOffset->data[i]);
132  }
133  int ok = _kinematics->setTcpOff(fw);
134  int error = (ok < 0) ? -1 : 0;
135  return error;
136 }
137 
139 
140 int kin_getType() {
141  if (!LibInstantiated)
142  return -1;
143  return _kinematics->getType();
144 }
145 
147  if (!LibInstantiated)
148  return -1;
149  return _kinematics->getMaxDOF();
150 }
151 
152 int kin_getDOF() {
153  if (!LibInstantiated)
154  return -1;
155  return _kinematics->getDOF();
156 }
157 
158 int kin_getDOM() {
159  if (!LibInstantiated)
160  return -1;
161  return _kinematics->getDOM();
162 }
163 
166  if (!LibInstantiated)
167  return -1;
168  std::vector<double> thetaw, dw, aw, alphaw;
169  int ok = _kinematics->getMDH(thetaw, dw, aw, alphaw);
170  for (int i = 0; i < (int)thetaw.size(); ++i) {
171  theta->data[i] = (float)thetaw.at(i);
172  d->data[i] = (float)dw.at(i);
173  a->data[i] = (float)aw.at(i);
174  alpha->data[i] = (float)alphaw.at(i);
175  }
176  theta->length = (int)thetaw.size();
177  d->length = (int)thetaw.size();
178  a->length = (int)thetaw.size();
179  alpha->length = (int)thetaw.size();
180  int error = (ok < 0) ? -1 : 0;
181  return error;
182 }
183 
185  if (!LibInstantiated)
186  return -1;
187  return _kinematics->getImmob();
188 }
189 
191  if (!LibInstantiated)
192  return -1;
193  std::vector<int> iw;
194  int ok = _kinematics->getEPC(iw);
195  for (int i = 0; i < (int)iw.size(); ++i) {
196  epc->data[i] = iw.at(i);
197  }
198  epc->length = (int)iw.size();
199  if ((int)iw.size() == 5) {
200  epc->data[5] = 51200;
201  epc->length++;
202  }
203  int error = (ok < 0) ? -1 : 0;
204  return error;
205 }
206 
207 int kin_getEncOff(IntVector* encOffset) {
208  if (!LibInstantiated)
209  return -1;
210  std::vector<int> iw;
211  int ok = _kinematics->getEncOff(iw);
212  for (int i = 0; i < (int)iw.size(); ++i) {
213  encOffset->data[i] = iw.at(i);
214  }
215  encOffset->length = iw.size();
216  if ((int)iw.size() == 5) {
217  encOffset->data[5] = 31000;
218  encOffset->length++;
219  }
220  int error = (ok < 0) ? -1 : 0;
221  return error;
222 }
223 
224 int kin_getRotDir(IntVector* rotDir) {
225  if (!LibInstantiated)
226  return -1;
227  std::vector<int> iw;
228  int ok = _kinematics->getRotDir(iw);
229  for (int i = 0; i < (int)iw.size(); ++i) {
230  rotDir->data[i] = iw.at(i);
231  }
232  rotDir->length = iw.size();
233  if ((int)iw.size() == 5) {
234  rotDir->data[5] = 1;
235  rotDir->length++;
236  }
237  int error = (ok < 0) ? -1 : 0;
238  return error;
239 }
240 
241 int kin_getAngOff(FloatVector* angleOffset) {
242  if (!LibInstantiated)
243  return -1;
244  std::vector<double> fw;
245  int ok = _kinematics->getAngOff(fw);
246  for (int i = 0; i < (int)fw.size(); ++i) {
247  angleOffset->data[i] = (float)fw.at(i);
248  }
249  angleOffset->length = fw.size();
250  if ((int)fw.size() == 5) {
251  angleOffset->data[5] = 0.0;
252  angleOffset->length++;
253  }
254  int error = (ok < 0) ? -1 : 0;
255  return error;
256 }
257 
258 int kin_getAngRan(FloatVector* angleRange) {
259  if (!LibInstantiated)
260  return -1;
261  std::vector<double> fw;
262  int ok = _kinematics->getAngRan(fw);
263  for (int i = 0; i < (int)fw.size(); ++i) {
264  angleRange->data[i] = (float)fw.at(i);
265  }
266  angleRange->length = fw.size();
267  if ((int)fw.size() == 5) {
268  angleRange->data[5] = 0.0;
269  angleRange->length++;
270  }
271  int error = (ok < 0) ? -1 : 0;
272  return error;
273 }
274 
275 int kin_getTcpOff(FloatVector* tcpOffset) {
276  if (!LibInstantiated)
277  return -1;
278  std::vector<double> fw;
279  int ok = _kinematics->getTcpOff(fw);
280  for (int i = 0; i < (int)fw.size(); ++i) {
281  tcpOffset->data[i] = (float)fw.at(i);
282  }
283  tcpOffset->length = fw.size();
284  int error = (ok < 0) ? -1 : 0;
285  return error;
286 }
287 
288 int kin_getVersion(IntVector* version) {
289  int error;
290  if (LibInstantiated) {
291  std::vector<int> iw;
292  int ok = _kinematics->getVersion(iw);
293  for (int i = 0; i < (int)iw.size(); ++i) {
294  version->data[i] = iw.at(i);
295  }
296  version->length = iw.size();
297  error = (ok < 0) ? -1 : 0;
298  } else {
299  version->data[0] = KINLIB_VERSION_MAJOR;
300  version->data[1] = KINLIB_VERSION_MINOR;
301  version->data[2] = KINLIB_VERSION_REVISION;
302  version->length = 3;
303  error = 0;
304  }
305  return error;
306 }
307 
309 
310 int kin_init() {
311  if (!LibInstantiated)
312  return -1;
313  int ok = _kinematics->init();
314  int error = (ok < 0) ? -1 : 0;
315  return error;
316 }
317 
319 
320 int kin_clean() {
321  if (LibInstantiated == true)
322  delete _kinematics;
323  LibInstantiated = false;
324  return 0;
325 }
326 
328 
330  if (!LibInstantiated)
331  return -1;
332  std::vector<double> k4d, mdh;
333  for (int i = 0; i < angleK4D->length; ++i) {
334  k4d.push_back(angleK4D->data[i]);
335  }
336  int ok = _kinematics->K4D2mDHAng(k4d, mdh);
337  for (int i = 0; i < (int)mdh.size(); ++i) {
338  angleMDH->data[i] = (float)mdh.at(i);
339  }
340  angleMDH->length = mdh.size();
341  int error = (ok < 0) ? -1 : 0;
342  return error;
343 }
344 
346  if (!LibInstantiated)
347  return -1;
348  std::vector<double> mdh, k4d;
349  for (int i = 0; i < angleMDH->length; ++i) {
350  mdh.push_back(angleMDH->data[i]);
351  }
352  int ok = _kinematics->mDH2K4DAng(mdh, k4d);
353  for (int i = 0; i < (int)k4d.size(); ++i) {
354  angleK4D->data[i] = (float)k4d.at(i);
355  }
356  angleK4D->length = k4d.size();
357  int error = (ok < 0) ? -1 : 0;
358  return error;
359 }
360 
362  if (!LibInstantiated)
363  return -1;
364  std::vector<int> iw;
365  for (int i = 0; i < enc->length; ++i) {
366  iw.push_back(enc->data[i]);
367  }
368  std::vector<double> fw;
369  int ok = _kinematics->enc2rad(iw, fw);
370  for (int i = 0; i < (int)fw.size(); ++i) {
371  angle->data[i] = (float)fw.at(i);
372  }
373  angle->length = fw.size();
374  int error = (ok < 0) ? -1 : 0;
375  return error;
376 }
377 
379  if (!LibInstantiated)
380  return -1;
381  std::vector<double> fw;
382  for (int i = 0; i < angle->length; ++i) {
383  fw.push_back(angle->data[i]);
384  }
385  std::vector<int> iw;
386  int ok = _kinematics->rad2enc(fw, iw);
387  for (int i = 0; i < (int)iw.size(); ++i) {
388  enc->data[i] = iw.at(i);
389  }
390  enc->length = iw.size();
391  int error = (ok < 0) ? -1 : 0;
392  return error;
393 }
394 
396  if (!LibInstantiated)
397  return -1;
398  std::vector<double> aw, pw;
399  for (int i = 0; i < angle->length; ++i) {
400  aw.push_back(angle->data[i]);
401  }
402  int ok = _kinematics->directKinematics(aw, pw);
403  for (int i = 0; i < (int)pw.size(); ++i) {
404  pose->data[i] = (float)pw.at(i);
405  }
406  pose->length = pw.size();
407  int error = (ok < 0) ? -1 : 0;
408  return error;
409 }
410 
412  int maxBisection) {
413  if (!LibInstantiated)
414  return -1;
415  std::vector<double> pw;
416  for (int i = 0; i < pose->length; ++i) {
417  pw.push_back(pose->data[i]);
418  }
419  std::vector<double> paw;
420  for (int i = 0; i < prev->length; ++i) {
421  paw.push_back(prev->data[i]);
422  }
423  std::vector<double> aw;
424  int ok = _kinematics->inverseKinematics(pw, paw, aw, maxBisection);
425  for (int i = 0; i < (int)aw.size(); ++i) {
426  angle->data[i] = (float)aw.at(i);
427  }
428  angle->length = aw.size();
429  int error = (ok < 0) ? -1 : 0;
430  return error;
431 }
432 
434 } // end extern "C"
435 
436 
437 
int kin_getMaxDOF()
FloatVector FloatVector FloatVector int typeNr
FloatVector * prev
Kinematics class using the kinematics lib.
Definition: kinematics.h:178
int kin_K4D2mDHAng(FloatVector *angleK4D, FloatVector *angleMDH)
int kin_IK(FloatVector *pose, FloatVector *prev, FloatVector *angle, int maxBisection)
int kin_setAngRan(FloatVector *angleRange)
int setEncOff(std::vector< int > encOffset)
This sets the encoder offsets.
Definition: kinematics.cpp:422
int getImmob()
Get the immobile flag of the last joint.
Definition: kinematics.cpp:607
int setAngRan(std::vector< double > angleRange)
This sets the angle range.
Definition: kinematics.cpp:510
int kin_init()
int kin_getDOM()
int kin_getRotDir(IntVector *rotDir)
int kin_setEncOff(IntVector *encOffset)
int kin_getVersion(IntVector *version)
int rad2enc(std::vector< double > angles, std::vector< int > &encoders)
Converts angles to encoders.
Definition: kinematics.cpp:826
int kin_setType(int type)
int setRotDir(std::vector< int > rotDir)
This sets the rotation direction.
Definition: kinematics.cpp:434
int data[MaxDof]
int kin_setMDH(FloatVector *theta, FloatVector *d, FloatVector *a, FloatVector *alpha, int typeNr)
int kin_setLinkLen(FloatVector *links)
int kin_getType()
int getAngRan(std::vector< double > &angleRange)
Get the angle range.
Definition: kinematics.cpp:655
FloatVector * pose
int kin_getImmob()
FloatVector * angleK4D
bool LibInstantiated
int getEncOff(std::vector< int > &encOffset)
Get the encoder offsets.
Definition: kinematics.cpp:622
int kin_clean()
int kin_DK(FloatVector *angle, FloatVector *pose)
int setMDH(std::vector< double > theta, std::vector< double > d, std::vector< double > a, std::vector< double > alpha, int typeNr=-1)
This sets the modified Denavit-Hartenberg parameters.
Definition: kinematics.cpp:323
FloatVector * d
KinematicsLib * _kinematics
int getAngOff(std::vector< double > &angleOffset)
Get the angle offsets.
Definition: kinematics.cpp:644
int getDOF()
Get the degree of freedom.
Definition: kinematics.cpp:581
FloatVector * angleMDH
int kin_mDH2K4DAng(FloatVector *angleMDH, FloatVector *angleK4D)
#define KINLIB_VERSION_MINOR
Definition: kinematics.h:31
int kin_getAngRan(FloatVector *angleRange)
FloatVector FloatVector int maxBisection
int getDOM()
Get the degree of mobility.
Definition: kinematics.cpp:585
int kin_getEPC(IntVector *epc)
int kin_setEPC(IntVector *epc)
#define KINLIB_VERSION_MAJOR
Definition: kinematics.h:30
int init()
This initializes the kinematics.
Definition: kinematics.cpp:730
int kin_getAngOff(FloatVector *angleOffset)
int setAngOff(std::vector< double > angleOffset)
This sets the angle offsets.
Definition: kinematics.cpp:450
int kin_setRotDir(IntVector *rotDir)
int enc[10]
int mDH2K4DAng(std::vector< double > angleMDH, std::vector< double > &angleK4D)
This converts angles from mDH to Katana4D convention.
Definition: kinematics.cpp:776
int getType()
Get the robot type.
Definition: kinematics.cpp:573
FloatVector FloatVector * a
FloatVector * angle
int kin_setTcpOff(FloatVector *tcpOffset)
int kin_setImmob(int immobile)
int kin_getDOF()
int setImmob(int immobile)
This sets the immobile flag of the last joint.
Definition: kinematics.cpp:393
int kin_enc2rad(IntVector *enc, FloatVector *angle)
int kin_rad2enc(FloatVector *angle, IntVector *enc)
int getTcpOff(std::vector< double > &tcpOffset)
Get the tcp offset.
Definition: kinematics.cpp:710
int kin_setAngOff(FloatVector *angleOffset)
FloatVector FloatVector FloatVector * alpha
int inverseKinematics(std::vector< double > pose, std::vector< double > prev, std::vector< double > &angles, int maxBisection=0)
Calculates the inverse kinematics.
int setLinkLen(std::vector< double > links)
This sets the link length parameters (part of mDH-parameters)
Definition: kinematics.cpp:355
int getVersion(std::vector< int > &version)
Get the version number of the library.
Definition: kinematics.cpp:721
int setTcpOff(std::vector< double > tcpOffset)
This sets the tcp offset.
Definition: kinematics.cpp:561
int getEPC(std::vector< int > &epc)
Get the encoders per cycle.
Definition: kinematics.cpp:611
int K4D2mDHAng(std::vector< double > angleK4D, std::vector< double > &angleMDH)
This converts angles from Katana4D to mDH convention.
Definition: kinematics.cpp:740
int kin_getEncOff(IntVector *encOffset)
int directKinematics(std::vector< double > angles, std::vector< double > &pose)
Calculates the direct kinematics.
Definition: kinematics.cpp:840
float data[MaxDof]
int getRotDir(std::vector< int > &rotDir)
Get the rotation direction.
Definition: kinematics.cpp:633
int enc2rad(std::vector< int > encoders, std::vector< double > &angles)
Converts encoders to angles.
Definition: kinematics.cpp:812
#define KINLIB_VERSION_REVISION
Definition: kinematics.h:32
int kin_getMDH(FloatVector *theta, FloatVector *d, FloatVector *a, FloatVector *alpha)
int getMaxDOF()
Get the maximum degree of freedom.
Definition: kinematics.cpp:577
int setEPC(std::vector< int > epc)
This sets the encoders per cycle.
Definition: kinematics.cpp:410
double theta
Definition: kni_wrapper.h:53
int kin_getTcpOff(FloatVector *tcpOffset)
int getMDH(std::vector< double > &theta, std::vector< double > &d, std::vector< double > &a, std::vector< double > &alpha)
Get the modified Denavit-Hartenberg parameters.
Definition: kinematics.cpp:589


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