KatanaKinematics6M90G.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 
21 #include "KNI_InvKin/KatanaKinematics6M90G.h"
22 #include "common/MathHelperFunctions.h"
23 #include <algorithm>
24 
25 //#include <iostream>
26 using namespace std;
27 
28 namespace KNI {
29 
30 const double KatanaKinematics6M90G::_tolerance = 0.001;
31 const int KatanaKinematics6M90G::_nrOfPossibleSolutions = 8;
32 
33 void
34 KatanaKinematics6M90G::DK(coordinates& solution, encoders const& current_encoders) const {
35  using namespace KNI_MHF;
36  // numering the angles starting by 0-5
37 
38  double x0, x1, x2, x3;
39  double y0, y1, y2, y3;
40  double z0, z1, z2, z3;
41  double R13, R23, R33, R31, R32;
42 
43  angles current_angles(6);
44  for(int z = 0; z < 6; ++z) {
45  current_angles[z] = enc2rad(current_encoders[z], _parameters[z].angleOffset, _parameters[z].epc, _parameters[z].encOffset, _parameters[z].rotDir);
46 // cout << "Motor " << z << ": " << endl;
47 // cout << "epc: " << _parameters[z].epc << endl;
48 // cout << "encOffset: " << _parameters[z].encOffset << endl;
49 // cout << "angleOffset: " << _parameters[z].angleOffset << endl;
50 // cout << "angleStop: " << _parameters[z].angleStop << endl;
51 // cout << "rotDir: " << _parameters[z].rotDir << endl;
52  }
53 
54  // needs refactoring:
55  current_angles[1] = current_angles[1] - M_PI/2.0;
56  current_angles[2] = current_angles[2] - M_PI;
57  current_angles[3] = M_PI - current_angles[3];
58 
59  coordinates pose(6);
60 
61  angles cx(current_angles.size()), sx(current_angles.size());
62  angles::iterator cx_iter, sx_iter;
63 
64  angles angle = current_angles;
65 
66  angle[2] = angle[1]+angle[2];
67  angle[3] = angle[2]+angle[3];
68 
69  cx_iter = cx.begin();
70  sx_iter = sx.begin();
71  std::transform(angle.begin(), angle.end(), sx_iter, unary_precalc_sin<double>() );
72  std::transform(angle.begin(), angle.end(), cx_iter, unary_precalc_cos<double>() );
73 
74  R13 = (-1*cx[0]*cx[3]*cx[4])-(sx[0]*sx[4]);
75  R23 = (-1*sx[0]*cx[3]*cx[4])+(cx[0]*sx[4]);
76  R33 = sx[3]*cx[4];
77  R31 = cx[3];
78  R32 =(-1)*sx[3]*sx[4];
79 
80  //x
81  x0 = cx[0]*sx[1];
82  x1 = cx[0]*sx[2];
83  x2 = cx[0]*sx[3];
84  x3 = -cx[0]*cx[3]*cx[4]-sx[0]*sx[4];
85  pose[0] = x0*_length[0]+x1*_length[1]+x2*_length[2]+x3*_length[3];
86 
87  //y
88  y0 = sx[0]*sx[1];
89  y1 = sx[0]*sx[2];
90  y2 = sx[0]*sx[3];
91  y3 = -sx[0]*cx[3]*cx[4]+cx[0]*sx[4];
92  pose[1] = y0*_length[0]+y1*_length[1]+y2*_length[2]+y3*_length[3];
93 
94  //z
95  z0 = cx[1];
96  z1 = cx[2];
97  z2 = cx[3];
98  z3 = cx[4]*sx[3];
99  pose[2] = z0*_length[0]+z1*_length[1]+z2*_length[2]+z3*_length[3];
100 
101  // phi, theta, psi
102  pose[4] = acos(R33);
103  if(pose[4]==0) {
104  pose[3] = atan2(pose[1],pose[0]);
105  pose[5] = 0;
106  } else if(pose[4]==M_PI) {
107  pose[3] = atan2(pose[1],pose[0])+M_PI/2;
108  pose[5] = M_PI/2;
109  } else {
110  pose[3] = atan2(R13,-R23);
111  pose[5] = atan2(R31,R32);
112  }
113 
114 
115  std::swap(solution, pose);
116 }
117 
118 void
119 KatanaKinematics6M90G::init( metrics const& length, parameter_container const& parameters ) {
120  assert( (length.size() == 4) && "You have to provide the metrics for exactly 4 links" ); // we have 4 links
121  assert( (parameters.size() == 6) && "You have to provide exactly 5 motor parameters" ); // 5 motors are used for IK calculations
122  _setLength( length );
123  _setParameters ( parameters );
124 }
125 
126 
127 void
128 KatanaKinematics6M90G::IK_theta234theta5(angles_calc& angle, const position &p_gr) const {
129  using namespace KNI_MHF;
130 
131  angle.theta234 = -acotan( ( (p_gr.x * p_gr.z * cos(angle.theta1) ) -
132  sqrt( ( -pow2(p_gr.z) ) *
133  ( -pow2(_length[3]) + pow2(p_gr.x) + pow2(p_gr.z) ) * pow2(sin(angle.theta1))
134  )
135  ) / pow2(p_gr.z)
136  );
137 
138  angle.theta5 = acos( p_gr.z/(_length[3]*sin(angle.theta234)) );
139 
140  if(p_gr.z==0) {
141  angle.theta234=0;
142  angle.theta5=angle.theta1-atan1(-p_gr.x,-p_gr.y);
143  }
144 
145  bool griptest;
146  griptest = GripperTest(p_gr, angle);
147  if(!griptest) {
148  angle.theta5=-angle.theta5;
149  griptest=GripperTest(p_gr, angle);
150  if(!griptest) {
151  angle.theta234 = -acotan( ( ( p_gr.x * p_gr.z * cos(angle.theta1) ) +
152  sqrt( ( -pow2(p_gr.z) ) *
153  ( -pow2(_length[3]) + pow2(p_gr.x) + pow2(p_gr.z) ) * pow2(sin(angle.theta1))
154  )
155  ) / pow2(p_gr.z)
156  );
157  angle.theta5 = acos( p_gr.z / (_length[3]*sin(angle.theta234)) );
158  if(p_gr.z==0) {
159  angle.theta234=-M_PI;
160  angle.theta5=atan1(p_gr.x,p_gr.y) - angle.theta1;
161  }
162 
163  griptest=GripperTest(p_gr, angle);
164  if(!griptest) {
165  angle.theta5=-angle.theta5;
166  }
167  }
168  }
169 
170 }
171 
172 bool
173 KatanaKinematics6M90G::GripperTest(const position &p_gr, const angles_calc &angle) const {
174  using namespace KNI_MHF;
175  double xgr2, ygr2, zgr2;
176 
177  xgr2 = -_length[3]*(cos(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)+sin(angle.theta1)*sin(angle.theta5));
178  ygr2 = -_length[3]*(sin(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)-cos(angle.theta1)*sin(angle.theta5));
179  zgr2 = _length[3]*sin(angle.theta234)*cos(angle.theta5);
180 
181  if((pow2(p_gr.x-xgr2)+pow2(p_gr.y-ygr2)+pow2(p_gr.z-zgr2))>=_tolerance)
182  return false;
183 
184  return true;
185 }
186 
187 void
188 KatanaKinematics6M90G::IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const {
189  using namespace KNI_MHF;
190 
191  double xg, yg, zg;
192  double d5 = _length[2] + _length[3];
193  xg = p.x + ( _length[3] * cos(angle.theta1) * sin(angle.theta234) );
194  yg = p.y + ( _length[3] * sin(angle.theta1) * sin(angle.theta234) );
195  zg = p.z + ( _length[3] * cos(angle.theta234) );
196 
197 
198  angle.b1 = xg*cos(angle.theta1) + yg*sin(angle.theta1) - d5*sin(angle.theta234);
199  angle.b2 = zg - d5*cos(angle.theta234);
200  angle.costh3 = -( pow2(angle.b1) + pow2(angle.b2) - pow2(_length[0]) - pow2(_length[1]) ) / ( 2.0*_length[0]*_length[1] );
201 
202 }
203 
204 void
205 KatanaKinematics6M90G::thetacomp(angles_calc &angle, const position &p_m) const {
206  using namespace KNI_MHF;
207 
208  angle.theta2 = -M_PI/2.0 - ( atan0(angle.b1,angle.b2)+atan0(_length[0]+_length[1]*cos(angle.theta3),_length[1]*sin(angle.theta3)) );
209  angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
210 
211  if(!PositionTest6MS(angle,p_m)) {
212  angle.theta2 = angle.theta2+M_PI;
213  angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
214  }
215 
216 }
217 
218 bool
219 KatanaKinematics6M90G::PositionTest6MS(const angles_calc &a, const position &p) const {
220  using namespace KNI_MHF;
221  double temp, xm2, ym2, zm2;
222 
223  temp = _length[0]*sin(a.theta2)+_length[1]*sin(a.theta2+a.theta3)+_length[2]*sin(a.theta234);
224  xm2 = cos(a.theta1)*temp;
225  ym2 = sin(a.theta1)*temp;
226  zm2 = _length[0]*cos(a.theta2)+_length[1]*cos(a.theta2+a.theta3)+_length[2]*cos(a.theta234);
227 
228  if((pow2(p.x-xm2)+pow2(p.y-ym2)+pow2(p.z-zm2))>=_tolerance)
229  return false;
230 
231  return true;
232 }
233 
234 bool
235 KatanaKinematics6M90G::angledef(angles_calc &a) const {
236  using namespace KNI_MHF;
237 
238  // constants here. needs refactoring:
239  a.theta2=anglereduce(a.theta2+M_PI/2.0);
240  a.theta3=anglereduce(a.theta3+M_PI);
241  a.theta4=anglereduce(M_PI-a.theta4);
242  a.theta5=anglereduce(a.theta5);
243 
244  if(a.theta1>_parameters[0].angleStop) {
245  a.theta1=a.theta1-2.0*M_PI;
246  }
247  if(a.theta2>M_PI) {
248  a.theta2=a.theta2-2.0*M_PI;
249  }
250  if(a.theta5<_parameters[4].angleOffset) {
251  a.theta5=a.theta5+2.0*M_PI;
252  }
253 
254  return AnglePositionTest(a);
255 
256 }
257 
258 bool
259 KatanaKinematics6M90G::AnglePositionTest(const angles_calc &a) const {
260 
261  if( (a.theta1+0.0087<_parameters[0].angleOffset)||(a.theta1>_parameters[0].angleStop) )
262  return false;
263 
264  if( (a.theta2-0.0087>_parameters[1].angleOffset)||(a.theta2<_parameters[1].angleStop) )
265  return false;
266 
267  if( (a.theta3<_parameters[2].angleOffset)||(a.theta3>_parameters[2].angleStop) )
268  return false;
269 
270  if( (a.theta4<_parameters[3].angleOffset)||(a.theta4>_parameters[3].angleStop) )
271  return false;
272 
273  if( (a.theta5<_parameters[4].angleOffset)||(a.theta5>_parameters[4].angleStop) )
274  return false;
275 
276  return true;
277 }
278 
279 
280 
281 
282 void
283 KatanaKinematics6M90G::IK(encoders::iterator solution, coordinates const& pose, encoders const& current_encoders) const {
284  using namespace KNI_MHF;
285 
286  // pose: Winkel Deg->Rad
287  // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
288  // 0-3 fr theta1_1
289  // 4-7 fr theta1_2
290 
291  // Declaration
292  position p_gr;
293  position p_m;
294  angles_container angle(_nrOfPossibleSolutions);
295 
296  // calculation of the gripper vector
297  p_gr.x = _length[3]*sin(pose[4])*sin(pose[3]);
298  p_gr.y = -_length[3]*sin(pose[4])*cos(pose[3]);
299  p_gr.z = _length[3]*cos(pose[4]);
300 
301  p_m.x = pose[0]-p_gr.x;
302  p_m.y = pose[1]-p_gr.y;
303  p_m.z = pose[2]-p_gr.z;
304 
305  // calculate theta1_1 and theta1_2
306  angle[0].theta1 = atan1(p_m.x,p_m.y);
307  angle[4].theta1 = angle[0].theta1+M_PI;
308 
309 
310  // check the borders according to the settings
311  if(angle[0].theta1>_parameters[0].angleStop)
312  angle[0].theta1=angle[0].theta1-2.0*M_PI;
313 
314  if(angle[0].theta1<_parameters[0].angleOffset)
315  angle[0].theta1=angle[0].theta1+2.0*M_PI;
316 
317  if(angle[4].theta1>_parameters[0].angleStop)
318  angle[4].theta1=angle[4].theta1-2.0*M_PI;
319 
320  if(angle[4].theta1<_parameters[0].angleOffset)
321  angle[4].theta1=angle[4].theta1+2.0*M_PI;
322 
323 
324  //====THETA1_1==================
325  //-------THETA234_1-------------
326  IK_theta234theta5(angle[0], p_gr);
327  IK_b1b2costh3_6MS(angle[0], p_m);
328 
329  angle[1]=angle[0];
330  angle[0].theta3 = acos(angle[0].costh3)-M_PI;
331  thetacomp(angle[0], p_m);
332  angle[1].theta3 = -acos(angle[1].costh3)+M_PI;
333  thetacomp(angle[1], p_m);
334 
335  //-------THETA234_2-------------
336  angle[2].theta1=angle[0].theta1;
337  angle[2].theta234=angle[0].theta234-M_PI;
338  angle[2].theta5=M_PI-angle[0].theta5;
339 
340  IK_b1b2costh3_6MS(angle[2], p_m);
341  angle[3]=angle[2];
342  angle[2].theta3 = acos(angle[2].costh3)-M_PI;
343  thetacomp(angle[2], p_m);
344  angle[3].theta3 = -acos(angle[3].costh3)+M_PI;
345  thetacomp(angle[3], p_m);
346 
347 
348  //====THETA1_2==================
349  //-------THETA234_1-------------
350  IK_theta234theta5(angle[4], p_gr);
351  IK_b1b2costh3_6MS(angle[4], p_m);
352 
353  angle[5]=angle[4];
354  angle[4].theta3 = acos(angle[4].costh3)-M_PI;
355  thetacomp(angle[4], p_m);
356  angle[5].theta3 = -acos(angle[5].costh3)+M_PI;
357  thetacomp(angle[5], p_m);
358 
359  //-------THETA234_2-------------
360  angle[6].theta1=angle[4].theta1;
361  angle[6].theta234=angle[4].theta234-M_PI;
362  angle[6].theta5=M_PI-angle[4].theta5;
363  IK_b1b2costh3_6MS(angle[6], p_m);
364  angle[7]=angle[6];
365  angle[6].theta3 = acos(angle[6].costh3)-M_PI;
366  thetacomp(angle[6], p_m);
367  angle[7].theta3 = -acos(angle[7].costh3)+M_PI;
368  thetacomp(angle[7], p_m);
369 
370  for( ::std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); /* do iter forward in body */ ) {
371  if( pow2(iter->costh3) <= 1.0) {
372  if(!angledef(*iter))
373  iter = angle.erase(iter);
374  else
375  ++iter;
376  continue;
377  }
378  iter = angle.erase(iter);
379  }
380 
381 
382  if(angle.size() == 0) {
383  throw NoSolutionException();
384  }
385 
386  ::std::vector< ::std::vector<int> > PossibleTargetsInEncoders;
387  for( ::std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
388  ::std::vector<int> solution(5);
389 
390  solution[0] = rad2enc(i->theta1, _parameters[0].angleOffset, _parameters[0].epc, _parameters[0].encOffset, _parameters[0].rotDir);
391  solution[1] = rad2enc(i->theta2, _parameters[1].angleOffset, _parameters[1].epc, _parameters[1].encOffset, _parameters[1].rotDir);
392  solution[2] = rad2enc(i->theta3, _parameters[2].angleOffset, _parameters[2].epc, _parameters[2].encOffset, _parameters[2].rotDir);
393  solution[3] = rad2enc(i->theta4, _parameters[3].angleOffset, _parameters[3].epc, _parameters[3].encOffset, _parameters[3].rotDir);
394  solution[4] = rad2enc(i->theta5, _parameters[4].angleOffset, _parameters[4].epc, _parameters[4].encOffset, _parameters[4].rotDir);
395 
396  PossibleTargetsInEncoders.push_back(solution);
397  }
398 
399 
400  ::std::vector< ::std::vector<int> >::const_iterator sol = KinematicsDefaultEncMinAlgorithm()(PossibleTargetsInEncoders.begin(), PossibleTargetsInEncoders.end(), current_encoders.begin(), current_encoders.end());
401 
402  assert( sol != PossibleTargetsInEncoders.end() && "All solutions are out of range");
403 
404 
405  encoders::iterator gripper_encoder_iter = std::copy( (*sol).begin(), (*sol).end(), solution );
406  *gripper_encoder_iter = current_encoders[5]; // copy gripper-encoders from current
407 
408 }
409 
410 
411 
412 } // NAMESPACE: KNI
413 
KNI_MHF::rad2enc
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
Definition: include/common/MathHelperFunctions.h:142
KNI::KatanaKinematics::angles
std::vector< double > angles
Being used to store angles (in radian).
Definition: KatanaKinematics.h:71
KNI_MHF::unary_precalc_cos
Definition: include/common/MathHelperFunctions.h:56
KNI::KatanaKinematics6M90G::angles_container
std::vector< angles_calc > angles_container
Definition: KatanaKinematics6M90G.h:70
KNI_MHF::anglereduce
_T anglereduce(const _T a)
Definition: include/common/MathHelperFunctions.h:134
swap
void swap(Matrix &A, Matrix &B)
Definition: newmat.h:2159
KNI::KatanaKinematics6M90G::angles_calc::theta5
double theta5
Definition: KatanaKinematics6M90G.h:63
pose
FloatVector * pose
Definition: include/libKinematics.h:343
KNI::KatanaKinematics::parameter_container
std::vector< KinematicParameters > parameter_container
Definition: KatanaKinematics.h:67
KNI_MHF::unary_precalc_sin
Definition: include/common/MathHelperFunctions.h:47
KNI::KatanaKinematics6M90G::angles_calc::b1
double b1
Definition: KatanaKinematics6M90G.h:65
KNI_MHF::pow2
_T pow2(const _T in)
Definition: include/common/MathHelperFunctions.h:100
KNI_MHF::acotan
_T acotan(const _T in)
Definition: include/common/MathHelperFunctions.h:85
KNI::KatanaKinematics6M90G::angles_calc::theta4
double theta4
Definition: KatanaKinematics6M90G.h:62
atan1
_T atan1(_T in1, _T in2)
Definition: kinematics.h:36
KNI::KatanaKinematics6M90G::angles_calc::theta3
double theta3
Definition: KatanaKinematics6M90G.h:61
KNI_MHF
Definition: include/common/MathHelperFunctions.h:36
KNI::KatanaKinematics6M90G::angles_calc::theta2
double theta2
Definition: KatanaKinematics6M90G.h:60
a
FloatVector FloatVector * a
Definition: include/libKinematics.h:78
angle
FloatVector * angle
Definition: include/libKinematics.h:323
KNI_MHF::enc2rad
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
Definition: include/common/MathHelperFunctions.h:156
KNI::KatanaKinematics::coordinates
std::vector< double > coordinates
To store coordinates.
Definition: KatanaKinematics.h:74
KNI_MHF::atan0
_T atan0(const _T in1, const _T in2)
Definition: include/common/MathHelperFunctions.h:93
KNI::KatanaKinematics::encoders
std::vector< int > encoders
To store encoders.
Definition: KatanaKinematics.h:80
KNI::KatanaKinematics6M90G::angles_calc::b2
double b2
Definition: KatanaKinematics6M90G.h:66
KNI::KatanaKinematics6M90G::angles_calc::costh3
double costh3
Definition: KatanaKinematics6M90G.h:67
KNI::KatanaKinematics6M90G::angles_calc
Definition: KatanaKinematics6M90G.h:58
KNI::KatanaKinematics6M90G::angles_calc::theta1
double theta1
Definition: KatanaKinematics6M90G.h:59
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
KNI::KatanaKinematics6M90G::angles_calc::theta234
double theta234
Definition: KatanaKinematics6M90G.h:64
KNI
Definition: Timer.h:30

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