kinematics6M90T.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2008 by Neuronics AG *
3  * support@neuronics.ch *
4  ***************************************************************************/
5 
6 #include <kinematics6M90T.h>
7 
8 namespace AnaGuess {
9 
12  initialize();
13 }
16 }
17 
19 std::vector<double> Kinematics6M90T::getLinkLength() {
20  std::vector<double> result(mSegmentLength);
21  return result;
22 }
24 std::vector<int> Kinematics6M90T::getEpc() {
25  std::vector<int> result(mEncodersPerCycle);
26  return result;
27 }
29 std::vector<int> Kinematics6M90T::getEncOff() {
30  std::vector<int> result(mEncoderOffset);
31  return result;
32 }
34 std::vector<int> Kinematics6M90T::getDir() {
35  std::vector<int> result(mRotationDirection);
36  return result;
37 }
39 std::vector<double> Kinematics6M90T::getAngOff() {
40  std::vector<double> result(mAngleOffset);
41  return result;
42 }
44 std::vector<double> Kinematics6M90T::getAngStop() {
45  std::vector<double> result(mAngleStop);
46  return result;
47 }
49 std::vector<double> Kinematics6M90T::getAngRange() {
50  std::vector<double> result;
51  double diff;
52  for (int i = 0; i < 6; i++) {
53  diff = mAngleStop[i] - mAngleOffset[i];
54  if (diff < 0) {
55  result.push_back(-diff);
56  } else {
57  result.push_back(diff);
58  }
59  }
60  return result;
61 }
63 std::vector<double> Kinematics6M90T::getAngMin() {
64  std::vector<double> result;
65  for (int i = 0; i < 6; i++) {
66  if (mAngleStop[i] < mAngleOffset[i]) {
67  result.push_back(mAngleStop[i]);
68  } else {
69  result.push_back(mAngleOffset[i]);
70  }
71  }
72  return result;
73 }
75 std::vector<double> Kinematics6M90T::getAngMax() {
76  std::vector<double> result;
77  for (int i = 0; i < 6; i++) {
78  if (mAngleStop[i] < mAngleOffset[i]) {
79  result.push_back(mAngleOffset[i]);
80  } else {
81  result.push_back(mAngleStop[i]);
82  }
83  }
84  return result;
85 }
86 
88 bool Kinematics6M90T::setLinkLength(const std::vector<double> aLengths) {
89  if ((int) aLengths.size() != mNumberOfSegments) {
90  return false;
91  }
92 
93  for (int i = 0; i < mNumberOfSegments; ++i) {
94  mSegmentLength[i] = aLengths.at(i);
95  }
96 
97  return true;
98 }
100 bool Kinematics6M90T::setAngOff(const std::vector<double> aAngOff) {
101  if ((int) aAngOff.size() != mNumberOfMotors) {
102  return false;
103  }
104 
105  for (int i = 0; i < mNumberOfMotors; ++i) {
106  mAngleOffset[i] = aAngOff.at(i);
107  }
108 
109  return true;
110 }
112 bool Kinematics6M90T::setAngStop(const std::vector<double> aAngStop) {
113  if ((int) aAngStop.size() != mNumberOfMotors) {
114  return false;
115  }
116 
117  for (int i = 0; i < mNumberOfMotors; ++i) {
118  mAngleStop[i] = aAngStop.at(i);
119  }
120 
121  return true;
122 }
123 
125 bool Kinematics6M90T::enc2rad(std::vector<double>& aAngles, const std::vector<int> aEncoders) {
126  for(int i = 0; i < 6; ++i) {
127  aAngles[i] = MHF::enc2rad(aEncoders[i], mAngleOffset[i], mEncodersPerCycle[i], mEncoderOffset[i], mRotationDirection[i]);
128  }
129  return true;
130 }
132 bool Kinematics6M90T::rad2enc(std::vector<int>& aEncoders, const std::vector<double> aAngles) {
133  for(int i = 0; i < 6; ++i ) {
134  aEncoders[i] = MHF::rad2enc(aAngles[i], mAngleOffset[i], mEncodersPerCycle[i], mEncoderOffset[i], mRotationDirection[i]);
135  }
136  return true;
137 }
138 
140 bool Kinematics6M90T::directKinematics(std::vector<double>& aPosition, const std::vector<double> aAngles) {
141  if(!mIsInitialized) {
142  initialize();
143  }
144 
145  // numering the angles starting by 0-5
146 
147  double x0, x1, x2, x3;
148  double y0, y1, y2, y3;
149  double z0, z1, z2, z3;
150 
151  std::vector<double> current_angles(6);
152  for(int i = 0; i < 6; ++i) {
153  current_angles[i] = aAngles[i];
154  }
155 
156  // needs refactoring:
157  current_angles[1] = current_angles[1] - MHF_PI/2.0;
158  current_angles[2] = current_angles[2] - MHF_PI;
159  current_angles[3] = MHF_PI - current_angles[3];
160  current_angles[5] = -current_angles[5];
161 
162  std::vector<double> pose(6);
163 
164  std::vector<double> cx(current_angles.size()), sx(current_angles.size());
165  std::vector<double>::iterator cx_iter, sx_iter;
166 
167  std::vector<double> angle = current_angles;
168 
169  angle[2] = angle[1]+angle[2];
170  angle[3] = angle[2]+angle[3];
171 
172  cx_iter = cx.begin();
173  sx_iter = sx.begin();
174  std::transform(angle.begin(), angle.end(), sx_iter, MHF::unary_precalc_sin<double>() );
175  std::transform(angle.begin(), angle.end(), cx_iter, MHF::unary_precalc_cos<double>() );
176 
177 
178  //x
179  x0 = cx[0]*sx[1];
180  x1 = cx[0]*sx[2];
181  x2 = cx[0]*sx[3];
182  x3 = -cx[0]*cx[3]*cx[4]-sx[0]*sx[4];
183  pose[0] = x0*mSegmentLength[0]+x1*mSegmentLength[1]+x2*mSegmentLength[2]+x3*mSegmentLength[3];
184 
185  //y
186  y0 = sx[0]*sx[1];
187  y1 = sx[0]*sx[2];
188  y2 = sx[0]*sx[3];
189  y3 = -sx[0]*cx[3]*cx[4]+cx[0]*sx[4];
190  pose[1] = y0*mSegmentLength[0]+y1*mSegmentLength[1]+y2*mSegmentLength[2]+y3*mSegmentLength[3];
191 
192  //z
193  z0 = cx[1];
194  z1 = cx[2];
195  z2 = cx[3];
196  z3 = cx[4]*sx[3];
197  pose[2] = z0*mSegmentLength[0]+z1*mSegmentLength[1]+z2*mSegmentLength[2]+z3*mSegmentLength[3];
198 
199 
200  //theta
201  pose[4] = acos(cx[4]*sx[3]);
202 
203  // phi & psi
204 
205 
206  const double theta1 = angle[0];
207  const double theta5 = angle[4];
208  const double theta6 = angle[5];
209  const double theta234 = angle[3];
210 
211  if( std::abs(pose[4])<cTolerance || std::abs(pose[4]-MHF_PI)<cTolerance ) { // catch the case where theta=0, resp. theta=180
212  //phi
213  std::vector<double> v1(2), v2(2);
214 
215  double R11 = -sin(theta1)*cos(theta5) *sin(theta6) + cos(theta1)*(sin(theta234)*cos(theta6)+cos(theta234)*sin(theta5)*sin(theta6));
216  double R21 = sin(theta1)*sin(theta234)*cos(theta6) + sin(theta6)*(cos(theta1) *cos(theta5)+cos(theta234)*sin(theta1)*sin(theta5));
217 
218  v1[0] = acos( R11 );
219  v1[1] = -v1[0];
220  v2[0] = asin( R21 );
221  v2[1] = MHF_PI - v2[0];
222 
223  pose[3] = MHF::anglereduce(findFirstEqualAngle(v1, v2));
224 
225  //psi
226  pose[5] = 0;
227  } else {
228  //phi
229  const double R13 = -cos(theta1)*cos(theta234)*cos(theta5) - sin(theta1)*sin(theta5);
230  const double R23 = -sin(theta1)*cos(theta234)*cos(theta5) + cos(theta1)*sin(theta5);
231  pose[3] = atan2(R13, -R23);
232 
233  //psi
234  const double R31 = cos(theta234)*cos(theta6) - sin(theta234)*sin(theta5)*sin(theta6);
235  const double R32 = -cos(theta234)*sin(theta6) - sin(theta234)*sin(theta5)*cos(theta6);
236  pose[5] = atan2(R31, R32);
237  }
238 
239  std::swap(aPosition, pose);
240  return true;
241 }
243 bool Kinematics6M90T::inverseKinematics(std::vector<double>& aAngles, const std::vector<double> aPosition,
244  const std::vector<double> aStartingAngles) {
245  if(!mIsInitialized) {
246  initialize();
247  }
248  // pose: Winkel Deg->Rad
249  // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
250  // 0-3 fr theta1_1
251  // 4-7 fr theta1_2
252 
253  // Declaration
254  position p_gr;
255  position p_m;
257 
258  // calculation of the gripper vector
259  p_gr.x = mSegmentLength[3]*sin(aPosition[4])*sin(aPosition[3]);
260  p_gr.y = -mSegmentLength[3]*sin(aPosition[4])*cos(aPosition[3]);
261  p_gr.z = mSegmentLength[3]*cos(aPosition[4]);
262 
263  p_m.x = aPosition[0]-p_gr.x;
264  p_m.y = aPosition[1]-p_gr.y;
265  p_m.z = aPosition[2]-p_gr.z;
266 
267  // calculate theta1_1 and theta1_2
268  angle[0].theta1 = MHF::atan1(p_m.x,p_m.y);
269  angle[4].theta1 = angle[0].theta1+MHF_PI;
270 
271 
272  // check the borders according to the settings
273  if(angle[0].theta1>mAngleStop[0])
274  angle[0].theta1=angle[0].theta1-2.0*MHF_PI;
275 
276  if(angle[0].theta1<mAngleOffset[0])
277  angle[0].theta1=angle[0].theta1+2.0*MHF_PI;
278 
279  if(angle[4].theta1>mAngleStop[0])
280  angle[4].theta1=angle[4].theta1-2.0*MHF_PI;
281 
282  if(angle[4].theta1<mAngleOffset[0])
283  angle[4].theta1=angle[4].theta1+2.0*MHF_PI;
284 
285 
286  //====THETA1_1==================
287  //-------THETA234_1-------------
288  IK_theta234theta5(angle[0], p_gr);
289  IK_b1b2costh3_6MS(angle[0], p_m);
290 
291  angle[1]=angle[0];
292  angle[0].theta3 = acos(angle[0].costh3)-MHF_PI;
293  thetacomp(angle[0], p_m, aPosition);
294  angle[1].theta3 = -acos(angle[1].costh3)+MHF_PI;
295  thetacomp(angle[1], p_m, aPosition);
296 
297  //-------THETA234_2-------------
298  angle[2].theta1=angle[0].theta1;
299  angle[2].theta234=angle[0].theta234-MHF_PI;
300  angle[2].theta5=MHF_PI-angle[0].theta5;
301 
302  IK_b1b2costh3_6MS(angle[2], p_m);
303  angle[3]=angle[2];
304  angle[2].theta3 = acos(angle[2].costh3)-MHF_PI;
305  thetacomp(angle[2], p_m, aPosition);
306  angle[3].theta3 = -acos(angle[3].costh3)+MHF_PI;
307  thetacomp(angle[3], p_m, aPosition);
308 
309 
310  //====THETA1_2==================
311  //-------THETA234_1-------------
312  IK_theta234theta5(angle[4], p_gr);
313  IK_b1b2costh3_6MS(angle[4], p_m);
314 
315  angle[5]=angle[4];
316  angle[4].theta3 = acos(angle[4].costh3)-MHF_PI;
317  thetacomp(angle[4], p_m, aPosition);
318  angle[5].theta3 = -acos(angle[5].costh3)+MHF_PI;
319  thetacomp(angle[5], p_m, aPosition);
320 
321  //-------THETA234_2-------------
322  angle[6].theta1=angle[4].theta1;
323  angle[6].theta234=angle[4].theta234-MHF_PI;
324  angle[6].theta5=MHF_PI-angle[4].theta5;
325  IK_b1b2costh3_6MS(angle[6], p_m);
326  angle[7]=angle[6];
327  angle[6].theta3 = acos(angle[6].costh3)-MHF_PI;
328  thetacomp(angle[6], p_m, aPosition);
329  angle[7].theta3 = -acos(angle[7].costh3)+MHF_PI;
330  thetacomp(angle[7], p_m, aPosition);
331 
332  // delete solutions out of range (in joint space)
333  for( std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end();) {
334  if( MHF::pow2(iter->costh3) <= 1.0) {
335  if(!angledef(*iter))
336  iter = angle.erase(iter);
337  else
338  ++iter;
339  continue;
340  }
341  iter = angle.erase(iter);
342  }
343 
344 
345  // check if solutions in range left
346  if(angle.size() == 0) {
347  throw NoSolutionException();
348  }
349 
350  // store possible solution angles to std::vector<std::vector<double>>
351  std::vector< std::vector<double> > PossibleTargets;
352  for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
353  std::vector<double> possangles(6);
354 
355  possangles[0] = i->theta1;
356  possangles[1] = i->theta2;
357  possangles[2] = i->theta3;
358  possangles[3] = i->theta4;
359  possangles[4] = i->theta5;
360  possangles[5] = i->theta6;
361 
362  PossibleTargets.push_back(possangles);
363  }
364 
365  // choose best solution
366  std::vector< std::vector<double> >::const_iterator sol = KinematicsDefaultRadMinAlgorithm()(PossibleTargets.begin(), PossibleTargets.end(), aStartingAngles.begin(), aStartingAngles.end());
367 
368  if(sol == PossibleTargets.end()) {
369  throw NoSolutionException();
370  }
371 
372  // copy solution to aAngles vector
373  for (int i = aAngles.size(); i < 6; ++i)
374  aAngles.push_back(0.0);
375  std::vector<double>::iterator gripper_iter = std::copy( (*sol).begin(), (*sol).end(), aAngles.begin() );
376 
377  return true;
378 }
381  // NOTE: data for Katana6M90A with flange
382 
383  mIsInitialized = false;
384 
385  //fill in segment data
386  mNumberOfSegments = 4;
387 
388  mSegmentLength.push_back(190.0);
389  mSegmentLength.push_back(139.0);
390  mSegmentLength.push_back(147.3);
391  mSegmentLength.push_back(36.0);
392 
393  //fill in joint data
394  mNumberOfMotors = 6;
395 
396  mAngleOffset.push_back(0.116064);
397  mAngleStop.push_back(6.154904);
398  mEncodersPerCycle.push_back(51200);
399  mEncoderOffset.push_back(31000);
400  mRotationDirection.push_back(1);
401 
402  mAngleOffset.push_back(2.168572);
403  mAngleStop.push_back(-0.274889);
404  mEncodersPerCycle.push_back(94976);
405  mEncoderOffset.push_back(-31000);
406  mRotationDirection.push_back(1);
407 
408  mAngleOffset.push_back(0.919789);
409  mAngleStop.push_back(5.283112);
410  mEncodersPerCycle.push_back(47488);
411  mEncoderOffset.push_back(-31000);
412  mRotationDirection.push_back(-1);
413 
414  mAngleOffset.push_back(1.108284);
415  mAngleStop.push_back(5.122541);
416  mEncodersPerCycle.push_back(51200);
417  mEncoderOffset.push_back(31000);
418  mRotationDirection.push_back(1);
419 
420  mAngleOffset.push_back(0.148353); // for 6M90B: -2.99324
421  mAngleStop.push_back(6.117379); // for 6M90B: 2.975787
422  mEncodersPerCycle.push_back(51200);
423  mEncoderOffset.push_back(31000);
424  mRotationDirection.push_back(1);
425 
426  mAngleOffset.push_back(-2.085668);
427  mAngleStop.push_back(3.656465);
428  mEncodersPerCycle.push_back(51200);
429  mEncoderOffset.push_back(31000);
430  mRotationDirection.push_back(1);
431 
432  mIsInitialized = true;
433 
434  return mIsInitialized;
435 }
438  if(p_gr.z==0) {
439  angle.theta234=0;
440  angle.theta5=angle.theta1-MHF::atan1(-p_gr.x,-p_gr.y);
441  } else {
442  angle.theta234 = -MHF::acotan( ( (p_gr.x * p_gr.z * cos(angle.theta1) ) -
443  sqrt( ( -MHF::pow2(p_gr.z) ) *
444  ( -MHF::pow2(mSegmentLength[3]) + MHF::pow2(p_gr.x) + MHF::pow2(p_gr.z) ) * MHF::pow2(sin(angle.theta1))
445  )
446  ) / MHF::pow2(p_gr.z)
447  );
448  angle.theta5 = acos( p_gr.z/(mSegmentLength[3]*sin(angle.theta234)) );
449  }
450 
451  bool griptest;
452  griptest = GripperTest(p_gr, angle);
453  if(!griptest) {
454  angle.theta5=-angle.theta5;
455  griptest=GripperTest(p_gr, angle);
456  if(!griptest) {
457  angle.theta234 = -MHF::acotan( ( ( p_gr.x * p_gr.z * cos(angle.theta1) ) +
458  sqrt( ( -MHF::pow2(p_gr.z) ) *
459  ( -MHF::pow2(mSegmentLength[3]) + MHF::pow2(p_gr.x) + MHF::pow2(p_gr.z) ) * MHF::pow2(sin(angle.theta1))
460  )
461  ) / MHF::pow2(p_gr.z)
462  );
463  angle.theta5 = acos( p_gr.z / (mSegmentLength[3]*sin(angle.theta234)) );
464  if(p_gr.z==0) {
465  angle.theta234=-MHF_PI;
466  angle.theta5=MHF::atan1(p_gr.x,p_gr.y) - angle.theta1;
467  }
468 
469  griptest=GripperTest(p_gr, angle);
470  if(!griptest) {
471  angle.theta5=-angle.theta5;
472  }
473  }
474  }
475 
476 }
478 bool Kinematics6M90T::GripperTest(const position &p_gr, const angles_calc &angle) const {
479  double xgr2, ygr2, zgr2;
480 
481  xgr2 = -mSegmentLength[3]*(cos(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)+sin(angle.theta1)*sin(angle.theta5));
482  ygr2 = -mSegmentLength[3]*(sin(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)-cos(angle.theta1)*sin(angle.theta5));
483  zgr2 = mSegmentLength[3]*sin(angle.theta234)*cos(angle.theta5);
484 
485  if((MHF::pow2(p_gr.x-xgr2)+MHF::pow2(p_gr.y-ygr2)+MHF::pow2(p_gr.z-zgr2))>=cTolerance)
486  return false;
487 
488  return true;
489 }
492  double xg, yg, zg;
493  double d5 = mSegmentLength[2] + mSegmentLength[3];
494  xg = p.x + ( mSegmentLength[3] * cos(angle.theta1) * sin(angle.theta234) );
495  yg = p.y + ( mSegmentLength[3] * sin(angle.theta1) * sin(angle.theta234) );
496  zg = p.z + ( mSegmentLength[3] * cos(angle.theta234) );
497 
498 
499  angle.b1 = xg*cos(angle.theta1) + yg*sin(angle.theta1) - d5*sin(angle.theta234);
500  angle.b2 = zg - d5*cos(angle.theta234);
501  angle.costh3 = -( MHF::pow2(angle.b1) + MHF::pow2(angle.b2) - MHF::pow2(mSegmentLength[0]) - MHF::pow2(mSegmentLength[1]) ) / ( 2.0*mSegmentLength[0]*mSegmentLength[1] );
502 
503 }
505 double Kinematics6M90T::findFirstEqualAngle(const std::vector<double>& v1, const std::vector<double>& v2) const {
506  for(std::vector<double>::const_iterator i = v1.begin(); i != v1.end(); ++i) {
507  for(std::vector<double>::const_iterator j = v2.begin(); j != v2.end(); ++j) {
508  if(std::abs(MHF::anglereduce(*j) - MHF::anglereduce(*i)) < cTolerance)
509  return *i;
510  }
511  }
512  throw Exception("precondition for findFirstEqualAngle failed -> no equal angles found", -2);
513  return 0;
514 }
515 
516 
517 void Kinematics6M90T::thetacomp(angles_calc &angle, const position &p_m, const std::vector<double>& pose) const {
518  const double theta1 = angle.theta1;
519  double theta2 = 0;
520  const double theta3 = angle.theta3;
521  double theta4 = 0;
522  const double theta5 = angle.theta5;
523  double theta6 = 0;
524  const double theta234 = angle.theta234;
525  const double b1 = angle.b1;
526  const double b2 = angle.b2;
527 
528  const double phi = pose[3];
529  const double theta = pose[4];
530  const double psi = pose[5];
531 
532 
533  theta2 = -MHF_PI/2.0 - ( MHF::atan0(b1, b2)+MHF::atan0(mSegmentLength[0]+mSegmentLength[1]*cos(theta3),mSegmentLength[1]*sin(theta3)) );
534  theta4 = theta234 - theta2 - theta3;
535 
536  if(!PositionTest6MS(theta1, theta2, theta3, theta234 ,p_m)) {
537  theta2 = theta2+MHF_PI;
538  theta4 = theta234 - theta2 - theta3;
539  }
540 
541  const double R11 = cos(phi)*cos(psi) - sin(phi)*cos(theta)*sin(psi);
542  const double R21 = sin(phi)*cos(psi) + cos(phi)*cos(theta)*sin(psi);
543 
544  std::vector<double> theta16c(2), theta16s(2);
545 
546  if(std::abs(theta234 + MHF_PI/2) < cTolerance) {
547  if(std::abs(theta5) < cTolerance) {
548  theta16c[0] = acos(-R11);
549  theta16c[1] = -theta16c[0];
550  theta16s[0] = asin(-R21);
551  theta16s[1] = MHF_PI - theta16s[0];
552 
553  theta6 = theta1 - findFirstEqualAngle(theta16c, theta16s);
554 
555  } else if(std::abs(theta5-MHF_PI) < cTolerance) {
556  theta16c[0] = acos(-R11);
557  theta16c[1] = -theta16c[0];
558  theta16s[0] = asin(-R21);
559  theta16s[1] = MHF_PI - theta16s[0];
560 
561  theta6 = findFirstEqualAngle(theta16c, theta16s) - theta1;
562 
563  } else {
564  throw Exception("Special case \"|theta234+(1/2)*pi| = 0\" detected, but no solution found", -1);
565  }
566 
567  } else if(std::abs(theta234 + 3*MHF_PI/2) < cTolerance) {
568  if(std::abs(theta5) < cTolerance) {
569  theta16c[0] = acos(R11);
570  theta16c[1] = -theta16c[0];
571  theta16s[0] = asin(R21);
572  theta16s[1] = MHF_PI - theta16s[0];
573 
574  theta6 = findFirstEqualAngle(theta16c, theta16s) - theta1;
575 
576  } else if(std::abs(theta5-MHF_PI ) < cTolerance) {
577  theta16c[0] = acos(R11);
578  theta16c[1] = -theta16c[0];
579  theta16s[0] = asin(R21);
580  theta16s[1] = MHF_PI -theta16s[0];
581 
582  theta6 = - theta1 - findFirstEqualAngle(theta16c, theta16s);
583  } else {
584  throw Exception("Special case \"|theta234+(3/2)*pi| = 0\" detected, but no solution found", -1);
585  }
586 
587  } else {
588 
589  const double R31 = sin(theta)*sin(psi);
590  const double R32 = sin(theta)*cos(psi);
591 
592  const double temp1 = cos(theta234);
593  const double temp2 = -sin(theta234)*sin(theta5);
594 
595  const double c = ( R31*temp1 + R32*temp2 ) / ( MHF::pow2(temp1) + MHF::pow2(temp2) );
596  const double s = ( R31*temp2 - R32*temp1 ) / ( MHF::pow2(temp1) + MHF::pow2(temp2) );
597 
598  theta16c[0] = acos(c);
599  theta16c[1] = -theta16c[0];
600  theta16s[0] = asin(s);
601  theta16s[1] = MHF_PI - theta16s[0];
602 
603  theta6 = findFirstEqualAngle(theta16c, theta16s);
604  }
605 
606 
607  angle.theta2 = theta2;
608  angle.theta4 = theta4;
609  angle.theta6 = theta6;
610 }
612 bool Kinematics6M90T::PositionTest6MS(const double& theta1, const double& theta2, const double& theta3, const double& theta234, const position &p) const {
613  double temp, xm2, ym2, zm2;
614 
615  temp = mSegmentLength[0]*sin(theta2) + mSegmentLength[1]*sin(theta2+theta3) + mSegmentLength[2]*sin(theta234);
616  xm2 = cos(theta1)*temp;
617  ym2 = sin(theta1)*temp;
618  zm2 = mSegmentLength[0]*cos(theta2) + mSegmentLength[1]*cos(theta2+theta3) + mSegmentLength[2]*cos(theta234);
619 
620  if((MHF::pow2(p.x-xm2)+MHF::pow2(p.y-ym2)+MHF::pow2(p.z-zm2))>=cTolerance)
621  return false;
622 
623  return true;
624 }
627  // constants here. needs refactoring:
632  a.theta6=-a.theta6;
633 
634  if(a.theta1>mAngleStop[0]) {
635  a.theta1=a.theta1-2.0*MHF_PI;
636  }
637  if(a.theta2>MHF_PI) {
638  a.theta2=a.theta2-2.0*MHF_PI;
639  }
640  if(a.theta6<mAngleOffset[5]) {
641  a.theta6=a.theta6+2.0*MHF_PI;
642  } else if(a.theta6>mAngleStop[5]) {
643  a.theta6=a.theta6-2.0*MHF_PI;
644  }
645  if(a.theta5<mAngleOffset[4]) {
646  a.theta5 += 2.0*MHF_PI;
647  }
648 
649  return AnglePositionTest(a);
650 
651 }
654 
655  if( (a.theta1+0.0087<mAngleOffset[0])||(a.theta1>mAngleStop[0]) ) {
656  return false;
657  }
658  if( (a.theta2-0.0087>mAngleOffset[1])||(a.theta2<mAngleStop[1]) ) {
659  return false;
660  }
661  if( (a.theta3<mAngleOffset[2])||(a.theta3>mAngleStop[2]) ) {
662  return false;
663  }
664 
665  if( (a.theta4<mAngleOffset[3])||(a.theta4>mAngleStop[3]) ) {
666  return false;
667  }
668 
669  if( (a.theta5<mAngleOffset[4])||(a.theta5>mAngleStop[4]) ) {
670  return false;
671  }
672  if( (a.theta6<mAngleOffset[5])||(a.theta6>mAngleStop[5]) ) {
673  return false;
674  }
675 
676  return true;
677 }
679 } // namespace
std::vector< double > mAngleStop
Angle stop vector [rad].
bool directKinematics(std::vector< double > &aPosition, const std::vector< double > aAngles)
std::vector< double > getAngMin()
get angle min
std::vector< int > getDir()
get direction
void thetacomp(angles_calc &angle, const position &p_m, const std::vector< double > &pose) const
bool enc2rad(std::vector< double > &aAngles, const std::vector< int > aEncoders)
std::vector< double > getAngStop()
get angle stop
std::vector< double > getLinkLength()
get link length
void swap(Matrix &A, Matrix &B)
Definition: newmat.h:2159
bool PositionTest6MS(const double &theta1, const double &theta2, const double &theta3, const double &theta234, const position &p) const
void IK_theta234theta5(angles_calc &angle, const position &p_gr) const
helper functions
FloatVector * pose
bool mIsInitialized
Initialized flag.
BaseException Exception
Definition: myexcept.h:98
bool AnglePositionTest(const angles_calc &a) const
bool setAngOff(const std::vector< double > aAngOff)
set angle offset
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
std::vector< int > mRotationDirection
Rotation direction vector [1|-1].
bool setLinkLength(const std::vector< double > aLengths)
set link length
int mNumberOfMotors
Number of motors of the robot.
bool rad2enc(std::vector< int > &aEncoders, const std::vector< double > aAngles)
std::vector< angles_calc > angles_container
structs, type and constants used in inverse kinematics calculation
int mNumberOfSegments
Number of segments of the robot.
std::vector< double > getAngMax()
get angle max
FloatVector FloatVector * a
FloatVector * angle
bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)
std::vector< double > mSegmentLength
Effector segment lengths vector [m].
std::vector< double > getAngRange()
get angle range
void IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const
std::vector< int > mEncodersPerCycle
Encoders per cycle vector.
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
std::vector< double > getAngOff()
get angle offset
double findFirstEqualAngle(const std::vector< double > &v1, const std::vector< double > &v2) const
bool setAngStop(const std::vector< double > aAngStop)
set angle stop
bool initialize()
initialization routine
std::vector< int > getEncOff()
get encoder offset
std::vector< int > mEncoderOffset
Encoder offset vector.
std::vector< int > getEpc()
get encoders per cycle
bool GripperTest(const position &p_gr, const angles_calc &angle) const
static const int cNrOfPossibleSolutions
std::vector< double > mAngleOffset
Angle offset vector [rad].
bool angledef(angles_calc &a) const


kni
Author(s): Martin Günther
autogenerated on Fri Jun 7 2019 22:06:44