lmBase.cpp
Go to the documentation of this file.
1 /*
2  * Katana Native Interface - A C++ interface to the robot arm Katana.
3  * Copyright (C) 2005 Neuronics AG
4  * Check out the AUTHORS file for detailed contact information.
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  */
20 
21 
22 #include "KNI_LM/lmBase.h"
23 
24 #include <iostream>
25 
26 bool VDEBUG = false;
27 const int MINIMAL_POLY_DISTANCE = 16;
28 /****************************************************************************/
29 /****************************************************************************/
30 
31 // Linear movement using multiple splines
32 void CLMBase::movLM2P(double X1, double Y1, double Z1, double Ph1,
33  double Th1, double Ps1, double X2, double Y2, double Z2, double Ph2,
34  double Th2, double Ps2, bool exactflag, double vmax, bool wait,
35  int tolerance, long timeout) {
36 
37  // check if the robot buffer is ready to receive a new linear movement
38  bool motors_ready = false;
39  while (!motors_ready) {
40  motors_ready = true;
41  for (int idx = 0; idx < getNumberOfMotors() - 1; idx++) {
42  base->GetMOT()->arr[idx].recvPVP();
43  motors_ready &= (base->GetMOT()->arr[idx].GetPVP()->msf != 152);
44  }
45  }
46 
47  // distance between the two points
48  double distance = sqrt(pow(X2-X1, 2.0) + pow(Y2-Y1, 2.0) + pow(Z2-Z1, 2.0));
49 
50  // acceleration limits in mm/s^2, hardcoded for now
51  double acc = 1500;
52  double dec = 1500;
53 
54  // calculate time for whole movement
55  double totaltime = totalTime(distance, acc, dec, vmax);
56 
57  // calculate number of splines needed
58  double maxtimeperspline = 0.5;
59  int steps = (int) (totaltime / maxtimeperspline) + 1;
60  short timeperspline;
61  timeperspline = (short) floor(100*(totaltime/(steps))+1);
62 
63  // calculate intermediate points
64  int numberofmotors = getNumberOfMotors();
65  int i, j;
66  double* timearray = new double [steps + 1];
67  double** dataarray = new double* [steps + 1];
68  for (i = 0; i < (steps + 1); i++)
69  dataarray[i] = new double [numberofmotors];
70  double relposition, time, lasttime, x, y, z, phi, theta, psi;
71  lasttime = 0;
72  std::vector<int> solution(numberofmotors, 0), lastsolution(numberofmotors, 0);
73  for (i = 0; i <= steps; i++) {
74  // calculate parameters for i-th position
75  if(i<steps)
76  time = 0.01 * i * (double)timeperspline;
77  else
78  time = totaltime;
79 
80  relposition = relPosition((double) time, distance, acc, dec, vmax);
81  x = X1 + relposition * (X2 - X1);
82  y = Y1 + relposition * (Y2 - Y1);
83  z = Z1 + relposition * (Z2 - Z1);
84  phi = Ph1 + relposition * (Ph2 - Ph1);
85  theta = Th1 + relposition * (Th2 - Th1);
86  psi = Ps1 + relposition * (Ps2 - Ps1);
87 
88  // check kinematics
89  try {
90  IKCalculate(x, y, z, phi, theta, psi, solution.begin());
91  } catch(Exception NoSolutionException) {
92  throw KNI::NoSolutionException();
93  }
94 
95  // store data
96  for (j = 0; j < numberofmotors; j++) {
97  dataarray[i][j] = (double) solution.at(j);
98  }
99  timearray[i] = time;
100 
101  // check joint speeds, stop program if failed
102  if (time > 0) {
103  if (!checkJointSpeed(lastsolution, solution, (time - lasttime))) {
104  throw JointSpeedException();
105  }
106  }
107  lasttime = time;
108  lastsolution.clear();
109  lastsolution.assign(solution.begin(), solution.end());
110  }
111 
112  // calculate spline
113  short*** parameters = new short** [steps];
114  for (i = 0; i < steps; i++)
115  parameters[i] = new short* [numberofmotors];
116  for (i = 0; i < steps; i++)
117  for (j = 0; j < numberofmotors; j++)
118  parameters[i][j] = new short[7];
119  double* encoderarray = new double [steps + 1];
120  double* arr_p1 = new double [steps];
121  double* arr_p2 = new double [steps];
122  double* arr_p3 = new double [steps];
123  double* arr_p4 = new double [steps];
124  double s_time;
125  for (i = 0; i < numberofmotors; i++) {
126  // one motor at a time
127  for (j = 0; j <= steps; j++) {
128  encoderarray[j] = dataarray[j][i];
129  }
130  splineCoefficients(steps, timearray, encoderarray, arr_p1, arr_p2,
131  arr_p3, arr_p4);
132  // store parameters for G command to motor i
133  for (j = 0; j < steps; j++) {
134  // motor number
135  parameters[j][i][0] = (short) i;
136  // targetencoder
137  parameters[j][i][1] = (short) encoderarray[j + 1];
138  // robot time (in 10ms steps)
139  s_time = (timearray[j + 1] - timearray[j]) * 100;
140  if(j < steps-1)
141  parameters[j][i][2] = (short) timeperspline;
142  else
143  parameters[j][i][2] = (short) s_time;
144  // the four spline coefficients
145  // the actual position, round
146  parameters[j][i][3] = (short) floor(arr_p1[j] + 0.5);
147  // shift to be firmware compatible and round
148  parameters[j][i][4] = (short) floor(64 * arr_p2[j] / s_time +
149  0.5);
150  parameters[j][i][5] = (short) floor(1024 * arr_p3[j] /
151  pow(s_time, 2) + 0.5);
152  parameters[j][i][6] = (short) floor(32768 * arr_p4[j] /
153  pow(s_time, 3) + 0.5);
154  }
155  }
156 
157  // send spline
158  long spline_timeout = (long) parameters[0][0][2] * 10;// - 2;
159  KNI::Timer t(timeout), spline_t(spline_timeout);
160  t.Start();
161  spline_t.Start();
162  //wait for motor
163  int wait_timeout = 5000;
164  if (mKatanaType == 450) {
165  int totalsplinetime = 0;
166  for (i = 0; i < steps; i++) {
167  // ignore further steps if timeout elapsed
168  if (t.Elapsed())
169  break;
170  // calculate total time from beginning of spline
171  totalsplinetime += parameters[i][0][2] * 10;
172  // set and start movement
173  int activityflag = 0;
174  if (i == (steps-1)) {
175  // last spline, start movement
176  activityflag = 1; // no_next
177  } else if (totalsplinetime < 400) {
178  // more splines following, do not start movement yet
179  activityflag = 2; // no_start
180  } else {
181  // more splines following, start movement
182  activityflag = 0;
183  }
184 //spline_t.Start();
185  std::vector<short> polynomial;
186  for(j = 0; j < numberofmotors; j++) {
187  polynomial.push_back(parameters[i][j][2]); // time
188  polynomial.push_back(parameters[i][j][1]); // target
189  polynomial.push_back(parameters[i][j][3]); // p0
190  polynomial.push_back(parameters[i][j][4]); // p1
191  polynomial.push_back(parameters[i][j][5]); // p2
192  polynomial.push_back(parameters[i][j][6]); // p3
193  }
194  setAndStartPolyMovement(polynomial, exactflag, activityflag);
195 //std::cout << "time to send and start poly: " << spline_t.ElapsedTime() << "ms" << std::endl;
196  }
197  } else if (mKatanaType == 400) {
198  int totalsplinetime = 0;
199  for (i = 0; i < steps; i++) {
200  // ignore further steps if timeout elapsed
201  if (t.Elapsed())
202  break;
203  // send parameters
204 //spline_t.Start();
205  for(j = 0; j < numberofmotors; j++) {
206  sendSplineToMotor((unsigned short) parameters[i][j][0],
207  parameters[i][j][1], parameters[i][j][2],
208  parameters[i][j][3], parameters[i][j][4],
209  parameters[i][j][5], parameters[i][j][6]);
210  }
211  totalsplinetime += parameters[i][0][2] * 10;
212  // start movement
213  if (i == (steps-1)) {
214  // last spline, start movement
215  startSplineMovement(exactflag, 1);
216  } else if (totalsplinetime < 400) {
217  // more splines following, do not start movement yet
218  startSplineMovement(exactflag, 2);
219  } else {
220  // more splines following, start movement
221  startSplineMovement(exactflag, 0);
222  }
223 //std::cout << "time to send and start poly: " << spline_t.ElapsedTime() << "ms" << std::endl;
224  }
225  } else {
226  for (i = 0; i < steps; i++) {
227  // ignore further steps if timeout elapsed
228  if (t.Elapsed())
229  break;
230  // wait for motor to finish movement
231  waitForMotor(0, 0, tolerance, 2, wait_timeout);
232  // send parameters
233  for(j = 0; j < numberofmotors; j++) {
234  sendSplineToMotor((unsigned short) parameters[i][j][0],
235  parameters[i][j][1], parameters[i][j][2],
236  parameters[i][j][3], parameters[i][j][4],
237  parameters[i][j][5], parameters[i][j][6]);
238  }
239  // start movement
240  startSplineMovement(exactflag);
241  }
242  }
243 //std::cout << "time to send and start linmov: " << t.ElapsedTime() << "ms" << std::endl;
244  // cleanup
245  delete timearray;
246  for (i = 0; i < (steps + 1); i++)
247  delete dataarray[i];
248  delete dataarray;
249  for (i = 0; i < steps; i++)
250  for (j = 0; j < numberofmotors; j++)
251  delete parameters[i][j];
252  for (i = 0; i < steps; i++)
253  delete parameters[i];
254  delete parameters;
255  delete encoderarray;
256  delete arr_p1;
257  delete arr_p2;
258  delete arr_p3;
259  delete arr_p4;
260 
261  // wait for end of linear movement
262  if(wait){
263  waitFor(MSF_NLINMOV, wait_timeout);
264  }
265 }
266 
267 double CLMBase::totalTime(double distance, double acc, double dec,
268  double vmax) {
269 
270  // minimum distance to reach vmax
271  double borderdistance = pow(vmax, 2.0) / 2.0 * (1 / acc + 1 / dec);
272 
273  double time;
274  if (distance > borderdistance) {
275  time = distance / vmax + vmax / 2.0 * (1 / acc + 1 / dec);
276  } else {
277  time = sqrt(8 * distance / (acc + dec));
278  }
279 
280  return time;
281 }
282 
283 
284 double CLMBase::relPosition(double reltime, double distance, double acc, double dec,
285  double vmax) {
286 
287  // minimum distance to reach vmax
288  double borderdistance = pow(vmax, 2.0) / 2.0 * (1 / acc + 1 / dec);
289 
290  double position, totaltime, time;
291  if (distance > borderdistance) { // vmax reached during movement
292  totaltime = distance / vmax + vmax / 2.0 * (1 / acc + 1 / dec);
293  time = reltime ;
294  if (time < vmax / acc) { // accelerating
295  position = acc / 2 * pow(time, 2);
296  } else if (time < totaltime - (vmax / dec)) { // at vmax
297  position = vmax * (time - vmax / acc / 2);
298  } else { // decelerating
299  position = distance - dec * (pow(time, 2) / 2 - totaltime * time +
300  pow(totaltime, 2) /2);
301  }
302  } else { // vmax not reached during movement
303  totaltime = sqrt(8 * distance / (acc + dec));
304  time = reltime ;
305  if (time < totaltime * dec / (acc + dec)) { // accelerating
306  position = acc / 2 * pow(time, 2);
307  } else { // decelerating
308  position = distance - dec * (pow(time, 2) / 2 - totaltime * time +
309  pow(totaltime, 2) /2);
310  }
311  }
312 
313  return (position / distance);
314 }
315 
316 void CLMBase::splineCoefficients(int steps, double *timearray, double *encoderarray,
317  double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4) {
318 
319  int i, j; // countervariables
320 
321  // calculate time differences between points and b-coefficients
322  double* deltatime = new double [steps];
323  double* b = new double [steps];
324  for (i = 0; i < steps; i++) {
325  deltatime[i] = timearray[i + 1] - timearray[i];
326  b[i] = 1.0 / deltatime[i];
327  }
328 
329  // calculate a-coefficients
330  double* a = new double [steps - 1];
331  for (i = 0; i < (steps - 1); i++) {
332  a[i] = (2 / deltatime[i]) + (2 / deltatime[i + 1]);
333  }
334 
335  // build up the right hand side of the linear system
336  double* c = new double [steps];
337  double* d = new double [steps + 1];
338  d[0] = 0; // f_1' and f_n' equal zero
339  d[steps] = 0;
340  for (i = 0; i < steps; i++) {
341  c[i] = (encoderarray[i + 1] - encoderarray[i]) / (deltatime[i] * deltatime[i]);
342  }
343  for (i = 0; i < (steps - 1); i++) {
344  d[i + 1] = 3 * (c[i] + c[i + 1]);
345  }
346 
347  // compose A * f' = d
348  double** Alin = new double* [steps - 1]; // last column of Alin is right hand side
349  for (i = 0; i < (steps - 1); i++)
350  Alin[i] = new double [steps];
351  // fill with zeros
352  for (i = 0; i < (steps - 1); i++) {
353  for (j = 0; j < steps; j++) {
354  Alin[i][j] = 0.0;
355  }
356  }
357  // insert values
358  for (i = 0; i < (steps - 1); i++) {
359  if (i == 0) {
360  Alin[0][0] = a[0];
361  Alin[0][1] = b[1];
362  Alin[0][steps - 1] = d[1];
363  } else {
364  Alin[i][i - 1] = b[i];
365  Alin[i][i] = a[i];
366  Alin[i][i + 1] = b[i + 1];
367  Alin[i][steps - 1] = d[i + 1];
368  }
369  }
370 
371  // solve linear equation
372  boost::numeric::ublas::matrix<double> ublas_A(steps - 1, steps - 1);
373  boost::numeric::ublas::matrix<double> ublas_b(steps - 1, 1);
374  for (i = 0; i < (steps - 1); i++) {
375  for (j = 0; j < (steps - 1); j++) {
376  ublas_A(i, j) = Alin[i][j];
377  }
378  ublas_b(i, 0) = Alin[i][steps - 1];
379  }
380  boost::numeric::ublas::permutation_matrix<unsigned int> piv(steps - 1);
381  lu_factorize(ublas_A, piv);
382  lu_substitute(ublas_A, piv, ublas_b);
383 
384  // save result in derivatives array
385  double* derivatives = new double [steps + 1];
386  derivatives[0] = 0;
387  for (i = 0; i < (steps - 1); i++) {
388  derivatives[i + 1] = ublas_b(i, 0);
389  }
390  derivatives[steps] = 0;
391  // build the hermite polynom with difference scheme
392  // Q(t) = a0 + (b0 + (c0 + d0 * t) * (t - 1)) * t = a0 + (b0 - c0) * t +
393  // (c0 - d0) * t^2 + d0 * t^3 = p0 + p1 * t + p2 * t^2 + p3 * t^3
394  double a0, b0, c0, d0;
395  for (i = 0; i < steps; i++) {
396  a0 = encoderarray[i];
397  b0 = encoderarray[i + 1] - a0;
398  c0 = b0 - deltatime[i] * derivatives[i];
399  d0 = deltatime[i] * (derivatives[i + 1] + derivatives[i]) - 2 * b0;
400  arr_p1[i] = a0;
401  arr_p2[i] = b0 - c0;
402  arr_p3[i] = c0 - d0;
403  arr_p4[i] = d0;
404  }
405 }
407 bool CLMBase::checkJointSpeed(std::vector<int> lastsolution,
408  std::vector<int> solution, double time) {
409  const int speedlimit = 180; // encoder per 10ms
410  bool speedok = true;
411  int localtime = (int) (time * 100); // in 10ms
412  int speed;
413  int i;
414  // check speed for every motor
415  for (i = 0; i < ((int) solution.size()); i++) {
416  speed = abs(solution.at(i) - lastsolution.at(i)) / localtime;
417  if (speed > speedlimit)
418  speedok = false;
419  }
420  return speedok;
421 }
423 int CLMBase::getSpeed(int distance, int acceleration, int time) {
424  // shorten argument names
425  int d = distance;
426  int a = acceleration;
427  int t = time;
428  // distance needs to be positive
429  if (d < 0){
430  if(VDEBUG) std::cout << "getSpeed(): distance not positive\n";
431  return -1;
432  }
433  // acceleration needs to be positive
434  if (a < 0){
435  if(VDEBUG) std::cout << "getSpeed(): acceleration not positive\n";
436  return -1;
437  }
438  // time needs to be at least 3 (acceleration, speed and deceleration polynomial with length of at least 1)
439  if (t < 3){
440  if(VDEBUG) std::cout << "getSpeed(): time smaller than 3\n";
441  return -1;
442  }
443  // need to reach at least d with t/2 acceleration and t/2 deceleration
444  if (a * t * t < d * 4){
445  if(VDEBUG) std::cout << "getSpeed(): need to reach at least d with t/2 acceleration and t/2 deceleration\n";
446  return -1;
447  }
448  // calculate speed (derived from 't = d / speed + speed / a')
449  int speed = static_cast<int>(ceil(a * t / 2.0 - sqrt(a * a * t * t / 4.0 - a * d)));
450  if ((speed % a) != 0)
451  speed += (a - speed % a); // round up to multiple of a to reach in less than t
452 
453  //if(VDEBUG) std::cout << "getSpeed(): calculated speed: " << speed << "\n";
454  return speed;
455 }
456 // Point to point movement using splines
457 void CLMBase::movP2P(double X1, double Y1, double Z1, double Ph1, double Th1,
458  double Ps1, double X2, double Y2, double Z2, double Ph2, double Th2,
459  double Ps2, bool exactflag, double vmax, bool wait, long timeout) {
460  // variable declaration
461  int nOfMot = getNumberOfMotors();
462  int amax = 2;
463  int smax = abs(static_cast<int>(vmax));
464  smax -= smax % amax; // round down to multiple of amax
465  if (smax == 0)
466  smax = amax;
467  std::vector<int> start_enc(nOfMot);
468  std::vector<int> target_enc(nOfMot);
469  std::vector<int> distance(nOfMot);
470  std::vector<int> dir(nOfMot);
471  std::vector<bool> tooShortDistance(nOfMot);
472  bool reachmax;
473  int maxtime;
474  int maxdist = 0;
475  int maxmot = 0;
476 
477  // wait for motors to be ready
478  bool motors_ready = false;
479  KNI::Timer t(timeout);
480  t.Start();
481  while (!motors_ready) {
482  motors_ready = true;
483  for (int idx = 0; idx < nOfMot - 1; idx++) {
484  base->GetMOT()->arr[idx].recvPVP();
485  motors_ready &= (base->GetMOT()->arr[idx].GetPVP()->msf != 152);
486  }
487  if (t.Elapsed())
488  return;
489  }
490 
491  // calculate start encoders
492  IKCalculate(X1, Y1, Z1, Ph1, Th1, Ps1, start_enc.begin());
493 
494  // calculate target encoders
495  IKCalculate(X2, Y2, Z2, Ph2, Th2, Ps2, target_enc.begin(), start_enc);
496 
497  // calculate distances and directions
498  for (int i = 0; i < nOfMot; i++){
499  distance[i] = abs(target_enc[i] - start_enc[i]);
500  dir[i] = target_enc[i] - start_enc[i] < 0 ? -1 : 1;
501  if(distance[i] < MINIMAL_POLY_DISTANCE)
502  tooShortDistance[i] = true;
503  else
504  tooShortDistance[i] = false;
505  }
506 
507  // get maximum of distances (maxdist) and associated motor (maxmot)
508  for (int i = 0; i < nOfMot; i++) {
509  if (distance[i] > maxdist) {
510  maxmot = i;
511  maxdist = distance[i];
512  }
513  }
514 
515  // check if maxmot reaches given maximum speed
516  reachmax = (distance[maxmot] >= (((smax / amax) + 1) * smax));
517 
518  // calculate time maxmot needs for movement (will be common maxtime)
519  int maxpadding = 3; // maximum number of padding polynomials (with length 1)
520  if(reachmax) {
521  maxtime = (maxdist / smax + 1) + (smax / amax) + maxpadding;
522  } else{
523  // s^2 + a*s - a*d = 0 -> s = sqrt(a^2/4 + a*d) - a/2
524  int smaxnew = static_cast<int>(sqrt(static_cast<double>(amax * amax) / 4.0 + static_cast<double>(amax * maxdist)) - (static_cast<double>(amax) / 2.0));
525  smaxnew -= smaxnew % amax; // round down to multiple of amax
526  if (smaxnew == 0)
527  smaxnew = amax;
528  maxtime = (maxdist / smaxnew + 1) + (smaxnew / amax) + maxpadding;
529  }
530  if (maxtime < 6)
531  maxtime = 6;
532 
534  //spline calculation
535  std::vector<int> speed(nOfMot); // maximum speed for this motor
536  std::vector<int> corrspeed(nOfMot); // speed at correction polynomial
537  std::vector<int> t1(nOfMot); // time for first polynomial (acceleration)
538  std::vector<int> t2(nOfMot); // time for second polynomial (max speed)
539  std::vector<int> t3(nOfMot); // time for third polynomial (deceleration)
540  std::vector<int> t4(nOfMot); // time for second polynomial (correction or padding)
541  std::vector<int> t5(nOfMot); // time for second polynomial (rest of deceleration or padding)
542  std::vector<int> t6(nOfMot); // time for second polynomial (padding)
543  std::vector<int> p1_enc(nOfMot); // position between acceleration and max speed
544  std::vector<int> p2_enc(nOfMot); // position between max speed and deceleration
545  std::vector<int> p3_enc(nOfMot); // position between deceleration and (corr or padd)
546  std::vector<int> p4_enc(nOfMot); // position between (corr or padd) and (rod or padd)
547  std::vector<int> p5_enc(nOfMot); // position between (rod or padd) and padding
548  std::vector<short> target(nOfMot);
549  std::vector<short> time(nOfMot);
550  std::vector<short> pp0(nOfMot); //polynomial coefficients
551  std::vector<short> pp1(nOfMot);
552  std::vector<short> pp2(nOfMot);
553  std::vector<short> pp3(nOfMot);
554  for (int i = 0; i < nOfMot; i++) {
555  speed[i] = getSpeed(distance[i], amax, maxtime-maxpadding);
556  if (speed[i] < 0)
557  return;
558  if(speed[i] == 0) // only when distance == 0, avoid division by zero
559  speed[i] = amax;
560  corrspeed[i] = distance[i] % speed[i];
561  corrspeed[i] -= corrspeed[i] % amax; // round down to multiple of amax
562  t1[i] = speed[i] / amax;
563  p1_enc[i] = start_enc[i] + dir[i] * t1[i] * speed[i] / 2;
564  t2[i] = (distance[i] - speed[i] * speed[i] / amax) / speed[i]; // div speed, rest in correction polynomial
565  p2_enc[i] = p1_enc[i] + dir[i] * t2[i] * speed[i];
566  t3[i] = (speed[i] - corrspeed[i]) / amax;
567  p3_enc[i] = p2_enc[i] + dir[i] * t3[i] * (speed[i] + corrspeed[i]) / 2;
568  t4[i] = 1;
569  p4_enc[i] = p3_enc[i] + dir[i] * corrspeed[i];
570  t5[i] = corrspeed[i] / amax;
571  if (t5[i] == 0) // if padding polynomial (corrspeed == 0), lengh is 1
572  t5[i] = 1;
573  p5_enc[i] = p4_enc[i] + dir[i] * t5[i] * corrspeed[i] / 2;
574  t6[i] = maxtime - t1[i] - t2[i] - t3[i] - t4[i] - t5[i];
575  if(VDEBUG && (i == 0)){
576  std::cout << "\nparams axis " << i+1 << ":" << \
577  "\n distance: " << distance[i] << \
578  "\n dir: " << dir[i] << \
579  "\n speed: " << speed[i] << \
580  "\n correctionspeed: " << corrspeed[i] << \
581  "\n t1: " << t1[i] << \
582  "\n t2: " << t2[i] << \
583  "\n t3: " << t3[i] << \
584  "\n t4: " << t4[i] << \
585  "\n t5: " << t5[i] << \
586  "\n t6: " << t6[i] << \
587  "\n start_enc " << start_enc[i] << \
588  "\n p1_enc: " << p1_enc[i] << \
589  "\n p2_enc: " << p2_enc[i] << \
590  "\n p3_enc: " << p3_enc[i] << \
591  "\n p4_enc: " << p4_enc[i] << \
592  "\n p5_enc: " << p5_enc[i] << \
593  "\n target_enc " << target_enc[i] << std::endl;
594  }
595  }
596 
597  //Polynomial 1 (acceleration)
598  for (int i = 0; i < nOfMot; i++) {
599  if(!tooShortDistance[i]){
600  target[i] = static_cast<short>(p1_enc[i]);
601  time[i] = static_cast<short>(t1[i]);
602  pp0[i] = static_cast<short>(start_enc[i]);
603  pp1[i] = 0;
604  pp2[i] = static_cast<short>(1024*(0.5 * dir[i] * amax));
605  pp3[i] = 0;
606  }
607  else{
608  target[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] / 5);
609  time[i] = static_cast<short>(maxtime / 6);
610  pp0[i] = static_cast<short>(start_enc[i]);
611  pp1[i] = 0;
612  pp2[i] = 0;
613  pp3[i] = 0;
614  }
615  if(VDEBUG && (i == 0)){
616  std::cout << "pp axis " << i+1 << ":"\
617  "\t target: " << target[i] << \
618  "\t time: " << time[i] << \
619  "\tpp0: " << pp0[i] << \
620  ", pp1: " << pp1[i] << \
621  ", pp2: " << pp2[i] << \
622  ", pp3: " << pp3[i] << std::endl;
623  }
624  }
625  if (t.Elapsed())
626  return;
627  std::vector<short> polynomial;
628  for(int i = 0; i < nOfMot; ++i) {
629  polynomial.push_back(time[i]);
630  polynomial.push_back(target[i]);
631  polynomial.push_back(pp0[i]);
632  polynomial.push_back(pp1[i]);
633  polynomial.push_back(pp2[i]);
634  polynomial.push_back(pp3[i]);
635  }
636  setAndStartPolyMovement(polynomial, exactflag, 2);
637 
638  //Polynomial 2 (speed)
639  for (int i = 0; i < nOfMot; i++) {
640  if(!tooShortDistance[i]){
641  target[i] = static_cast<short>(p2_enc[i]);
642  time[i] = static_cast<short>(t2[i]);
643  pp0[i] = static_cast<short>(p1_enc[i]);
644  pp1[i] = static_cast<short>(64*(dir[i] * speed[i]));
645  pp2[i] = 0;
646  pp3[i] = 0;
647  }
648  else{
649  target[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 2 / 5);
650  time[i] = static_cast<short>(maxtime / 6);
651  pp0[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] / 5);
652  pp1[i] = 0;
653  pp2[i] = 0;
654  pp3[i] = 0;
655  }
656  if(VDEBUG && (i == 0)){
657  std::cout << "pp axis " << i+1 << ":"\
658  "\t target: " << target[i] << \
659  "\t time: " << time[i] << \
660  "\tpp0: " << pp0[i] << \
661  ", pp1: " << pp1[i] << \
662  ", pp2: " << pp2[i] << \
663  ", pp3: " << pp3[i] << std::endl;
664  }
665  }
666  if (t.Elapsed())
667  return;
668  polynomial.clear();
669  for(int i = 0; i < nOfMot; ++i) {
670  polynomial.push_back(time[i]);
671  polynomial.push_back(target[i]);
672  polynomial.push_back(pp0[i]);
673  polynomial.push_back(pp1[i]);
674  polynomial.push_back(pp2[i]);
675  polynomial.push_back(pp3[i]);
676  }
677  setAndStartPolyMovement(polynomial, exactflag, 2);
678 
679  //Polynomial 3 (deceleration)
680  for (int i = 0; i < nOfMot; i++) {
681  if(!tooShortDistance[i]){
682  target[i] = static_cast<short>(p3_enc[i]);
683  time[i] = static_cast<short>(t3[i]);
684  pp0[i] = static_cast<short>(p2_enc[i]);
685  pp1[i] = static_cast<short>(64*(dir[i] * speed[i]));
686  pp2[i] = static_cast<short>(1024*(-0.5 * dir[i] * amax));
687  pp3[i] = 0;
688  }
689  else{
690  target[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 3 / 5);
691  time[i] = static_cast<short>(maxtime / 6);
692  pp0[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 2 / 5);
693  pp1[i] = 0;
694  pp2[i] = 0;
695  pp3[i] = 0;
696  }
697  if(VDEBUG && (i == 0)){
698  std::cout << "pp axis " << i+1 << ":"\
699  "\t target: " << target[i] << \
700  "\t time: " << time[i] << \
701  "\tpp0: " << pp0[i] << \
702  ", pp1: " << pp1[i] << \
703  ", pp2: " << pp2[i] << \
704  ", pp3: " << pp3[i] << std::endl;
705  }
706  }
707  if (t.Elapsed())
708  return;
709  polynomial.clear();
710  for(int i = 0; i < nOfMot; ++i) {
711  polynomial.push_back(time[i]);
712  polynomial.push_back(target[i]);
713  polynomial.push_back(pp0[i]);
714  polynomial.push_back(pp1[i]);
715  polynomial.push_back(pp2[i]);
716  polynomial.push_back(pp3[i]);
717  }
718  setAndStartPolyMovement(polynomial, exactflag, 2);
719 
720  //Polynomial 4 (correction or padding if correction speed == 0)
721  for (int i = 0; i < nOfMot; i++) {
722  if(!tooShortDistance[i]){
723  target[i] = static_cast<short>(p4_enc[i]);
724  time[i] = static_cast<short>(t4[i]);
725  pp0[i] = static_cast<short>(p3_enc[i]);
726  pp1[i] = static_cast<short>(64*(dir[i] * corrspeed[i]));
727  pp2[i] = 0;
728  pp3[i] = 0;
729  }
730  else{
731  target[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 4 / 5);
732  time[i] = static_cast<short>(maxtime / 6);
733  pp0[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 3 / 5);
734  pp1[i] = 0;
735  pp2[i] = 0;
736  pp3[i] = 0;
737  }
738  if(VDEBUG && (i == 0)){
739  std::cout << "pp axis " << i+1 << ":"\
740  "\t target: " << target[i] << \
741  "\t time: " << time[i] << \
742  "\tpp0: " << pp0[i] << \
743  ", pp1: " << pp1[i] << \
744  ", pp2: " << pp2[i] << \
745  ", pp3: " << pp3[i] << std::endl;
746  }
747  }
748  if (t.Elapsed())
749  return;
750  polynomial.clear();
751  for(int i = 0; i < nOfMot; ++i) {
752  polynomial.push_back(time[i]);
753  polynomial.push_back(target[i]);
754  polynomial.push_back(pp0[i]);
755  polynomial.push_back(pp1[i]);
756  polynomial.push_back(pp2[i]);
757  polynomial.push_back(pp3[i]);
758  }
759  setAndStartPolyMovement(polynomial, exactflag, 2);
760 
761  //Polynomial 5 (deceleration or padding if correction speed == 0)
762  for (int i = 0; i < nOfMot; i++) {
763  if(!tooShortDistance[i]){
764  target[i] = static_cast<short>(p5_enc[i]);
765  time[i] = static_cast<short>(t5[i]);
766  pp0[i] = static_cast<short>(p4_enc[i]);
767  pp1[i] = static_cast<short>(64*(dir[i] * corrspeed[i]));
768  if (corrspeed[i] != 0) {
769  pp2[i] = static_cast<short>(1024*(-0.5 * dir[i] * amax));
770  } else {
771  pp2[i] = 0;
772  }
773  pp3[i] = 0;
774  }
775  else{
776  target[i] = static_cast<short>(target_enc[i]);
777  time[i] = static_cast<short>(maxtime / 6);
778  pp0[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 4 / 5);
779  pp1[i] = 0;
780  pp2[i] = 0;
781  pp3[i] = 0;
782  }
783  if(VDEBUG && (i == 0)){
784  std::cout << "pp axis " << i+1 << ":"\
785  "\t target: " << target[i] << \
786  "\t time: " << time[i] << \
787  "\tpp0: " << pp0[i] << \
788  ", pp1: " << pp1[i] << \
789  ", pp2: " << pp2[i] << \
790  ", pp3: " << pp3[i] << std::endl;
791  }
792  }
793  if (t.Elapsed())
794  return;
795  polynomial.clear();
796  for(int i = 0; i < nOfMot; ++i) {
797  polynomial.push_back(time[i]);
798  polynomial.push_back(target[i]);
799  polynomial.push_back(pp0[i]);
800  polynomial.push_back(pp1[i]);
801  polynomial.push_back(pp2[i]);
802  polynomial.push_back(pp3[i]);
803  }
804  setAndStartPolyMovement(polynomial, exactflag, 2);
805 
806  //Polynomial 6 (padding)
807  for (int i = 0; i < nOfMot; i++) {
808  if(!tooShortDistance[i]){
809  target[i] = static_cast<short>(target_enc[i]);
810  time[i] = static_cast<short>(t6[i]);
811  pp0[i] = static_cast<short>(p5_enc[i]);
812  pp1[i] = 0;
813  pp2[i] = 0;
814  pp3[i] = 0;
815  }
816  else{
817  target[i] = static_cast<short>(target_enc[i]);
818  time[i] = static_cast<short>(maxtime - (maxtime / 6) * 5);
819  pp0[i] = static_cast<short>(target_enc[i]);
820  pp1[i] = 0;
821  pp2[i] = 0;
822  pp3[i] = 0;
823  }
824  if(VDEBUG && (i == 0)){
825  std::cout << "pp axis " << i+1 << ":"\
826  "\t target: " << target[i] << \
827  "\t time: " << time[i] << \
828  "\tpp0: " << pp0[i] << \
829  ", pp1: " << pp1[i] << \
830  ", pp2: " << pp2[i] << \
831  ", pp3: " << pp3[i] << std::endl;
832  }
833  }
834  if (t.Elapsed())
835  return;
836  polynomial.clear();
837  for(int i = 0; i < nOfMot; ++i) {
838  polynomial.push_back(time[i]);
839  polynomial.push_back(target[i]);
840  polynomial.push_back(pp0[i]);
841  polynomial.push_back(pp1[i]);
842  polynomial.push_back(pp2[i]);
843  polynomial.push_back(pp3[i]);
844  }
845  setAndStartPolyMovement(polynomial, exactflag, 1); // no next & start movement
846 
847  // wait for end of linear movement
848  if(wait){
849  waitFor(MSF_NLINMOV, timeout);
850  }
851 }
853 void CLMBase::movLM(double X, double Y, double Z,
854  double Al, double Be, double Ga,
855  bool exactflag, double vmax, bool wait, int tolerance, long timeout) {
856 
857  double arr_tarpos[6] = {X, Y, Z, Al, Be, Ga};
858  // target position in cartesian units
859  double arr_actpos[6];
860  // current position in cartesian units, NO REFRESH, SINCE ALREADY DONE IN moveRobotLinearTo()
861  getCoordinates(arr_actpos[0], arr_actpos[1], arr_actpos[2], arr_actpos[3], arr_actpos[4], arr_actpos[5], false);
862 
863  movLM2P(arr_actpos[0], arr_actpos[1], arr_actpos[2], arr_actpos[3], arr_actpos[4], arr_actpos[5],
864  arr_tarpos[0], arr_tarpos[1], arr_tarpos[2], arr_tarpos[3], arr_tarpos[4], arr_tarpos[5],
865  exactflag, vmax, wait, tolerance, timeout);
866 
867 }
868 
869 void CLMBase::moveRobotLinearTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached, int waitTimeout) {
870  // refresh encoders to make sure we use the right actual position
871  base->recvMPS();
872 
873  movLM(x, y, z, phi, theta, psi, _activatePositionController, _maximumVelocity, waitUntilReached, 100, waitTimeout);
874 }
875 
876 void CLMBase::moveRobotLinearTo(std::vector<double> coordinates, bool waitUntilReached, int waitTimeout) {
877  moveRobotLinearTo( coordinates.at(0), coordinates.at(1), coordinates.at(2), coordinates.at(3), coordinates.at(4), coordinates.at(5), waitUntilReached, waitTimeout);
878 }
879 
880 void CLMBase::moveRobotTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached, int waitTimeout) {
881 
882  // TODO: remove call of old implementation
883 // std::cout << "moveRobotTo(): calling old implementation." << std::endl;
884 // CikBase::moveRobotTo(x, y, z, phi, theta, psi, waitUntilReached, waitTimeout);
885 // return;
886 
887  // current position in cartesian units (with encoder refresh)
888  double cp[6];
889  getCoordinates(cp[0], cp[1], cp[2], cp[3], cp[4], cp[5], true);
890 
891  movP2P(cp[0], cp[1], cp[2], cp[3], cp[4], cp[5], x, y, z, phi, theta, psi,
892  _activatePositionController, _maximumVelocity, waitUntilReached, waitTimeout);
893 }
894 
895 void CLMBase::moveRobotTo(std::vector<double> coordinates, bool waitUntilReached, int waitTimeout) {
896  moveRobotTo( coordinates.at(0), coordinates.at(1), coordinates.at(2), coordinates.at(3), coordinates.at(4), coordinates.at(5), waitUntilReached, waitTimeout);
897 }
898 
899 // set maximum linear velocity in mm/s
900 void CLMBase::setMaximumLinearVelocity(double maximumVelocity) {
901  if (maximumVelocity < 1)
902  maximumVelocity = 1;
903  if (maximumVelocity > 300)
904  maximumVelocity = 300;
905  _maximumVelocity = maximumVelocity;
906 }
907 double CLMBase::getMaximumLinearVelocity() const {
908  return _maximumVelocity;
909 }
910 
911 void CLMBase::setActivatePositionController(bool activate) {
912  _activatePositionController = activate;
913 }
914 bool CLMBase::getActivatePositionController() {
915  return _activatePositionController;
916 }
917 
918 
CKatana::mKatanaType
int mKatanaType
The type of KatanaXXX (300 or 400)
Definition: kmlExt.h:73
acceleration
int acceleration
Definition: wrapper_control_demo.cpp:40
CKatana::sendSplineToMotor
void sendSplineToMotor(short number, short targetPosition, short duration, short p1, short p2, short p3, short p4)
Definition: kmlExt.cpp:631
CKatana::base
CKatBase * base
base katana
Definition: kmlExt.h:67
KNI::Timer::Elapsed
bool Elapsed() const
Definition: Timer.cpp:69
CMotBase::GetPVP
const TMotPVP * GetPVP()
Definition: kmlMotBase.h:246
CLMBase::_maximumVelocity
double _maximumVelocity
Definition: lmBase.h:76
CLMBase::moveRobotLinearTo
void moveRobotLinearTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS)
Definition: lmBase.cpp:869
CLMBase::totalTime
double totalTime(double distance, double acc, double dec, double vmax)
Definition: lmBase.cpp:267
MSF_NLINMOV
non-linear movement ended, new: poly move finished
Definition: kmlMotBase.h:65
CMotBase::recvPVP
void recvPVP()
receive data
Definition: kmlMotBase.cpp:90
CKatana::waitFor
void waitFor(TMotStsFlg status, int waitTimeout)
Definition: kmlExt.cpp:458
TKatMOT::arr
CMotBase * arr
array of motors
Definition: kmlMotBase.h:42
CLMBase::_activatePositionController
bool _activatePositionController
Definition: lmBase.h:77
CKatana::dec
void dec(long idx, int dif, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Decrements the motor specified by an index postion in encoders.
Definition: kmlExt.cpp:42
CikBase::getCoordinates
void getCoordinates(double &x, double &y, double &z, double &phi, double &theta, double &psi, bool refreshEncoders=true)
Definition: ikBase.cpp:349
CLMBase::setActivatePositionController
void setActivatePositionController(bool activate)
Definition: lmBase.cpp:911
CLMBase::getActivatePositionController
bool getActivatePositionController()
Check if the position controller will be activated after the linear movement.
Definition: lmBase.cpp:914
CKatBase::GetMOT
const TKatMOT * GetMOT()
Get a pointer to the desired structure.
Definition: kmlBase.h:165
CKatana::startSplineMovement
void startSplineMovement(bool exactflag, int moreflag=1)
Definition: kmlExt.cpp:618
d
FloatVector * d
Definition: include/libKinematics.h:78
CLMBase::setMaximumLinearVelocity
void setMaximumLinearVelocity(double maximumVelocity)
Definition: lmBase.cpp:900
CKatBase::recvMPS
void recvMPS()
read all motor positions simultaneously
Definition: kmlBase.cpp:247
CLMBase::movP2P
void movP2P(double X1, double Y1, double Z1, double Ph1, double Th1, double Ps1, double X2, double Y2, double Z2, double Ph2, double Th2, double Ps2, bool exactflag, double vmax, bool wait=true, long timeout=TM_ENDLESS)
Definition: lmBase.cpp:457
CLMBase::movLM2P
void movLM2P(double X1, double Y1, double Z1, double Al1, double Be1, double Ga1, double X2, double Y2, double Z2, double Al2, double Be2, double Ga2, bool exactflag, double vmax, bool wait=true, int tolerance=100, long timeout=TM_ENDLESS)
Definition: lmBase.cpp:32
KNI::Timer::Start
void Start()
Definition: Timer.cpp:51
CLMBase::splineCoefficients
void splineCoefficients(int steps, double *timearray, double *encoderarray, double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4)
Definition: lmBase.cpp:316
CKatana::waitForMotor
void waitForMotor(short number, int encoders, int encTolerance=100, short mode=0, int waitTimeout=5000)
Definition: kmlExt.cpp:453
a
FloatVector FloatVector * a
Definition: include/libKinematics.h:78
CLMBase::getSpeed
int getSpeed(int distance, int acceleration, int time)
Definition: lmBase.cpp:423
CLMBase::getMaximumLinearVelocity
double getMaximumLinearVelocity() const
Definition: lmBase.cpp:907
TMotPVP::msf
TMotStsFlg msf
motor status flag
Definition: kmlMotBase.h:163
MINIMAL_POLY_DISTANCE
const int MINIMAL_POLY_DISTANCE
Definition: lmBase.cpp:27
CLMBase::moveRobotTo
void moveRobotTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS)
Definition: lmBase.cpp:880
CKatana::getNumberOfMotors
short getNumberOfMotors() const
Definition: kmlExt.cpp:357
CLMBase::movLM
void movLM(double X, double Y, double Z, double Al, double Be, double Ga, bool exactflag, double vmax, bool wait=true, int tolerance=100, long timeout=TM_ENDLESS)
Definition: lmBase.cpp:853
CikBase::IKCalculate
void IKCalculate(double X, double Y, double Z, double Al, double Be, double Ga, std::vector< int >::iterator solution_iter)
Definition: ikBase.cpp:200
CLMBase::relPosition
double relPosition(double reltime, double distance, double acc, double dec, double vmax)
Definition: lmBase.cpp:284
CLMBase::checkJointSpeed
bool checkJointSpeed(std::vector< int > lastsolution, std::vector< int > solution, double time)
Definition: lmBase.cpp:407
VDEBUG
bool VDEBUG
Definition: lmBase.cpp:26
CKatana::setAndStartPolyMovement
void setAndStartPolyMovement(std::vector< short > polynomial, bool exactflag, int moreflag)
Definition: kmlExt.cpp:636

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