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) {
38 bool motors_ready =
false;
39 while (!motors_ready) {
48 double distance = sqrt(pow(X2-X1, 2.0) + pow(Y2-Y1, 2.0) + pow(Z2-Z1, 2.0));
55 double totaltime =
totalTime(distance, acc, dec, vmax);
58 double maxtimeperspline = 0.5;
59 int steps = (int) (totaltime / maxtimeperspline) + 1;
61 timeperspline = (short) floor(100*(totaltime/(steps))+1);
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;
72 std::vector<int> solution(numberofmotors, 0), lastsolution(numberofmotors, 0);
73 for (i = 0; i <= steps; i++) {
76 time = 0.01 * i * (double)timeperspline;
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);
90 IKCalculate(x, y, z, phi, theta, psi, solution.begin());
96 for (j = 0; j < numberofmotors; j++) {
97 dataarray[i][j] = (double) solution.at(j);
108 lastsolution.clear();
109 lastsolution.assign(solution.begin(), solution.end());
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];
125 for (i = 0; i < numberofmotors; i++) {
127 for (j = 0; j <= steps; j++) {
128 encoderarray[j] = dataarray[j][i];
133 for (j = 0; j < steps; j++) {
135 parameters[j][i][0] = (short) i;
137 parameters[j][i][1] = (short) encoderarray[j + 1];
139 s_time = (timearray[j + 1] - timearray[j]) * 100;
141 parameters[j][i][2] = (short) timeperspline;
143 parameters[j][i][2] = (short) s_time;
146 parameters[j][i][3] = (short) floor(arr_p1[j] + 0.5);
148 parameters[j][i][4] = (short) floor(64 * arr_p2[j] / s_time +
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);
158 long spline_timeout = (long) parameters[0][0][2] * 10;
159 KNI::Timer t(timeout), spline_t(spline_timeout);
163 int wait_timeout = 5000;
165 int totalsplinetime = 0;
166 for (i = 0; i < steps; i++) {
171 totalsplinetime += parameters[i][0][2] * 10;
173 int activityflag = 0;
174 if (i == (steps-1)) {
177 }
else if (totalsplinetime < 400) {
185 std::vector<short> polynomial;
186 for(j = 0; j < numberofmotors; j++) {
187 polynomial.push_back(parameters[i][j][2]);
188 polynomial.push_back(parameters[i][j][1]);
189 polynomial.push_back(parameters[i][j][3]);
190 polynomial.push_back(parameters[i][j][4]);
191 polynomial.push_back(parameters[i][j][5]);
192 polynomial.push_back(parameters[i][j][6]);
198 int totalsplinetime = 0;
199 for (i = 0; i < steps; i++) {
205 for(j = 0; j < numberofmotors; j++) {
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]);
211 totalsplinetime += parameters[i][0][2] * 10;
213 if (i == (steps-1)) {
216 }
else if (totalsplinetime < 400) {
226 for (i = 0; i < steps; i++) {
233 for(j = 0; j < numberofmotors; j++) {
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]);
246 for (i = 0; i < (steps + 1); i++)
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];
271 double borderdistance = pow(vmax, 2.0) / 2.0 * (1 / acc + 1 /
dec);
274 if (distance > borderdistance) {
275 time = distance / vmax + vmax / 2.0 * (1 / acc + 1 /
dec);
277 time = sqrt(8 * distance / (acc + dec));
288 double borderdistance = pow(vmax, 2.0) / 2.0 * (1 / acc + 1 /
dec);
290 double position, totaltime, time;
291 if (distance > borderdistance) {
292 totaltime = distance / vmax + vmax / 2.0 * (1 / acc + 1 /
dec);
294 if (time < vmax / acc) {
295 position = acc / 2 * pow(time, 2);
296 }
else if (time < totaltime - (vmax / dec)) {
297 position = vmax * (time - vmax / acc / 2);
299 position = distance - dec * (pow(time, 2) / 2 - totaltime * time +
300 pow(totaltime, 2) /2);
303 totaltime = sqrt(8 * distance / (acc + dec));
305 if (time < totaltime * dec / (acc + dec)) {
306 position = acc / 2 * pow(time, 2);
308 position = distance - dec * (pow(time, 2) / 2 - totaltime * time +
309 pow(totaltime, 2) /2);
313 return (position / distance);
317 double *arr_p1,
double *arr_p2,
double *arr_p3,
double *arr_p4) {
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];
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]);
336 double* c =
new double [steps];
337 double*
d =
new double [steps + 1];
340 for (i = 0; i < steps; i++) {
341 c[i] = (encoderarray[i + 1] - encoderarray[i]) / (deltatime[i] * deltatime[i]);
343 for (i = 0; i < (steps - 1); i++) {
344 d[i + 1] = 3 * (c[i] + c[i + 1]);
348 double** Alin =
new double* [steps - 1];
349 for (i = 0; i < (steps - 1); i++)
350 Alin[i] =
new double [steps];
352 for (i = 0; i < (steps - 1); i++) {
353 for (j = 0; j < steps; j++) {
358 for (i = 0; i < (steps - 1); i++) {
362 Alin[0][steps - 1] = d[1];
364 Alin[i][i - 1] = b[i];
366 Alin[i][i + 1] = b[i + 1];
367 Alin[i][steps - 1] = d[i + 1];
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];
378 ublas_b(i, 0) = Alin[i][steps - 1];
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);
385 double* derivatives =
new double [steps + 1];
387 for (i = 0; i < (steps - 1); i++) {
388 derivatives[i + 1] = ublas_b(i, 0);
390 derivatives[steps] = 0;
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;
408 std::vector<int> solution,
double time) {
409 const int speedlimit = 180;
411 int localtime = (int) (time * 100);
415 for (i = 0; i < ((int) solution.size()); i++) {
416 speed = abs(solution.at(i) - lastsolution.at(i)) / localtime;
417 if (speed > speedlimit)
430 if(
VDEBUG) std::cout <<
"getSpeed(): distance not positive\n";
435 if(
VDEBUG) std::cout <<
"getSpeed(): acceleration not positive\n";
440 if(
VDEBUG) std::cout <<
"getSpeed(): time smaller than 3\n";
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";
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);
458 double Ps1,
double X2,
double Y2,
double Z2,
double Ph2,
double Th2,
459 double Ps2,
bool exactflag,
double vmax,
bool wait,
long timeout) {
463 int smax = abs(static_cast<int>(vmax));
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);
478 bool motors_ready =
false;
481 while (!motors_ready) {
483 for (
int idx = 0; idx < nOfMot - 1; idx++) {
492 IKCalculate(X1, Y1, Z1, Ph1, Th1, Ps1, start_enc.begin());
495 IKCalculate(X2, Y2, Z2, Ph2, Th2, Ps2, target_enc.begin(), start_enc);
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;
502 tooShortDistance[i] =
true;
504 tooShortDistance[i] =
false;
508 for (
int i = 0; i < nOfMot; i++) {
509 if (distance[i] > maxdist) {
511 maxdist = distance[i];
516 reachmax = (distance[maxmot] >= (((smax / amax) + 1) * smax));
521 maxtime = (maxdist / smax + 1) + (smax / amax) + maxpadding;
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;
528 maxtime = (maxdist / smaxnew + 1) + (smaxnew / amax) + maxpadding;
535 std::vector<int> speed(nOfMot);
536 std::vector<int> corrspeed(nOfMot);
537 std::vector<int> t1(nOfMot);
538 std::vector<int> t2(nOfMot);
539 std::vector<int> t3(nOfMot);
540 std::vector<int> t4(nOfMot);
541 std::vector<int> t5(nOfMot);
542 std::vector<int> t6(nOfMot);
543 std::vector<int> p1_enc(nOfMot);
544 std::vector<int> p2_enc(nOfMot);
545 std::vector<int> p3_enc(nOfMot);
546 std::vector<int> p4_enc(nOfMot);
547 std::vector<int> p5_enc(nOfMot);
548 std::vector<short> target(nOfMot);
549 std::vector<short> time(nOfMot);
550 std::vector<short> pp0(nOfMot);
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);
560 corrspeed[i] = distance[i] % speed[i];
561 corrspeed[i] -= corrspeed[i] % 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];
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;
569 p4_enc[i] = p3_enc[i] + dir[i] * corrspeed[i];
570 t5[i] = corrspeed[i] / amax;
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];
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;
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]);
604 pp2[i] =
static_cast<short>(1024*(0.5 * dir[i] * amax));
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]);
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;
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]);
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]));
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);
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;
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]);
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));
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);
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;
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]);
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]));
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);
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;
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]);
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));
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);
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;
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]);
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]);
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]);
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;
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]);
854 double Al,
double Be,
double Ga,
855 bool exactflag,
double vmax,
bool wait,
int tolerance,
long timeout) {
857 double arr_tarpos[6] = {X, Y, Z, Al, Be, Ga};
859 double arr_actpos[6];
861 getCoordinates(arr_actpos[0], arr_actpos[1], arr_actpos[2], arr_actpos[3], arr_actpos[4], arr_actpos[5],
false);
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);
869 void CLMBase::moveRobotLinearTo(
double x,
double y,
double z,
double phi,
double theta,
double psi,
bool waitUntilReached,
int waitTimeout) {
873 movLM(x, y, z, phi, theta, psi,
_activatePositionController,
_maximumVelocity, waitUntilReached, 100, waitTimeout);
877 moveRobotLinearTo( coordinates.at(0), coordinates.at(1), coordinates.at(2), coordinates.at(3), coordinates.at(4), coordinates.at(5), waitUntilReached, waitTimeout);
880 void CLMBase::moveRobotTo(
double x,
double y,
double z,
double phi,
double theta,
double psi,
bool waitUntilReached,
int waitTimeout) {
891 movP2P(cp[0], cp[1], cp[2], cp[3], cp[4], cp[5], x, y, z, phi, theta, psi,
896 moveRobotTo( coordinates.at(0), coordinates.at(1), coordinates.at(2), coordinates.at(3), coordinates.at(4), coordinates.at(5), waitUntilReached, waitTimeout);
901 if (maximumVelocity < 1)
903 if (maximumVelocity > 300)
904 maximumVelocity = 300;
int mKatanaType
The type of KatanaXXX (300 or 400)
void sendSplineToMotor(short number, short targetPosition, short duration, short p1, short p2, short p3, short p4)
CKatBase * base
base katana
void moveRobotLinearTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS)
double totalTime(double distance, double acc, double dec, double vmax)
non-linear movement ended, new: poly move finished
void recvPVP()
receive data
void waitFor(TMotStsFlg status, int waitTimeout)
CMotBase * arr
array of motors
bool _activatePositionController
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.
void getCoordinates(double &x, double &y, double &z, double &phi, double &theta, double &psi, bool refreshEncoders=true)
void setActivatePositionController(bool activate)
bool getActivatePositionController()
Check if the position controller will be activated after the linear movement.
const TKatMOT * GetMOT()
Get a pointer to the desired structure.
void startSplineMovement(bool exactflag, int moreflag=1)
void setMaximumLinearVelocity(double maximumVelocity)
void recvMPS()
read all motor positions simultaneously
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)
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)
void splineCoefficients(int steps, double *timearray, double *encoderarray, double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4)
void waitForMotor(short number, int encoders, int encTolerance=100, short mode=0, int waitTimeout=5000)
FloatVector FloatVector * a
int getSpeed(int distance, int acceleration, int time)
double getMaximumLinearVelocity() const
TMotStsFlg msf
motor status flag
const int MINIMAL_POLY_DISTANCE
void moveRobotTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS)
short getNumberOfMotors() const
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)
void IKCalculate(double X, double Y, double Z, double Al, double Be, double Ga, std::vector< int >::iterator solution_iter)
double relPosition(double reltime, double distance, double acc, double dec, double vmax)
bool checkJointSpeed(std::vector< int > lastsolution, std::vector< int > solution, double time)
void setAndStartPolyMovement(std::vector< short > polynomial, bool exactflag, int moreflag)