kinematics6M180.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2008 by Neuronics AG *
3  * support@neuronics.ch *
4  ***************************************************************************/
5 
6 #include <kinematics6M180.h>
7 //#include <iostream>
8 namespace AnaGuess {
9 
12  initialize();
13 }
16 }
17 
19 std::vector<double> Kinematics6M180::getLinkLength() {
20  std::vector<double> result(mSegmentLength);
21  return result;
22 }
24 std::vector<int> Kinematics6M180::getEpc() {
25  std::vector<int> result(mEncodersPerCycle);
26  return result;
27 }
29 std::vector<int> Kinematics6M180::getEncOff() {
30  std::vector<int> result(mEncoderOffset);
31  return result;
32 }
34 std::vector<int> Kinematics6M180::getDir() {
35  std::vector<int> result(mRotationDirection);
36  return result;
37 }
39 std::vector<double> Kinematics6M180::getAngOff() {
40  std::vector<double> result(mAngleOffset);
41  return result;
42 }
44 std::vector<double> Kinematics6M180::getAngStop() {
45  std::vector<double> result(mAngleStop);
46  return result;
47 }
49 std::vector<double> Kinematics6M180::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> Kinematics6M180::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> Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::directKinematics(std::vector<double>& aPosition, const std::vector<double> aAngles) {
141  if(!mIsInitialized) {
142  initialize();
143  }
144 
145  // copy aAngles to currentAngles
146  std::vector<double> currentAngles(6);
147  for(int i = 0; i < 6; ++i) {
148  currentAngles[i] = aAngles[i];
149  }
150 
151  // adjust angles (needs refactoring)
152  currentAngles[1] = currentAngles[1] - MHF_PI/2.0;
153  currentAngles[2] = currentAngles[2] - MHF_PI;
154  currentAngles[3] = MHF_PI - currentAngles[3];
155  currentAngles[4] = -currentAngles[4];
156 
157  double factor;
158  double r13, r23, r31, r32;
159 
160  std::vector<double> pose(6);
161 
162  std::vector<double> cx(currentAngles.size()), sx(currentAngles.size());
163  std::vector<double>::iterator cx_iter, sx_iter;
164 
165  std::vector<double> angle = currentAngles;
166 
167  angle[2] = angle[1]+angle[2];
168  angle[3] = angle[2]+angle[3];
169 
170  cx_iter = cx.begin();
171  sx_iter = sx.begin();
172  std::transform(angle.begin(), angle.end(), sx_iter, MHF::unary_precalc_sin<double>() );
173  std::transform(angle.begin(), angle.end(), cx_iter, MHF::unary_precalc_cos<double>() );
174 
175  factor = (mSegmentLength[0]*sx[1]+mSegmentLength[1]*sx[2]+(mSegmentLength[2]+mSegmentLength[3])*sx[3]);
176  // x = px (compare homogenous transformation matrix)
177  pose[0] = cx[0]*factor;
178 
179  // y = py (compare homogenous transformation matrix)
180  pose[1] = sx[0]*factor;
181 
182  // z = pz (compare homogenous transformation matrix)
183  pose[2] = mSegmentLength[0]*cx[1]+mSegmentLength[1]*cx[2]+(mSegmentLength[2]+mSegmentLength[3])*cx[3];
184 
185  // phi = atan2(r13/-r23) (compare homogenous transformation matrix)
186  r13 = cx[0]*sx[3];
187  r23 = sx[0]*sx[3];
188  pose[3] = atan2(r13,-r23);
189 
190  // theta = acos(r33) (compare homogenous transformation matrix)
191  pose[4] = acos(cx[3]);
192 
193  // psi = atan2(r31/r32) (compare homogenous transformation matrix)
194  r31 = sx[3]*sx[4];
195  r32 = sx[3]*cx[4];
196  pose[5] = atan2(r31,r32);
197 
198  std::swap(aPosition, pose);
199  return true;
200 }
202 bool Kinematics6M180::inverseKinematics(std::vector<double>& aAngles, const std::vector<double> aPosition,
203  const std::vector<double> aStartingAngles) {
204  if(!mIsInitialized) {
205  initialize();
206  }
207 
208  // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
209  // 0-3 fuer theta1_1
210  // 4-7 fuer theta1_2
211 
212  // Declarations
213  position p_m;
215  double coeff1, coeff2, theta234;
216  double costh5, sinth5, theta5[2];
217  double R11, R21, R31, R32;
218  double phi, theta, psi;
219 
220  // Initialization
221  p_m.x = aPosition[0];
222  p_m.y = aPosition[1];
223  p_m.z = aPosition[2];
224 
225  // calculate theta1_1 and theta1_2
226  angle[0].theta1 = MHF::atan1(aPosition[0],aPosition[1]);
227  if (angle[0].theta1 > MHF_PI) {
228  angle[0].theta1 = angle[0].theta1 - MHF_PI;
229  if (angle[0].theta1 > (179.91/180*MHF_PI)) {
230  angle[0].theta1 = angle[0].theta1 - MHF_PI;
231  }
232  }
233  angle[4].theta1 = angle[0].theta1+MHF_PI;
234 
235  theta = aPosition[4];
236  psi = aPosition[5];
237  phi = MHF::atan1(p_m.x,p_m.y)+MHF_PI/2.0;
238  theta234 = aPosition[4];
239 
240  R11 = cos(phi)*cos(psi)-sin(phi)*cos(theta)*sin(psi);
241  R21 = sin(phi)*cos(psi)+cos(phi)*cos(theta)*sin(psi);
242  R31 = sin(theta)*sin(psi);
243  R32 = sin(theta)*cos(psi);
244 
245  // calculate theta5
246  if(theta234==0) {
247  //std::cout << "Warning: Singularity theta234=0 !" << std::endl;
248  for (int i=0; i<2; ++i) {
249  coeff1 = -sin(angle[i*4].theta1);
250  coeff2 = -cos(angle[i*4].theta1);
251  costh5 = coeff1*R11-coeff2*R21;
252  sinth5 = coeff1*R21+coeff2*R11;
253  theta5[i] = -MHF::findFirstEqualAngle(costh5, sinth5, cTolerance);
254  }
255  for (int i=0; i<cNrOfPossibleSolutions; ++i) {
256  if(i<4)
257  angle[i].theta5 = theta5[0];
258  else
259  angle[i].theta5 = theta5[1];
260  }
261  } else if(theta234==MHF_PI) {
262  //std::cout << "Warning: Singularity theta234=PI !" << std::endl;
263  for (int i=0; i<2; ++i) {
264  coeff1 = -sin(angle[i*4].theta1);
265  coeff2 = cos(angle[i*4].theta1);
266  costh5 = coeff1*R11+coeff2*R21;
267  sinth5 = -coeff1*R21+coeff2*R11;
268  theta5[i] = -MHF::findFirstEqualAngle(costh5, sinth5, cTolerance);
269  }
270  for (int i=0; i<cNrOfPossibleSolutions; ++i) {
271  if(i<4)
272  angle[i].theta5 = theta5[0];
273  else
274  angle[i].theta5 = theta5[1];
275  }
276  } else {
277  theta5[0] = -atan2(R31/sin(theta234),R32/sin(theta234));
278  theta5[1] = -atan2(R31/sin(-theta234),R32/sin(-theta234));
279  for (int i=0; i<cNrOfPossibleSolutions; ++i) {
280  if(i%4==0 || i%4==1)
281  angle[i].theta5 = theta5[0];
282  else
283  angle[i].theta5 = theta5[1];
284  }
285  }
286 
287  //====THETA1_1==================
288  //-------THETA234_1-------------
289  angle[0].theta234 = aPosition[4];
290  //angle[0].theta5 = pose[5];
291  IK_b1b2costh3_6M180(angle[0], p_m);
292 
293  angle[1] =angle[0];
294  angle[0].theta3 = acos(angle[0].costh3)-MHF_PI;
295  thetacomp(angle[0], p_m);
296  angle[1].theta3 = -acos(angle[1].costh3)+MHF_PI;
297  thetacomp(angle[1], p_m);
298 
299  //-------THETA234_2-------------
300  angle[2].theta1 = angle[0].theta1;
301  angle[2].theta234 = -angle[0].theta234;
302  //angle[2].theta5 = angle[0].theta5;
303  IK_b1b2costh3_6M180(angle[2], p_m);
304 
305  angle[3] = angle[2];
306  angle[2].theta3 = acos(angle[2].costh3)-MHF_PI;
307  thetacomp(angle[2], p_m);
308  angle[3].theta3 = -acos(angle[3].costh3)+MHF_PI;
309  thetacomp(angle[3], p_m);
310 
311  //====THETA1_2==================
312  //-------THETA234_1-------------
313  angle[4].theta234 = aPosition[4];
314  //angle[4].theta5 = pose[5];
315  IK_b1b2costh3_6M180(angle[4], p_m);
316 
317  angle[5] = angle[4];
318  angle[4].theta3 = acos(angle[4].costh3)-MHF_PI;
319  thetacomp(angle[4], p_m);
320  angle[5].theta3 = -acos(angle[5].costh3)+MHF_PI;
321  thetacomp(angle[5], p_m);
322 
323  //-------THETA234_2-------------
324  angle[6].theta1 = angle[4].theta1;
325  angle[6].theta234 = -angle[4].theta234;
326  //angle[6].theta5 = angle[4].theta5;
327  IK_b1b2costh3_6M180(angle[6], p_m);
328 
329  angle[7] = angle[6];
330  angle[6].theta3 = acos(angle[6].costh3)-MHF_PI;
331  thetacomp(angle[6], p_m);
332  angle[7].theta3 = -acos(angle[7].costh3)+MHF_PI;
333  thetacomp(angle[7], p_m);
334 
335  // delete solutions out of range (in joint space)
336  for( std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end();) {
337  if( MHF::pow2(iter->costh3) <= 1.0) {
338  if(!angledef(*iter))
339  iter = angle.erase(iter);
340  else
341  ++iter;
342  continue;
343  }
344  iter = angle.erase(iter);
345  }
346 
347  // check if solutions in range left
348  if(angle.size() == 0) {
349  throw NoSolutionException();
350  }
351 
352  // store possible solution angles to std::vector<std::vector<double>>
353  std::vector< std::vector<double> > PossibleTargets;
354  for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
355  std::vector<double> possangles(5);
356 
357  possangles[0] = i->theta1;
358  possangles[1] = i->theta2;
359  possangles[2] = i->theta3;
360  possangles[3] = i->theta4;
361  possangles[4] = i->theta5;
362 
363  PossibleTargets.push_back(possangles);
364  }
365 
366  // choose best solution
367  std::vector< std::vector<double> >::const_iterator sol = KinematicsDefaultRadMinAlgorithm()(PossibleTargets.begin(), PossibleTargets.end(), aStartingAngles.begin(), aStartingAngles.end());
368 
369  if(sol == PossibleTargets.end()) {
370  throw NoSolutionException();
371  }
372 
373  // copy solution to aAngles vector
374  for (int i = aAngles.size(); i < 6; ++i)
375  aAngles.push_back(0.0);
376  std::vector<double>::iterator gripper_iter = std::copy( (*sol).begin(), (*sol).end(), aAngles.begin() );
377  *gripper_iter = aStartingAngles[5]; // copy gripper-angle from current
378 
379  return true;
380 }
383  // NOTE: data for Katana6M180 with flange
384 
385  mIsInitialized = false;
386 
387  //fill in segment data
388  mNumberOfSegments = 4;
389 
390  mSegmentLength.push_back(190.0);
391  mSegmentLength.push_back(139.0);
392  mSegmentLength.push_back(147.3);
393  mSegmentLength.push_back(41.0); // for anglegripper: 155.5
394 
395  //fill in joint data
396  mNumberOfMotors = 6;
397 
398  mAngleOffset.push_back(0.116064);
399  mAngleStop.push_back(6.154904);
400  mEncodersPerCycle.push_back(51200);
401  mEncoderOffset.push_back(31000);
402  mRotationDirection.push_back(1);
403 
404  mAngleOffset.push_back(2.168572);
405  mAngleStop.push_back(-0.274889);
406  mEncodersPerCycle.push_back(94976);
407  mEncoderOffset.push_back(-31000);
408  mRotationDirection.push_back(1);
409 
410  mAngleOffset.push_back(0.919789);
411  mAngleStop.push_back(5.283112);
412  mEncodersPerCycle.push_back(47488);
413  mEncoderOffset.push_back(-31000);
414  mRotationDirection.push_back(-1);
415 
416  mAngleOffset.push_back(1.108284);
417  mAngleStop.push_back(5.122541);
418  mEncodersPerCycle.push_back(51200);
419  mEncoderOffset.push_back(31000);
420  mRotationDirection.push_back(1);
421 
422  mAngleOffset.push_back(0.148353);
423  mAngleStop.push_back(6.117379);
424  mEncodersPerCycle.push_back(51200);
425  mEncoderOffset.push_back(31000);
426  mRotationDirection.push_back(1);
427 
428  mAngleOffset.push_back(-2.085668);
429  mAngleStop.push_back(3.656465);
430  mEncodersPerCycle.push_back(51200);
431  mEncoderOffset.push_back(31000);
432  mRotationDirection.push_back(1);
433 
434  mIsInitialized = true;
435 
436  return mIsInitialized;
437 }
440  double d5 = mSegmentLength[2] + mSegmentLength[3];
441 
442  angle.b1 = p.x*cos(angle.theta1) + p.y*sin(angle.theta1) - d5*sin(angle.theta234);
443  angle.b2 = p.z - d5*cos(angle.theta234);
444  angle.costh3 = -( MHF::pow2(angle.b1) + MHF::pow2(angle.b2) - MHF::pow2(mSegmentLength[0]) - MHF::pow2(mSegmentLength[1]) ) / ( 2.0*mSegmentLength[0]*mSegmentLength[1] );
445 
446 }
449  angle.theta2 = -MHF_PI/2.0 - ( MHF::atan0(angle.b1,angle.b2)+MHF::atan0(mSegmentLength[0]+mSegmentLength[1]*cos(angle.theta3),mSegmentLength[1]*sin(angle.theta3)) );
450  angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
451 
452  if(!PositionTest6M180(angle,p_m)) {
453  angle.theta2 = angle.theta2+MHF_PI;
454  angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
455  }
456 
457 }
460  double temp, xm2, ym2, zm2;
461 
462  temp = mSegmentLength[0]*sin(a.theta2)+mSegmentLength[1]*sin(a.theta2+a.theta3)+(mSegmentLength[2]+mSegmentLength[3])*sin(a.theta234);
463  xm2 = cos(a.theta1)*temp;
464  ym2 = sin(a.theta1)*temp;
465  zm2 = mSegmentLength[0]*cos(a.theta2)+mSegmentLength[1]*cos(a.theta2+a.theta3)+(mSegmentLength[2]+mSegmentLength[3])*cos(a.theta234);
466 
467  if((MHF::pow2(p.x-xm2)+MHF::pow2(p.y-ym2)+MHF::pow2(p.z-zm2))>=cTolerance)
468  return false;
469 
470  return true;
471 }
474  // constants here. needs refactoring:
479 
480  if(a.theta1>mAngleStop[0]) {
481  a.theta1=a.theta1-2.0*MHF_PI;
482  }
483  if(a.theta2>MHF_PI) {
484  a.theta2=a.theta2-2.0*MHF_PI;
485  }
486  if(a.theta5<mAngleOffset[4]) {
487  a.theta5=a.theta5+2.0*MHF_PI;
488  }
489 
490  return AnglePositionTest(a);
491 
492 }
495 
496  if( (a.theta1+0.0087<mAngleOffset[0])||(a.theta1>mAngleStop[0]) )
497  return false;
498 
499  if( (a.theta2-0.0087>mAngleOffset[1])||(a.theta2<mAngleStop[1]) )
500  return false;
501 
502  if( (a.theta3<mAngleOffset[2])||(a.theta3>mAngleStop[2]) )
503  return false;
504 
505  if( (a.theta4<mAngleOffset[3])||(a.theta4>mAngleStop[3]) )
506  return false;
507 
508  if( (a.theta5<mAngleOffset[4])||(a.theta5>mAngleStop[4]) )
509  return false;
510 
511  return true;
512 }
514 
515 } // namespace
bool enc2rad(std::vector< double > &aAngles, const std::vector< int > aEncoders)
std::vector< int > mEncodersPerCycle
Encoders per cycle vector.
int mNumberOfSegments
Number of segments of the robot.
void IK_b1b2costh3_6M180(angles_calc &a, const position &p) const
helper functions
bool directKinematics(std::vector< double > &aPosition, const std::vector< double > aAngles)
std::vector< int > getDir()
get direction
std::vector< int > mRotationDirection
Rotation direction vector [1|-1].
void swap(Matrix &A, Matrix &B)
Definition: newmat.h:2159
bool setAngOff(const std::vector< double > aAngOff)
set angle offset
double findFirstEqualAngle(double cosValue, double sinValue, double tolerance)
std::vector< double > mSegmentLength
Effector segment lengths vector [m].
std::vector< int > getEncOff()
get encoder offset
static const int cNrOfPossibleSolutions
FloatVector * pose
std::vector< int > getEpc()
get encoders per cycle
std::vector< angles_calc > angles_container
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
std::vector< double > getLinkLength()
get link length
std::vector< double > getAngRange()
get angle range
void thetacomp(angles_calc &a, const position &p_m) const
std::vector< double > mAngleStop
Angle stop vector [rad].
bool PositionTest6M180(const angles_calc &a, const position &p) const
bool setAngStop(const std::vector< double > aAngStop)
set angle stop
std::vector< double > getAngMin()
get angle min
std::vector< double > getAngMax()
get angle max
bool rad2enc(std::vector< int > &aEncoders, const std::vector< double > aAngles)
FloatVector FloatVector * a
FloatVector * angle
std::vector< double > getAngStop()
get angle stop
bool setLinkLength(const std::vector< double > aLengths)
set link length
std::vector< int > mEncoderOffset
Encoder offset vector.
int mNumberOfMotors
Number of motors of the robot.
std::vector< double > mAngleOffset
Angle offset vector [rad].
bool AnglePositionTest(const angles_calc &a) const
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
structs, type and constants used in inverse kinematics calculation
bool initialize()
initialization routine
bool mIsInitialized
Initialized flag.
std::vector< double > getAngOff()
get angle offset
bool angledef(angles_calc &a) const
bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)


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