KatanaKinematics6M90T.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2006 by Tiziano Mueller *
3  * tiziano.mueller@neuronics.ch *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the *
17  * Free Software Foundation, Inc., *
18  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19  ***************************************************************************/
20 
23 #include <algorithm>
24 
25 namespace KNI {
26 
27 const double KatanaKinematics6M90T::_tolerance = 0.001;
29 
30 void
31 KatanaKinematics6M90T::DK(coordinates& solution, encoders const& current_encoders) const {
32  using namespace KNI_MHF;
33  // numering the angles starting by 0-5
34 
35  double x0, x1, x2, x3;
36  double y0, y1, y2, y3;
37  double z0, z1, z2, z3;
38 
39  angles current_angles(current_encoders.size());
40  for(unsigned int z = 0; z < current_angles.size(); ++z) {
41  current_angles[z] = enc2rad(current_encoders[z], _parameters[z].angleOffset, _parameters[z].epc, _parameters[z].encOffset, _parameters[z].rotDir);
42  }
43 
44  // needs refactoring:
45  current_angles[1] = current_angles[1] - M_PI/2.0;
46  current_angles[2] = current_angles[2] - M_PI;
47  current_angles[3] = M_PI - current_angles[3];
48  current_angles[5] = -current_angles[5];
49 
50  coordinates pose(6);
51 
52  angles cx(current_angles.size()), sx(current_angles.size());
53  angles::iterator cx_iter, sx_iter;
54 
55  angles angle = current_angles;
56 
57  angle[2] = angle[1]+angle[2];
58  angle[3] = angle[2]+angle[3];
59 
60  cx_iter = cx.begin();
61  sx_iter = sx.begin();
62  std::transform(angle.begin(), angle.end(), sx_iter, unary_precalc_sin<double>() );
63  std::transform(angle.begin(), angle.end(), cx_iter, unary_precalc_cos<double>() );
64 
65 
66  //x
67  x0 = cx[0]*sx[1];
68  x1 = cx[0]*sx[2];
69  x2 = cx[0]*sx[3];
70  x3 = -cx[0]*cx[3]*cx[4]-sx[0]*sx[4];
71  pose[0] = x0*_length[0]+x1*_length[1]+x2*_length[2]+x3*_length[3];
72 
73  //y
74  y0 = sx[0]*sx[1];
75  y1 = sx[0]*sx[2];
76  y2 = sx[0]*sx[3];
77  y3 = -sx[0]*cx[3]*cx[4]+cx[0]*sx[4];
78  pose[1] = y0*_length[0]+y1*_length[1]+y2*_length[2]+y3*_length[3];
79 
80  //z
81  z0 = cx[1];
82  z1 = cx[2];
83  z2 = cx[3];
84  z3 = cx[4]*sx[3];
85  pose[2] = z0*_length[0]+z1*_length[1]+z2*_length[2]+z3*_length[3];
86 
87 
88  //theta
89  pose[4] = acos(cx[4]*sx[3]);
90 
91  // phi & psi
92 
93 
94  const double theta1 = angle[0];
95  const double theta5 = angle[4];
96  const double theta6 = angle[5];
97  const double theta234 = angle[3];
98 
99  if( std::abs(pose[4])<_tolerance || std::abs(pose[4]-M_PI)<_tolerance ) { // catch the case where theta=0, resp. theta=180
100  //phi
101  angles v1(2), v2(2);
102 
103  double R11 = -sin(theta1)*cos(theta5) *sin(theta6) + cos(theta1)*(sin(theta234)*cos(theta6)+cos(theta234)*sin(theta5)*sin(theta6));
104  double R21 = sin(theta1)*sin(theta234)*cos(theta6) + sin(theta6)*(cos(theta1) *cos(theta5)+cos(theta234)*sin(theta1)*sin(theta5));
105 
106  v1[0] = acos( R11 );
107  v1[1] = -v1[0];
108  v2[0] = asin( R21 );
109  v2[1] = M_PI - v2[0];
110 
111  pose[3] = anglereduce(findFirstEqualAngle(v1, v2));
112 
113  //psi
114  pose[5] = 0;
115  } else {
116  //phi
117  const double R13 = -cos(theta1)*cos(theta234)*cos(theta5) - sin(theta1)*sin(theta5);
118  const double R23 = -sin(theta1)*cos(theta234)*cos(theta5) + cos(theta1)*sin(theta5);
119  pose[3] = atan2(R13, -R23);
120 
121  //psi
122  const double R31 = cos(theta234)*cos(theta6) - sin(theta234)*sin(theta5)*sin(theta6);
123  const double R32 = -cos(theta234)*sin(theta6) - sin(theta234)*sin(theta5)*cos(theta6);
124  pose[5] = atan2(R31, R32);
125  }
126 
127  std::swap(solution, pose);
128 }
129 
130 void
131 KatanaKinematics6M90T::init( metrics const& length, parameter_container const& parameters ) {
132  assert( (length.size() == 4) && "You have to provide the metrics for exactly 4 links" ); // we have 4 links
133  assert( (parameters.size() == 6) && "You have to provide exactly 5 motor parameters" ); // 5 motors are used for IK calculations
134  _setLength( length );
135  _setParameters ( parameters );
136 }
137 
138 
139 void
141  using namespace KNI_MHF;
142 
143  if(p_gr.z==0) {
144  angle.theta234=0;
145  angle.theta5=angle.theta1-atan1(-p_gr.x,-p_gr.y);
146  } else {
147  angle.theta234 = -acotan( ( (p_gr.x * p_gr.z * cos(angle.theta1) ) -
148  sqrt( ( -pow2(p_gr.z) ) *
149  ( -pow2(_length[3]) + pow2(p_gr.x) + pow2(p_gr.z) ) * pow2(sin(angle.theta1))
150  )
151  ) / pow2(p_gr.z)
152  );
153  angle.theta5 = acos( p_gr.z/(_length[3]*sin(angle.theta234)) );
154  }
155 
156  bool griptest;
157  griptest = GripperTest(p_gr, angle);
158  if(!griptest) {
159  angle.theta5=-angle.theta5;
160  griptest=GripperTest(p_gr, angle);
161  if(!griptest) {
162  angle.theta234 = -acotan( ( ( p_gr.x * p_gr.z * cos(angle.theta1) ) +
163  sqrt( ( -pow2(p_gr.z) ) *
164  ( -pow2(_length[3]) + pow2(p_gr.x) + pow2(p_gr.z) ) * pow2(sin(angle.theta1))
165  )
166  ) / pow2(p_gr.z)
167  );
168  angle.theta5 = acos( p_gr.z / (_length[3]*sin(angle.theta234)) );
169  if(p_gr.z==0) {
170  angle.theta234=-M_PI;
171  angle.theta5=atan1(p_gr.x,p_gr.y) - angle.theta1;
172  }
173 
174  griptest=GripperTest(p_gr, angle);
175  if(!griptest) {
176  angle.theta5=-angle.theta5;
177  }
178  }
179  }
180 
181 }
182 
183 bool
185  using namespace KNI_MHF;
186  double xgr2, ygr2, zgr2;
187 
188  xgr2 = -_length[3]*(cos(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)+sin(angle.theta1)*sin(angle.theta5));
189  ygr2 = -_length[3]*(sin(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)-cos(angle.theta1)*sin(angle.theta5));
190  zgr2 = _length[3]*sin(angle.theta234)*cos(angle.theta5);
191 
192  if((pow2(p_gr.x-xgr2)+pow2(p_gr.y-ygr2)+pow2(p_gr.z-zgr2))>=_tolerance)
193  return false;
194 
195  return true;
196 }
197 
198 void
200  using namespace KNI_MHF;
201 
202  double xg, yg, zg;
203  double d5 = _length[2] + _length[3];
204  xg = p.x + ( _length[3] * cos(angle.theta1) * sin(angle.theta234) );
205  yg = p.y + ( _length[3] * sin(angle.theta1) * sin(angle.theta234) );
206  zg = p.z + ( _length[3] * cos(angle.theta234) );
207 
208 
209  angle.b1 = xg*cos(angle.theta1) + yg*sin(angle.theta1) - d5*sin(angle.theta234);
210  angle.b2 = zg - d5*cos(angle.theta234);
211  angle.costh3 = -( pow2(angle.b1) + pow2(angle.b2) - pow2(_length[0]) - pow2(_length[1]) ) / ( 2.0*_length[0]*_length[1] );
212 
213 }
214 
215 double
217  using namespace KNI_MHF;
218  for(angles::const_iterator i = v1.begin(); i != v1.end(); ++i) {
219  for(angles::const_iterator j = v2.begin(); j != v2.end(); ++j) {
220  if(std::abs(anglereduce(*j) - anglereduce(*i)) < _tolerance)
221  return *i;
222  }
223  }
224  assert(!"precondition for findFirstEqualAngle failed -> no equal angles found");
225  return 0;
226 }
227 
228 
229 void
231  using namespace KNI_MHF;
232 
233  const double theta1 = angle.theta1;
234  double theta2 = 0;
235  const double theta3 = angle.theta3;
236  double theta4 = 0;
237  const double theta5 = angle.theta5;
238  double theta6 = 0;
239  const double theta234 = angle.theta234;
240  const double b1 = angle.b1;
241  const double b2 = angle.b2;
242 
243  const double phi = pose[3];
244  const double theta = pose[4];
245  const double psi = pose[5];
246 
247 
248  theta2 = -M_PI/2.0 - ( atan0(b1, b2)+atan0(_length[0]+_length[1]*cos(theta3),_length[1]*sin(theta3)) );
249  theta4 = theta234 - theta2 - theta3;
250 
251  if(!PositionTest6MS(theta1, theta2, theta3, theta234 ,p_m)) {
252  theta2 = theta2+M_PI;
253  theta4 = theta234 - theta2 - theta3;
254  }
255 
256  const double R11 = cos(phi)*cos(psi) - sin(phi)*cos(theta)*sin(psi);
257  const double R21 = sin(phi)*cos(psi) + cos(phi)*cos(theta)*sin(psi);
258 
259  std::vector<double> theta16c(2), theta16s(2);
260 
261  if(std::abs(theta234 + M_PI/2) < _tolerance) {
262  if(std::abs(theta5) < _tolerance) {
263  theta16c[0] = acos(-R11);
264  theta16c[1] = -theta16c[0];
265  theta16s[0] = asin(-R21);
266  theta16s[1] = M_PI - theta16s[0];
267 
268  theta6 = theta1 - findFirstEqualAngle(theta16c, theta16s);
269 
270  } else if(std::abs(theta5-M_PI) < _tolerance) {
271  theta16c[0] = acos(-R11);
272  theta16c[1] = -theta16c[0];
273  theta16s[0] = asin(-R21);
274  theta16s[1] = M_PI - theta16s[0];
275 
276  theta6 = findFirstEqualAngle(theta16c, theta16s) - theta1;
277 
278  } else {
279  assert(!"Special case \"|theta234+(1/2)*pi| = 0\" detected, but no solution found");
280  }
281 
282  } else if(std::abs(theta234 + 3*M_PI/2) < _tolerance) {
283  if(std::abs(theta5) < _tolerance) {
284  theta16c[0] = acos(R11);
285  theta16c[1] = -theta16c[0];
286  theta16s[0] = asin(R21);
287  theta16s[1] = M_PI - theta16s[0];
288 
289  theta6 = findFirstEqualAngle(theta16c, theta16s) - theta1;
290 
291  } else if(std::abs(theta5-M_PI ) < _tolerance) {
292  theta16c[0] = acos(R11);
293  theta16c[1] = -theta16c[0];
294  theta16s[0] = asin(R21);
295  theta16s[1] = M_PI -theta16s[0];
296 
297  theta6 = - theta1 - findFirstEqualAngle(theta16c, theta16s);
298  } else {
299  assert(!"Special case \"|theta234+(3/2)*pi| = 0\" detected, but no solution found");
300  }
301 
302  } else {
303 
304  const double R31 = sin(theta)*sin(psi);
305  const double R32 = sin(theta)*cos(psi);
306 
307  const double temp1 = cos(theta234);
308  const double temp2 = -sin(theta234)*sin(theta5);
309 
310  const double c = ( R31*temp1 + R32*temp2 ) / ( pow2(temp1) + pow2(temp2) );
311  const double s = ( R31*temp2 - R32*temp1 ) / ( pow2(temp1) + pow2(temp2) );
312 
313  theta16c[0] = acos(c);
314  theta16c[1] = -theta16c[0];
315  theta16s[0] = asin(s);
316  theta16s[1] = M_PI - theta16s[0];
317 
318  theta6 = findFirstEqualAngle(theta16c, theta16s);
319  }
320 
321 
322  angle.theta2 = theta2;
323  angle.theta4 = theta4;
324  angle.theta6 = theta6;
325 }
326 
327 bool
328 KatanaKinematics6M90T::PositionTest6MS(const double& theta1, const double& theta2, const double& theta3, const double& theta234, const position &p) const {
329  using namespace KNI_MHF;
330  double temp, xm2, ym2, zm2;
331 
332  temp = _length[0]*sin(theta2) + _length[1]*sin(theta2+theta3) + _length[2]*sin(theta234);
333  xm2 = cos(theta1)*temp;
334  ym2 = sin(theta1)*temp;
335  zm2 = _length[0]*cos(theta2) + _length[1]*cos(theta2+theta3) + _length[2]*cos(theta234);
336 
337  if((pow2(p.x-xm2)+pow2(p.y-ym2)+pow2(p.z-zm2))>=_tolerance)
338  return false;
339 
340  return true;
341 }
342 
343 bool
345  using namespace KNI_MHF;
346 
347  // constants here. needs refactoring:
348  a.theta2=anglereduce(a.theta2+M_PI/2.0);
352  a.theta6=-a.theta6;
353 
354  if(a.theta1>_parameters[0].angleStop) {
355  a.theta1=a.theta1-2.0*M_PI;
356  }
357  if(a.theta2>M_PI) {
358  a.theta2=a.theta2-2.0*M_PI;
359  }
360  if(a.theta6<_parameters[5].angleOffset) {
361  a.theta6=a.theta6+2.0*M_PI;
362  } else if(a.theta6>_parameters[5].angleStop) {
363  a.theta6=a.theta6-2.0*M_PI;
364  }
365  if(a.theta5<_parameters[4].angleOffset) {
366  a.theta5 += 2.0*M_PI;
367  }
368 
369  return AnglePositionTest(a);
370 
371 }
372 
373 bool
375 
376  if( (a.theta1+0.0087<_parameters[0].angleOffset)||(a.theta1>_parameters[0].angleStop) ) {
377  return false;
378  }
379  if( (a.theta2-0.0087>_parameters[1].angleOffset)||(a.theta2<_parameters[1].angleStop) ) {
380  return false;
381  }
382  if( (a.theta3<_parameters[2].angleOffset)||(a.theta3>_parameters[2].angleStop) ) {
383  return false;
384  }
385 
386  if( (a.theta4<_parameters[3].angleOffset)||(a.theta4>_parameters[3].angleStop) ) {
387  return false;
388  }
389 
390  if( (a.theta5<_parameters[4].angleOffset)||(a.theta5>_parameters[4].angleStop) ) {
391  return false;
392  }
393  if( (a.theta6<_parameters[5].angleOffset)||(a.theta6>_parameters[5].angleStop) ) {
394  return false;
395  }
396 
397  return true;
398 }
399 
400 
401 
402 
403 void
404 KatanaKinematics6M90T::IK(encoders::iterator solution, coordinates const& pose, encoders const& current_encoders) const {
405  using namespace KNI_MHF;
406 
407  // pose: Winkel Deg->Rad
408  // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
409  // 0-3 fr theta1_1
410  // 4-7 fr theta1_2
411 
412  // Declaration
413  position p_gr;
414  position p_m;
416 
417  // calculation of the gripper vector
418  p_gr.x = _length[3]*sin(pose[4])*sin(pose[3]);
419  p_gr.y = -_length[3]*sin(pose[4])*cos(pose[3]);
420  p_gr.z = _length[3]*cos(pose[4]);
421 
422  p_m.x = pose[0]-p_gr.x;
423  p_m.y = pose[1]-p_gr.y;
424  p_m.z = pose[2]-p_gr.z;
425 
426  // calculate theta1_1 and theta1_2
427  angle[0].theta1 = atan1(p_m.x,p_m.y);
428  angle[4].theta1 = angle[0].theta1+M_PI;
429 
430 
431  // check the borders according to the settings
432  if(angle[0].theta1>_parameters[0].angleStop)
433  angle[0].theta1=angle[0].theta1-2.0*M_PI;
434 
435  if(angle[0].theta1<_parameters[0].angleOffset)
436  angle[0].theta1=angle[0].theta1+2.0*M_PI;
437 
438  if(angle[4].theta1>_parameters[0].angleStop)
439  angle[4].theta1=angle[4].theta1-2.0*M_PI;
440 
441  if(angle[4].theta1<_parameters[0].angleOffset)
442  angle[4].theta1=angle[4].theta1+2.0*M_PI;
443 
444 
445  //====THETA1_1==================
446  //-------THETA234_1-------------
447  IK_theta234theta5(angle[0], p_gr);
448  IK_b1b2costh3_6MS(angle[0], p_m);
449 
450  angle[1]=angle[0];
451  angle[0].theta3 = acos(angle[0].costh3)-M_PI;
452  thetacomp(angle[0], p_m, pose);
453  angle[1].theta3 = -acos(angle[1].costh3)+M_PI;
454  thetacomp(angle[1], p_m, pose);
455 
456  //-------THETA234_2-------------
457  angle[2].theta1=angle[0].theta1;
458  angle[2].theta234=angle[0].theta234-M_PI;
459  angle[2].theta5=M_PI-angle[0].theta5;
460 
461  IK_b1b2costh3_6MS(angle[2], p_m);
462  angle[3]=angle[2];
463  angle[2].theta3 = acos(angle[2].costh3)-M_PI;
464  thetacomp(angle[2], p_m, pose);
465  angle[3].theta3 = -acos(angle[3].costh3)+M_PI;
466  thetacomp(angle[3], p_m, pose);
467 
468 
469  //====THETA1_2==================
470  //-------THETA234_1-------------
471  IK_theta234theta5(angle[4], p_gr);
472  IK_b1b2costh3_6MS(angle[4], p_m);
473 
474  angle[5]=angle[4];
475  angle[4].theta3 = acos(angle[4].costh3)-M_PI;
476  thetacomp(angle[4], p_m, pose);
477  angle[5].theta3 = -acos(angle[5].costh3)+M_PI;
478  thetacomp(angle[5], p_m, pose);
479 
480  //-------THETA234_2-------------
481  angle[6].theta1=angle[4].theta1;
482  angle[6].theta234=angle[4].theta234-M_PI;
483  angle[6].theta5=M_PI-angle[4].theta5;
484  IK_b1b2costh3_6MS(angle[6], p_m);
485  angle[7]=angle[6];
486  angle[6].theta3 = acos(angle[6].costh3)-M_PI;
487  thetacomp(angle[6], p_m, pose);
488  angle[7].theta3 = -acos(angle[7].costh3)+M_PI;
489  thetacomp(angle[7], p_m, pose);
490 
491  for( ::std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); /* do iter forward in body */ ) {
492  if( pow2(iter->costh3) <= 1.0) {
493  if(!angledef(*iter))
494  iter = angle.erase(iter);
495  else
496  ++iter;
497  continue;
498  }
499  iter = angle.erase(iter);
500  }
501 
502 
503  if(angle.size() == 0) {
504  throw NoSolutionException();
505  }
506 
507  ::std::vector< ::std::vector<int> > PossibleTargetsInEncoders;
508  for( ::std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
509  ::std::vector<int> solution(6);
510 
511  solution[0] = rad2enc(i->theta1, _parameters[0].angleOffset, _parameters[0].epc, _parameters[0].encOffset, _parameters[0].rotDir);
512  solution[1] = rad2enc(i->theta2, _parameters[1].angleOffset, _parameters[1].epc, _parameters[1].encOffset, _parameters[1].rotDir);
513  solution[2] = rad2enc(i->theta3, _parameters[2].angleOffset, _parameters[2].epc, _parameters[2].encOffset, _parameters[2].rotDir);
514  solution[3] = rad2enc(i->theta4, _parameters[3].angleOffset, _parameters[3].epc, _parameters[3].encOffset, _parameters[3].rotDir);
515  solution[4] = rad2enc(i->theta5, _parameters[4].angleOffset, _parameters[4].epc, _parameters[4].encOffset, _parameters[4].rotDir);
516  solution[5] = rad2enc(i->theta6, _parameters[5].angleOffset, _parameters[5].epc, _parameters[5].encOffset, _parameters[5].rotDir);
517  PossibleTargetsInEncoders.push_back(solution);
518  }
519 
520 
521  ::std::vector< ::std::vector<int> >::const_iterator sol = KinematicsDefaultEncMinAlgorithm()(PossibleTargetsInEncoders.begin(), PossibleTargetsInEncoders.end(), current_encoders.begin(), current_encoders.end());
522 
523  assert( sol != PossibleTargetsInEncoders.end() && "All solutions are out of range");
524 
525 
526  encoders::iterator gripper_encoder_iter = std::copy( (*sol).begin(), (*sol).end(), solution );
527 
528 }
529 
530 
531 
532 } // NAMESPACE: KNI
533 
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
std::vector< double > angles
Being used to store angles (in radian).
void _setLength(metrics const &length)
void init(metrics const &length, parameter_container const &parameters)
void DK(coordinates &solution, encoders const &current_encoders) const
void swap(Matrix &A, Matrix &B)
Definition: newmat.h:2159
std::vector< angles_calc > angles_container
FloatVector * pose
void IK_theta234theta5(angles_calc &angle, const position &p_gr) const
std::vector< KinematicParameters > parameter_container
void _setParameters(parameter_container const &parameters)
static const int _nrOfPossibleSolutions
bool AnglePositionTest(const angles_calc &a) const
bool PositionTest6MS(const double &theta1, const double &theta2, const double &theta3, const double &theta234, const position &p) const
bool angledef(angles_calc &a) const
_T atan1(_T in1, _T in2)
Definition: kinematics.h:36
FloatVector FloatVector * a
FloatVector * angle
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
double findFirstEqualAngle(const angles &v1, const angles &v2) const
void IK(encoders::iterator solution, coordinates const &pose, encoders const &cur_angles) const
std::vector< double > coordinates
To store coordinates.
bool GripperTest(const position &p_gr, const angles_calc &angle) const
_T atan0(const _T in1, const _T in2)
std::vector< int > encoders
To store encoders.
void IK_b1b2costh3_6MS(angles_calc &a, const position &p) const
void thetacomp(angles_calc &a, const position &p_m, const coordinates &pose) const
std::vector< double > metrics
To store metrics, &#39;aka&#39; the length&#39;s of the different segments of the robot.
Definition: Timer.h:30


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