KatanaKinematics6M180.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 KatanaKinematics6M180::_tolerance = 0.001;
29 
30 void
31 KatanaKinematics6M180::DK(coordinates& solution, encoders const& current_encoders) const {
32  using namespace KNI_MHF;
33  // numering the angles starting by 0-5
34 
35  double factor;
36  double R13, R23, R31, R32;
37 
38  angles current_angles(6);
39  for(int z = 0; z < 6; ++z) {
40  current_angles[z] = enc2rad(current_encoders[z], _parameters[z].angleOffset, _parameters[z].epc, _parameters[z].encOffset, _parameters[z].rotDir);
41  }
42 
43  // needs refactoring:
44  current_angles[1] = current_angles[1] - M_PI/2.0;
45  current_angles[2] = current_angles[2] - M_PI;
46  current_angles[3] = M_PI - current_angles[3];
47  current_angles[4] = -current_angles[4];
48 
49  coordinates pose(6);
50 
51  angles cx(current_angles.size()), sx(current_angles.size());
52  angles::iterator cx_iter, sx_iter;
53 
54  angles angle = current_angles;
55 
56  angle[2] = angle[1]+angle[2];
57  angle[3] = angle[2]+angle[3];
58 
59  cx_iter = cx.begin();
60  sx_iter = sx.begin();
61  std::transform(angle.begin(), angle.end(), sx_iter, unary_precalc_sin<double>() );
62  std::transform(angle.begin(), angle.end(), cx_iter, unary_precalc_cos<double>() );
63 
64  factor = (_length[0]*sx[1]+_length[1]*sx[2]+(_length[2]+_length[3])*sx[3]);
65  // x = px (compare homogenous transformation matrix)
66  pose[0] = cx[0]*factor;
67 
68  // y = pz (compare homogenous transformation matrix)
69  pose[1] = sx[0]*factor;
70 
71  // z = pz (compare homogenous transformation matrix)
72  pose[2] = _length[0]*cx[1]+_length[1]*cx[2]+(_length[2]+_length[3])*cx[3];
73 
74  // phi = atan2(R13/-R23) (compare homogenous transformatio nmatrix)
75  R13 = cx[0]*sx[3];
76  R23 = sx[0]*sx[3];
77  pose[3] = atan2(R13,-R23);
78 
79  // theta = acos(R33) (compare homogenous transformation matrix)
80  pose[4] = acos(cx[3]);
81 
82  // psi = atan2(R31/R32) (compare homogenous transformation matrix)
83  R31 = sx[3]*sx[4];
84  R32 = sx[3]*cx[4];
85  pose[5] = atan2(R31,R32);
86 
87 
88  std::swap(solution, pose);
89 }
90 
91 void
92 KatanaKinematics6M180::init( metrics const& length, parameter_container const& parameters ) {
93  assert( (length.size() == 4) && "You have to provide the metrics for exactly 4 links" ); // we have 4 links
94  assert( (parameters.size() == 6) && "You have to provide exactly 5 motor parameters" ); // 5 motors are used for IK calculations
95  _setLength( length );
96  _setParameters ( parameters );
97 }
98 
99 void
101  using namespace KNI_MHF;
102 
103  double d5 = _length[2] + _length[3];
104 
105  angle.b1 = p.x*cos(angle.theta1) + p.y*sin(angle.theta1) - d5*sin(angle.theta234);
106  angle.b2 = p.z - d5*cos(angle.theta234);
107  angle.costh3 = -( pow2(angle.b1) + pow2(angle.b2) - pow2(_length[0]) - pow2(_length[1]) ) / ( 2.0*_length[0]*_length[1] );
108 
109 }
110 
111 void
113  using namespace KNI_MHF;
114 
115  angle.theta2 = -M_PI/2.0 - ( atan0(angle.b1,angle.b2)+atan0(_length[0]+_length[1]*cos(angle.theta3),_length[1]*sin(angle.theta3)) );
116  angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
117 
118  if(!PositionTest6M180(angle,p_m)) {
119  angle.theta2 = angle.theta2+M_PI;
120  angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
121  }
122 
123 }
124 
125 bool
127  using namespace KNI_MHF;
128  double temp, xm2, ym2, zm2;
129 
130  temp = _length[0]*sin(a.theta2)+_length[1]*sin(a.theta2+a.theta3)+(_length[2]+_length[3])*sin(a.theta234);
131  xm2 = cos(a.theta1)*temp;
132  ym2 = sin(a.theta1)*temp;
133  zm2 = _length[0]*cos(a.theta2)+_length[1]*cos(a.theta2+a.theta3)+(_length[2]+_length[3])*cos(a.theta234);
134 
135  if((pow2(p.x-xm2)+pow2(p.y-ym2)+pow2(p.z-zm2))>=_tolerance)
136  return false;
137 
138  return true;
139 }
140 
141 bool
143  using namespace KNI_MHF;
144 
145  // constants here. needs refactoring:
146  a.theta2=anglereduce(a.theta2+M_PI/2.0);
150 
151  if(a.theta1>_parameters[0].angleStop) {
152  a.theta1=a.theta1-2.0*M_PI;
153  }
154  if(a.theta2>M_PI) {
155  a.theta2=a.theta2-2.0*M_PI;
156  }
157  if(a.theta5<_parameters[4].angleOffset) {
158  a.theta5=a.theta5+2.0*M_PI;
159  }
160 
161  return AnglePositionTest(a);
162 
163 }
164 
165 bool
167 
168  if( (a.theta1+0.0087<_parameters[0].angleOffset)||(a.theta1>_parameters[0].angleStop) )
169  return false;
170 
171  if( (a.theta2-0.0087>_parameters[1].angleOffset)||(a.theta2<_parameters[1].angleStop) )
172  return false;
173 
174  if( (a.theta3<_parameters[2].angleOffset)||(a.theta3>_parameters[2].angleStop) )
175  return false;
176 
177  if( (a.theta4<_parameters[3].angleOffset)||(a.theta4>_parameters[3].angleStop) )
178  return false;
179 
180  if( (a.theta5<_parameters[4].angleOffset)||(a.theta5>_parameters[4].angleStop) )
181  return false;
182 
183  return true;
184 }
185 
186 
187 
188 
189 void
190 KatanaKinematics6M180::IK(encoders::iterator solution, coordinates const& pose, encoders const& current_encoders) const {
191  using namespace KNI_MHF;
192 
193  // pose: Winkel Deg->Rad
194  // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
195  // 0-3 fr theta1_1
196  // 4-7 fr theta1_2
197 
198  // Declarations
199  position p_m;
201  double coeff1, coeff2, theta234;
202  double costh5, sinth5, theta5[2];
203  double R11, R21, R31, R32;
204  double phi, theta, psi;
205 
206  // Initialization
207  p_m.x = pose[0];
208  p_m.y = pose[1];
209  p_m.z = pose[2];
210 
211  // calculate theta1_1 and theta1_2
212  angle[0].theta1 = atan1(pose[0],pose[1]);
213  if (angle[0].theta1 > M_PI) {
214  angle[0].theta1 = angle[0].theta1 - M_PI;
215  if (angle[0].theta1 > (179.91/180*M_PI)) {
216  angle[0].theta1 = angle[0].theta1 - M_PI;
217  }
218  }
219  angle[4].theta1 = angle[0].theta1+M_PI;
220 
221  theta = pose[4];
222  psi = pose[5];
223  phi = atan1(p_m.x,p_m.y)+M_PI/2.0;
224  theta234 = pose[4];
225 
226  R11 = cos(phi)*cos(psi)-sin(phi)*cos(theta)*sin(psi);
227  R21 = sin(phi)*cos(psi)+cos(phi)*cos(theta)*sin(psi);
228  R31 = sin(theta)*sin(psi);
229  R32 = sin(theta)*cos(psi);
230 
231  // calculate theta5
232  if(theta234==0) {
233  //std::cout << "Warning: Singularity theta234=0 !" << std::endl;
234  for (int i=0; i<2; ++i) {
235  coeff1 = -sin(angle[i*4].theta1);
236  coeff2 = -cos(angle[i*4].theta1);
237  costh5 = coeff1*R11-coeff2*R21;
238  sinth5 = coeff1*R21+coeff2*R11;
239  theta5[i] = -findFirstEqualAngle(costh5, sinth5, _tolerance);
240  }
241  for (int i=0; i<_nrOfPossibleSolutions; ++i) {
242  if(i<4)
243  angle[i].theta5 = theta5[0];
244  else
245  angle[i].theta5 = theta5[1];
246  }
247  } else if(theta234==M_PI) {
248  //std::cout << "Warning: Singularity theta234=PI !" << std::endl;
249  for (int i=0; i<2; ++i) {
250  coeff1 = -sin(angle[i*4].theta1);
251  coeff2 = cos(angle[i*4].theta1);
252  costh5 = coeff1*R11+coeff2*R21;
253  sinth5 = -coeff1*R21+coeff2*R11;
254  theta5[i] = -findFirstEqualAngle(costh5, sinth5, _tolerance);
255  }
256  for (int i=0; i<_nrOfPossibleSolutions; ++i) {
257  if(i<4)
258  angle[i].theta5 = theta5[0];
259  else
260  angle[i].theta5 = theta5[1];
261  }
262  } else {
263  theta5[0] = -atan2(R31/sin(theta234),R32/sin(theta234));
264  theta5[1] = -atan2(R31/sin(-theta234),R32/sin(-theta234));
265  for (int i=0; i<_nrOfPossibleSolutions; ++i) {
266  if(i%4==0 || i%4==1)
267  angle[i].theta5 = theta5[0];
268  else
269  angle[i].theta5 = theta5[1];
270  }
271  }
272 
273  //====THETA1_1==================
274  //-------THETA234_1-------------
275  angle[0].theta234 = pose[4];
276  //angle[0].theta5 = pose[5];
277  IK_b1b2costh3_6M180(angle[0], p_m);
278 
279  angle[1] =angle[0];
280  angle[0].theta3 = acos(angle[0].costh3)-M_PI;
281  thetacomp(angle[0], p_m);
282  angle[1].theta3 = -acos(angle[1].costh3)+M_PI;
283  thetacomp(angle[1], p_m);
284 
285  //-------THETA234_2-------------
286  angle[2].theta1 = angle[0].theta1;
287  angle[2].theta234 = -angle[0].theta234;
288  //angle[2].theta5 = angle[0].theta5;
289  IK_b1b2costh3_6M180(angle[2], p_m);
290 
291  angle[3] = angle[2];
292  angle[2].theta3 = acos(angle[2].costh3)-M_PI;
293  thetacomp(angle[2], p_m);
294  angle[3].theta3 = -acos(angle[3].costh3)+M_PI;
295  thetacomp(angle[3], p_m);
296 
297 
298  //====THETA1_2==================
299  //-------THETA234_1-------------
300  angle[4].theta234 = pose[4];
301  //angle[4].theta5 = pose[5];
302  IK_b1b2costh3_6M180(angle[4], p_m);
303 
304  angle[5] = angle[4];
305  angle[4].theta3 = acos(angle[4].costh3)-M_PI;
306  thetacomp(angle[4], p_m);
307  angle[5].theta3 = -acos(angle[5].costh3)+M_PI;
308  thetacomp(angle[5], p_m);
309 
310  //-------THETA234_2-------------
311  angle[6].theta1 = angle[4].theta1;
312  angle[6].theta234 = -angle[4].theta234;
313  //angle[6].theta5 = angle[4].theta5;
314  IK_b1b2costh3_6M180(angle[6], p_m);
315 
316  angle[7] = angle[6];
317  angle[6].theta3 = acos(angle[6].costh3)-M_PI;
318  thetacomp(angle[6], p_m);
319  angle[7].theta3 = -acos(angle[7].costh3)+M_PI;
320  thetacomp(angle[7], p_m);
321 
322  for( std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); /* do iter forward in body */ ) {
323  if( pow2(iter->costh3) <= 1.0) {
324  if(!angledef(*iter))
325  iter = angle.erase(iter);
326  else
327  ++iter;
328  continue;
329  }
330  iter = angle.erase(iter);
331  }
332 
333 
334  if(angle.size() == 0) {
335  throw NoSolutionException();
336  }
337 
338  std::vector< std::vector<int> > PossibleTargetsInEncoders;
339  for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
340  std::vector<int> solution(5);
341 
342  solution[0] = rad2enc(i->theta1, _parameters[0].angleOffset, _parameters[0].epc, _parameters[0].encOffset, _parameters[0].rotDir);
343  solution[1] = rad2enc(i->theta2, _parameters[1].angleOffset, _parameters[1].epc, _parameters[1].encOffset, _parameters[1].rotDir);
344  solution[2] = rad2enc(i->theta3, _parameters[2].angleOffset, _parameters[2].epc, _parameters[2].encOffset, _parameters[2].rotDir);
345  solution[3] = rad2enc(i->theta4, _parameters[3].angleOffset, _parameters[3].epc, _parameters[3].encOffset, _parameters[3].rotDir);
346  solution[4] = rad2enc(i->theta5, _parameters[4].angleOffset, _parameters[4].epc, _parameters[4].encOffset, _parameters[4].rotDir);
347 
348  PossibleTargetsInEncoders.push_back(solution);
349  }
350 
351 
352  std::vector< std::vector<int> >::const_iterator sol = KinematicsDefaultEncMinAlgorithm()(PossibleTargetsInEncoders.begin(), PossibleTargetsInEncoders.end(), current_encoders.begin(), current_encoders.end());
353 
354  assert( sol != PossibleTargetsInEncoders.end() && "All solutions are out of range");
355 
356 
357  encoders::iterator gripper_encoder_iter = std::copy( (*sol).begin(), (*sol).end(), solution );
358  *gripper_encoder_iter = current_encoders[5]; // copy gripper-encoders from current
359 
360 }
361 
362 
363 
364 } // NAMESPACE: KNI
365 
_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).
bool angledef(angles_calc &a) const
bool AnglePositionTest(const angles_calc &a) const
std::vector< angles_calc > angles_container
void thetacomp(angles_calc &a, const position &p_m) const
void swap(Matrix &A, Matrix &B)
Definition: newmat.h:2159
void DK(coordinates &solution, encoders const &current_encoders) const
void init(metrics const &length, parameter_container const &parameters)
FloatVector * pose
std::vector< KinematicParameters > parameter_container
void _setLength(metrics const &length)
_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(double cosValue, double sinValue, double tolerance)
void IK_b1b2costh3_6M180(angles_calc &a, const position &p) const
std::vector< double > coordinates
To store coordinates.
bool PositionTest6M180(const angles_calc &a, const position &p) const
void _setParameters(parameter_container const &parameters)
_T atan0(const _T in1, const _T in2)
std::vector< int > encoders
To store encoders.
static const int _nrOfPossibleSolutions
void IK(encoders::iterator solution, coordinates const &pose, encoders const &cur_angles) 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