lmBase.cpp
Go to the documentation of this file.
00001 /*
00002  *   Katana Native Interface - A C++ interface to the robot arm Katana.
00003  *   Copyright (C) 2005 Neuronics AG
00004  *   Check out the AUTHORS file for detailed contact information.
00005  *
00006  *   This program is free software; you can redistribute it and/or modify
00007  *   it under the terms of the GNU General Public License as published by
00008  *   the Free Software Foundation; either version 2 of the License, or
00009  *   (at your option) any later version.
00010  *
00011  *   This program is distributed in the hope that it will be useful,
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *   GNU General Public License for more details.
00015  *
00016  *   You should have received a copy of the GNU General Public License
00017  *   along with this program; if not, write to the Free Software
00018  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  */
00020 
00021 
00022 #include "KNI_LM/lmBase.h"
00023 
00024 #include <iostream>
00025 
00026 bool VDEBUG = false;
00027 const int MINIMAL_POLY_DISTANCE = 16;
00028 /****************************************************************************/
00029 /****************************************************************************/
00030 
00031 // Linear movement using multiple splines
00032 void CLMBase::movLM2P(double X1, double Y1, double Z1, double Ph1,
00033                 double Th1, double Ps1, double X2, double Y2, double Z2, double Ph2,
00034                 double Th2, double Ps2, bool exactflag, double vmax, bool wait,
00035                 int tolerance, long timeout) {
00036 
00037         // check if the robot buffer is ready to receive a new linear movement
00038         bool motors_ready = false;
00039         while (!motors_ready) {
00040                 motors_ready = true;
00041                 for (int idx = 0; idx < getNumberOfMotors() - 1; idx++) {
00042                         base->GetMOT()->arr[idx].recvPVP();
00043                         motors_ready &= (base->GetMOT()->arr[idx].GetPVP()->msf != 152);
00044                 }
00045         }
00046 
00047         // distance between the two points
00048         double distance = sqrt(pow(X2-X1, 2.0) + pow(Y2-Y1, 2.0) + pow(Z2-Z1, 2.0));
00049 
00050         // acceleration limits in mm/s^2, hardcoded for now
00051         double acc = 1500;
00052         double dec = 1500;
00053 
00054         // calculate time for whole movement
00055         double totaltime = totalTime(distance, acc, dec, vmax);
00056 
00057         // calculate number of splines needed
00058         double maxtimeperspline = 0.5;
00059         int steps = (int) (totaltime / maxtimeperspline) + 1;
00060         short timeperspline;
00061         timeperspline = (short) floor(100*(totaltime/(steps))+1);
00062 
00063         // calculate intermediate points
00064         int numberofmotors = getNumberOfMotors();
00065         int i, j;
00066         double* timearray = new double [steps + 1];
00067         double** dataarray = new double* [steps + 1];
00068         for (i = 0; i < (steps + 1); i++)
00069                 dataarray[i] = new double [numberofmotors];
00070         double relposition, time, lasttime, x, y, z, phi, theta, psi;
00071         lasttime = 0;
00072         std::vector<int> solution(numberofmotors, 0), lastsolution(numberofmotors, 0);
00073         for (i = 0; i <= steps; i++) {
00074                 // calculate parameters for i-th position
00075                 if(i<steps)
00076                         time = 0.01 * i * (double)timeperspline;
00077                 else
00078                         time = totaltime;
00079 
00080                 relposition = relPosition((double) time, distance, acc, dec, vmax);
00081                 x = X1 + relposition * (X2 - X1);
00082                 y = Y1 + relposition * (Y2 - Y1);
00083                 z = Z1 + relposition * (Z2 - Z1);
00084                 phi = Ph1 + relposition * (Ph2 - Ph1);
00085                 theta = Th1 + relposition * (Th2 - Th1);
00086                 psi = Ps1 + relposition * (Ps2 - Ps1);
00087 
00088                 // check kinematics
00089                 try {
00090                         IKCalculate(x, y, z, phi, theta, psi, solution.begin());
00091                 } catch(Exception NoSolutionException) {
00092                         throw KNI::NoSolutionException();
00093                 }
00094 
00095                 // store data
00096                 for (j = 0; j < numberofmotors; j++) {
00097                         dataarray[i][j] = (double) solution.at(j);
00098                 }
00099                 timearray[i] = time;
00100 
00101                 // check joint speeds, stop program if failed
00102                 if (time > 0) {
00103                         if (!checkJointSpeed(lastsolution, solution, (time - lasttime))) {
00104                                 throw JointSpeedException();
00105                         }
00106                 }
00107                 lasttime = time;
00108                 lastsolution.clear();
00109                 lastsolution.assign(solution.begin(), solution.end());
00110         }
00111 
00112         // calculate spline
00113         short*** parameters = new short** [steps];
00114         for (i = 0; i < steps; i++)
00115                 parameters[i] = new short* [numberofmotors];
00116         for (i = 0; i < steps; i++)
00117                 for (j = 0; j < numberofmotors; j++)
00118                         parameters[i][j] = new short[7];
00119         double* encoderarray = new double [steps + 1];
00120         double* arr_p1 = new double [steps];
00121         double* arr_p2 = new double [steps];
00122         double* arr_p3 = new double [steps];
00123         double* arr_p4 = new double [steps];
00124         double s_time;
00125         for (i = 0; i < numberofmotors; i++) {
00126                 // one motor at a time
00127                 for (j = 0; j <= steps; j++) {
00128                         encoderarray[j] = dataarray[j][i];
00129                 }
00130                 splineCoefficients(steps, timearray, encoderarray, arr_p1, arr_p2,
00131                         arr_p3, arr_p4);
00132                 // store parameters for G command to motor i
00133                 for (j = 0; j < steps; j++) {
00134                         // motor number
00135                         parameters[j][i][0] = (short) i;
00136                         // targetencoder
00137                         parameters[j][i][1] = (short) encoderarray[j + 1];
00138                         // robot time (in 10ms steps)
00139                         s_time = (timearray[j + 1] - timearray[j]) * 100;
00140                         if(j < steps-1)
00141                                 parameters[j][i][2] = (short) timeperspline;
00142                         else
00143                                 parameters[j][i][2] = (short) s_time;
00144                         // the four spline coefficients
00145                         // the actual position, round
00146                         parameters[j][i][3] = (short) floor(arr_p1[j] + 0.5);
00147                         // shift to be firmware compatible and round
00148                         parameters[j][i][4] = (short) floor(64 * arr_p2[j] / s_time +
00149                                 0.5);
00150                         parameters[j][i][5] = (short) floor(1024 * arr_p3[j] /
00151                                 pow(s_time, 2) + 0.5);
00152                         parameters[j][i][6] = (short) floor(32768 * arr_p4[j] /
00153                                 pow(s_time, 3) + 0.5);
00154                 }
00155         }
00156 
00157         // send spline
00158         long spline_timeout = (long) parameters[0][0][2] * 10;// - 2;
00159         KNI::Timer t(timeout), spline_t(spline_timeout);
00160         t.Start();
00161         spline_t.Start();
00162         //wait for motor
00163         int wait_timeout = 5000;
00164         if (mKatanaType == 450) {
00165                 int totalsplinetime = 0;
00166                 for (i = 0; i < steps; i++) {
00167                         // ignore further steps if timeout elapsed
00168                         if (t.Elapsed())
00169                                 break;
00170                         // calculate total time from beginning of spline
00171                         totalsplinetime += parameters[i][0][2] * 10;
00172                         // set and start movement
00173                         int activityflag = 0;
00174                         if (i == (steps-1)) {
00175                                 // last spline, start movement
00176                                 activityflag = 1; // no_next
00177                         } else if (totalsplinetime < 400) {
00178                                 // more splines following, do not start movement yet
00179                                 activityflag = 2; // no_start
00180                         } else {
00181                                 // more splines following, start movement
00182                                 activityflag = 0;
00183                         }
00184 //spline_t.Start();
00185                         std::vector<short> polynomial;
00186                         for(j = 0; j < numberofmotors; j++) {
00187                                 polynomial.push_back(parameters[i][j][2]); // time
00188                                 polynomial.push_back(parameters[i][j][1]); // target
00189                                 polynomial.push_back(parameters[i][j][3]); // p0
00190                                 polynomial.push_back(parameters[i][j][4]); // p1
00191                                 polynomial.push_back(parameters[i][j][5]); // p2
00192                                 polynomial.push_back(parameters[i][j][6]); // p3
00193                         }
00194                         setAndStartPolyMovement(polynomial, exactflag, activityflag);
00195 //std::cout << "time to send and start poly: " << spline_t.ElapsedTime() << "ms" << std::endl;
00196                 }
00197         } else if (mKatanaType == 400) {
00198                 int totalsplinetime = 0;
00199                 for (i = 0; i < steps; i++) {
00200                         // ignore further steps if timeout elapsed
00201                         if (t.Elapsed())
00202                                 break;
00203                         // send parameters
00204 //spline_t.Start();
00205                         for(j = 0; j < numberofmotors; j++) {
00206                                 sendSplineToMotor((unsigned short) parameters[i][j][0],
00207                                         parameters[i][j][1], parameters[i][j][2],
00208                                         parameters[i][j][3], parameters[i][j][4],
00209                                         parameters[i][j][5], parameters[i][j][6]);
00210                         }
00211                         totalsplinetime += parameters[i][0][2] * 10;
00212                         // start movement
00213                         if (i == (steps-1)) {
00214                                 // last spline, start movement
00215                                 startSplineMovement(exactflag, 1);
00216                         } else if (totalsplinetime < 400) {
00217                                 // more splines following, do not start movement yet
00218                                 startSplineMovement(exactflag, 2);
00219                         } else {
00220                                 // more splines following, start movement
00221                                 startSplineMovement(exactflag, 0);
00222                         }
00223 //std::cout << "time to send and start poly: " << spline_t.ElapsedTime() << "ms" << std::endl;
00224                 }
00225         } else {
00226                 for (i = 0; i < steps; i++) {
00227                         // ignore further steps if timeout elapsed
00228                         if (t.Elapsed())
00229                                 break;
00230                         // wait for motor to finish movement
00231                         waitForMotor(0, 0, tolerance, 2, wait_timeout);
00232                         // send parameters
00233                         for(j = 0; j < numberofmotors; j++) {
00234                                 sendSplineToMotor((unsigned short) parameters[i][j][0],
00235                                         parameters[i][j][1], parameters[i][j][2],
00236                                         parameters[i][j][3], parameters[i][j][4],
00237                                         parameters[i][j][5], parameters[i][j][6]);
00238                         }
00239                         // start movement
00240                         startSplineMovement(exactflag);
00241                 }
00242         }
00243 //std::cout << "time to send and start linmov: " << t.ElapsedTime() << "ms" << std::endl;
00244         // cleanup
00245         delete timearray;
00246     for (i = 0; i < (steps + 1); i++)
00247         delete dataarray[i];
00248         delete dataarray;
00249     for (i = 0; i < steps; i++)
00250         for (j = 0; j < numberofmotors; j++)
00251             delete parameters[i][j];
00252     for (i = 0; i < steps; i++)
00253         delete parameters[i];
00254         delete parameters;
00255         delete encoderarray;
00256         delete arr_p1;
00257         delete arr_p2;
00258         delete arr_p3;
00259         delete arr_p4;
00260         
00261         // wait for end of linear movement
00262         if(wait){
00263                 waitFor(MSF_NLINMOV, wait_timeout);
00264         }
00265 }
00266 
00267 double CLMBase::totalTime(double distance, double acc, double dec,
00268                 double vmax) {
00269 
00270         // minimum distance to reach vmax
00271         double borderdistance = pow(vmax, 2.0) / 2.0 * (1 / acc + 1 / dec);
00272 
00273         double time;
00274         if (distance > borderdistance) {
00275                 time = distance / vmax + vmax / 2.0 * (1 / acc + 1 / dec);
00276         } else {
00277                 time = sqrt(8 * distance / (acc + dec));
00278         }
00279 
00280         return time;
00281 }
00282 
00283 
00284 double CLMBase::relPosition(double reltime, double distance, double acc, double dec,
00285                 double vmax) {
00286 
00287         // minimum distance to reach vmax
00288         double borderdistance = pow(vmax, 2.0) / 2.0 * (1 / acc + 1 / dec);
00289 
00290         double position, totaltime, time;
00291         if (distance > borderdistance) { // vmax reached during movement
00292                 totaltime = distance / vmax + vmax / 2.0 * (1 / acc + 1 / dec);
00293                 time = reltime ;
00294                 if (time < vmax / acc) { // accelerating
00295                         position = acc / 2 * pow(time, 2);
00296                 } else if (time < totaltime - (vmax / dec)) { // at vmax
00297                         position = vmax * (time - vmax / acc / 2);
00298                 } else { // decelerating
00299                         position = distance - dec * (pow(time, 2) / 2 - totaltime * time +
00300                                 pow(totaltime, 2) /2);
00301                 }
00302         } else { // vmax not reached during movement
00303                 totaltime = sqrt(8 * distance / (acc + dec));
00304                 time = reltime ;
00305                 if (time < totaltime * dec / (acc + dec)) { // accelerating
00306                         position = acc / 2 * pow(time, 2);
00307                 } else { // decelerating
00308                         position = distance - dec * (pow(time, 2) / 2 - totaltime * time +
00309                                 pow(totaltime, 2) /2);
00310                 }
00311         }
00312 
00313         return (position / distance);
00314 }
00315 
00316 void CLMBase::splineCoefficients(int steps, double *timearray, double *encoderarray,
00317                 double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4) {
00318 
00319         int i, j; // countervariables
00320 
00321         // calculate time differences between points and b-coefficients
00322         double* deltatime = new double [steps];
00323         double* b = new double [steps];
00324         for (i = 0; i < steps; i++) {
00325                 deltatime[i] = timearray[i + 1] - timearray[i];
00326                 b[i] = 1.0 / deltatime[i];
00327         }
00328 
00329         // calculate a-coefficients
00330         double* a = new double [steps - 1];
00331         for (i = 0; i < (steps - 1); i++) {
00332                 a[i] = (2 / deltatime[i]) + (2 / deltatime[i + 1]);
00333         }
00334 
00335         // build up the right hand side of the linear system
00336         double* c = new double [steps];
00337         double* d = new double [steps + 1];
00338         d[0] = 0; // f_1' and f_n' equal zero
00339         d[steps] = 0;
00340         for (i = 0; i < steps; i++) {
00341                 c[i] = (encoderarray[i + 1] - encoderarray[i]) / (deltatime[i] * deltatime[i]);
00342         }
00343         for (i = 0; i < (steps - 1); i++) {
00344                 d[i + 1] = 3 * (c[i] + c[i + 1]);
00345         }
00346 
00347         // compose A * f' = d
00348         double** Alin = new double* [steps - 1]; // last column of Alin is right hand side
00349         for (i = 0; i < (steps - 1); i++)
00350                 Alin[i] = new double [steps];
00351         // fill with zeros
00352         for (i = 0; i < (steps - 1); i++) {
00353                 for (j = 0; j < steps; j++) {
00354                         Alin[i][j] = 0.0;
00355                 }
00356         }
00357         // insert values
00358         for (i = 0; i < (steps - 1); i++) {
00359                 if (i == 0) {
00360                         Alin[0][0] = a[0];
00361                         Alin[0][1] = b[1];
00362                         Alin[0][steps - 1] = d[1];
00363                 } else {
00364                         Alin[i][i - 1] = b[i];
00365                         Alin[i][i] = a[i];
00366                         Alin[i][i + 1] = b[i + 1];
00367                         Alin[i][steps - 1] = d[i + 1];
00368                 }
00369         }
00370 
00371         // solve linear equation
00372         boost::numeric::ublas::matrix<double> ublas_A(steps - 1, steps - 1);
00373         boost::numeric::ublas::matrix<double> ublas_b(steps - 1, 1);
00374         for (i = 0; i < (steps - 1); i++) {
00375                 for (j = 0; j < (steps - 1); j++) {
00376                         ublas_A(i, j) = Alin[i][j];
00377                 }
00378                 ublas_b(i, 0) = Alin[i][steps - 1];
00379         }
00380         boost::numeric::ublas::permutation_matrix<unsigned int> piv(steps - 1);
00381         lu_factorize(ublas_A, piv);
00382         lu_substitute(ublas_A, piv, ublas_b);
00383 
00384         // save result in derivatives array
00385         double* derivatives = new double [steps + 1];
00386         derivatives[0] = 0;
00387         for (i = 0; i < (steps - 1); i++) {
00388                 derivatives[i + 1] = ublas_b(i, 0);
00389         }
00390         derivatives[steps] = 0;
00391         // build the hermite polynom with difference scheme
00392         // Q(t) = a0 + (b0 + (c0 + d0 * t) * (t - 1)) * t = a0 + (b0 - c0) * t +
00393         //   (c0 - d0) * t^2 + d0 * t^3 = p0 + p1 * t + p2 * t^2 + p3 * t^3
00394         double a0, b0, c0, d0;
00395         for (i = 0; i < steps; i++) {
00396                 a0 = encoderarray[i];
00397                 b0 = encoderarray[i + 1] - a0;
00398                 c0 = b0 - deltatime[i] * derivatives[i];
00399                 d0 = deltatime[i] * (derivatives[i + 1] + derivatives[i]) - 2 * b0;
00400                 arr_p1[i] = a0;
00401                 arr_p2[i] = b0 - c0;
00402                 arr_p3[i] = c0 - d0;
00403                 arr_p4[i] = d0;
00404         }
00405 }
00407 bool CLMBase::checkJointSpeed(std::vector<int> lastsolution,
00408                 std::vector<int> solution, double time) {
00409         const int speedlimit = 180; // encoder per 10ms
00410         bool speedok = true;
00411         int localtime = (int) (time * 100); // in 10ms
00412         int speed;
00413         int i;
00414         // check speed for every motor
00415         for (i = 0; i < ((int) solution.size()); i++) {
00416                 speed = abs(solution.at(i) - lastsolution.at(i)) / localtime;
00417                 if (speed > speedlimit)
00418                         speedok = false;
00419         }
00420         return speedok;
00421 }
00423 int CLMBase::getSpeed(int distance, int acceleration, int time) {
00424         // shorten argument names
00425         int d = distance;
00426         int a = acceleration;
00427         int t = time;
00428         // distance needs to be positive
00429         if (d < 0){
00430                 if(VDEBUG) std::cout << "getSpeed(): distance not positive\n";
00431                 return -1;
00432         }
00433         // acceleration needs to be positive
00434         if (a < 0){
00435                 if(VDEBUG) std::cout << "getSpeed(): acceleration not positive\n";
00436                 return -1;
00437         }
00438         // time needs to be at least 3 (acceleration, speed and deceleration polynomial with length of at least 1)
00439         if (t < 3){
00440                 if(VDEBUG) std::cout << "getSpeed(): time smaller than 3\n";
00441                 return -1;
00442         }
00443         // need to reach at least d with t/2 acceleration and t/2 deceleration
00444         if (a * t * t < d * 4){
00445                 if(VDEBUG) std::cout << "getSpeed(): need to reach at least d with t/2 acceleration and t/2 deceleration\n";
00446                 return -1;
00447         }
00448         // calculate speed (derived from 't = d / speed + speed / a')
00449         int speed = static_cast<int>(ceil(a * t / 2.0 - sqrt(a * a * t * t / 4.0 - a * d)));
00450         if ((speed % a) != 0)
00451                 speed += (a - speed % a); // round up to multiple of a to reach in less than t
00452         
00453         //if(VDEBUG) std::cout << "getSpeed(): calculated speed: " << speed << "\n";
00454         return speed;
00455 }
00456 // Point to point movement using splines
00457 void CLMBase::movP2P(double X1, double Y1, double Z1, double Ph1, double Th1,
00458                 double Ps1, double X2, double Y2, double Z2, double Ph2, double Th2,
00459                 double Ps2, bool exactflag, double vmax, bool wait, long timeout) {
00460         // variable declaration
00461         int nOfMot = getNumberOfMotors();
00462         int amax = 2; 
00463         int smax = abs(static_cast<int>(vmax));
00464         smax -= smax % amax; // round down to multiple of amax
00465         if (smax == 0)
00466                 smax = amax;
00467         std::vector<int> start_enc(nOfMot);
00468         std::vector<int> target_enc(nOfMot);
00469         std::vector<int> distance(nOfMot);
00470         std::vector<int> dir(nOfMot);
00471         std::vector<bool> tooShortDistance(nOfMot);
00472         bool reachmax;
00473         int maxtime;
00474         int maxdist = 0;
00475         int maxmot = 0;
00476         
00477         // wait for motors to be ready
00478         bool motors_ready = false;
00479         KNI::Timer t(timeout);
00480         t.Start();
00481         while (!motors_ready) {
00482                 motors_ready = true;
00483                 for (int idx = 0; idx < nOfMot - 1; idx++) {
00484                         base->GetMOT()->arr[idx].recvPVP();
00485                         motors_ready &= (base->GetMOT()->arr[idx].GetPVP()->msf != 152);
00486                 }
00487                 if (t.Elapsed())
00488                         return;
00489         }
00490         
00491         // calculate start encoders
00492         IKCalculate(X1, Y1, Z1, Ph1, Th1, Ps1, start_enc.begin());
00493         
00494         // calculate target encoders
00495         IKCalculate(X2, Y2, Z2, Ph2, Th2, Ps2, target_enc.begin(), start_enc);
00496         
00497         // calculate distances and directions
00498         for (int i = 0; i < nOfMot; i++){
00499                 distance[i] = abs(target_enc[i] - start_enc[i]);
00500                 dir[i] = target_enc[i] - start_enc[i] < 0 ? -1 : 1;
00501                 if(distance[i] < MINIMAL_POLY_DISTANCE)
00502                         tooShortDistance[i] = true;
00503                 else
00504                         tooShortDistance[i] = false;
00505         }
00506         
00507         // get maximum of distances (maxdist) and associated motor (maxmot)
00508         for (int i = 0; i < nOfMot; i++) {
00509                 if (distance[i] > maxdist) {
00510                         maxmot = i;
00511                         maxdist = distance[i];
00512                 }
00513         }
00514         
00515         // check if maxmot reaches given maximum speed
00516         reachmax = (distance[maxmot] >= (((smax / amax) + 1) * smax));
00517         
00518         // calculate time maxmot needs for movement (will be common maxtime)
00519         int maxpadding = 3; // maximum number of padding polynomials (with length 1)
00520         if(reachmax) { 
00521                 maxtime = (maxdist / smax + 1) + (smax / amax) + maxpadding;
00522         } else{
00523                 // s^2 + a*s - a*d = 0  ->  s = sqrt(a^2/4 + a*d) - a/2
00524                 int smaxnew = static_cast<int>(sqrt(static_cast<double>(amax * amax) / 4.0 + static_cast<double>(amax * maxdist)) - (static_cast<double>(amax) / 2.0));
00525                 smaxnew -= smaxnew % amax; // round down to multiple of amax
00526                 if (smaxnew == 0)
00527                         smaxnew = amax;
00528                 maxtime = (maxdist / smaxnew + 1) + (smaxnew / amax) + maxpadding;
00529         }
00530         if (maxtime < 6)
00531                 maxtime = 6;
00532         
00534         //spline calculation
00535         std::vector<int> speed(nOfMot); // maximum speed for this motor
00536         std::vector<int> corrspeed(nOfMot); // speed at correction polynomial
00537         std::vector<int> t1(nOfMot); // time for first polynomial (acceleration)
00538         std::vector<int> t2(nOfMot); // time for second polynomial (max speed)
00539         std::vector<int> t3(nOfMot); // time for third polynomial (deceleration)
00540         std::vector<int> t4(nOfMot); // time for second polynomial (correction or padding)
00541         std::vector<int> t5(nOfMot); // time for second polynomial (rest of deceleration or padding)
00542         std::vector<int> t6(nOfMot); // time for second polynomial (padding)
00543         std::vector<int> p1_enc(nOfMot); // position between acceleration and max speed
00544         std::vector<int> p2_enc(nOfMot); // position between max speed and deceleration
00545         std::vector<int> p3_enc(nOfMot); // position between deceleration and (corr or padd)
00546         std::vector<int> p4_enc(nOfMot); // position between (corr or padd) and (rod or padd)
00547         std::vector<int> p5_enc(nOfMot); // position between (rod or padd) and padding
00548         std::vector<short> target(nOfMot);
00549         std::vector<short> time(nOfMot);
00550         std::vector<short> pp0(nOfMot); //polynomial coefficients
00551         std::vector<short> pp1(nOfMot);
00552         std::vector<short> pp2(nOfMot);
00553         std::vector<short> pp3(nOfMot);
00554         for (int i = 0; i < nOfMot; i++) {
00555                 speed[i] = getSpeed(distance[i], amax, maxtime-maxpadding);
00556                 if (speed[i] < 0)
00557                         return;
00558                 if(speed[i] == 0) // only when distance == 0, avoid division by zero
00559                         speed[i] = amax;
00560                 corrspeed[i] = distance[i] % speed[i];
00561                 corrspeed[i] -= corrspeed[i] % amax; // round down to multiple of amax
00562                 t1[i] = speed[i] / amax; 
00563                 p1_enc[i] = start_enc[i] + dir[i] * t1[i] * speed[i] / 2;
00564                 t2[i] = (distance[i] - speed[i] * speed[i] / amax) / speed[i]; // div speed, rest in correction polynomial
00565                 p2_enc[i] = p1_enc[i] + dir[i] * t2[i] * speed[i];
00566                 t3[i] = (speed[i] - corrspeed[i]) / amax;
00567                 p3_enc[i] = p2_enc[i] + dir[i] * t3[i] * (speed[i] + corrspeed[i]) / 2;
00568                 t4[i] = 1;
00569                 p4_enc[i] = p3_enc[i] + dir[i] * corrspeed[i];
00570                 t5[i] = corrspeed[i] / amax;
00571                 if (t5[i] == 0) // if padding polynomial (corrspeed == 0), lengh is 1
00572                         t5[i] = 1;
00573                 p5_enc[i] = p4_enc[i] + dir[i] * t5[i] * corrspeed[i] / 2;
00574                 t6[i] = maxtime - t1[i] - t2[i] - t3[i] - t4[i] - t5[i];
00575                 if(VDEBUG && (i == 0)){
00576                         std::cout << "\nparams axis " << i+1 << ":" << \
00577                         "\n distance: " << distance[i] << \
00578                         "\n dir: " << dir[i] << \
00579                         "\n speed: " << speed[i] << \
00580                         "\n correctionspeed: " << corrspeed[i] << \
00581                         "\n t1: " << t1[i] << \
00582                         "\n t2: " << t2[i] << \
00583                         "\n t3: " << t3[i] << \
00584                         "\n t4: " << t4[i] << \
00585                         "\n t5: " << t5[i] << \
00586                         "\n t6: " << t6[i] << \
00587                         "\n start_enc " << start_enc[i] << \
00588                         "\n p1_enc: " << p1_enc[i] << \
00589                         "\n p2_enc: " << p2_enc[i] << \
00590                         "\n p3_enc: " << p3_enc[i] << \
00591                         "\n p4_enc: " << p4_enc[i] << \
00592                         "\n p5_enc: " << p5_enc[i] << \
00593                         "\n target_enc " << target_enc[i] << std::endl;
00594                 }
00595         }
00596         
00597         //Polynomial 1 (acceleration)
00598         for (int i = 0; i < nOfMot; i++) {
00599                 if(!tooShortDistance[i]){
00600                         target[i] = static_cast<short>(p1_enc[i]);
00601                         time[i] = static_cast<short>(t1[i]);
00602                         pp0[i] = static_cast<short>(start_enc[i]);
00603                         pp1[i] = 0; 
00604                         pp2[i] = static_cast<short>(1024*(0.5 * dir[i] * amax));
00605                         pp3[i] = 0; 
00606                 }
00607                 else{ 
00608                         target[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] / 5);
00609                         time[i] = static_cast<short>(maxtime / 6);
00610                         pp0[i] = static_cast<short>(start_enc[i]);
00611                         pp1[i] = 0;
00612                         pp2[i] = 0; 
00613                         pp3[i] = 0; 
00614                 }
00615                 if(VDEBUG && (i == 0)){
00616                         std::cout << "pp axis " << i+1 << ":"\
00617                         "\t target: " << target[i] << \
00618                         "\t time: " << time[i] << \
00619                         "\tpp0: " << pp0[i] << \
00620                         ", pp1: " << pp1[i] << \
00621                         ", pp2: " << pp2[i] << \
00622                         ", pp3: " << pp3[i] << std::endl;
00623                 }
00624         }
00625         if (t.Elapsed())
00626                 return;
00627         std::vector<short> polynomial;
00628         for(int i = 0; i < nOfMot; ++i) {
00629                 polynomial.push_back(time[i]);
00630                 polynomial.push_back(target[i]);
00631                 polynomial.push_back(pp0[i]);
00632                 polynomial.push_back(pp1[i]);
00633                 polynomial.push_back(pp2[i]);
00634                 polynomial.push_back(pp3[i]);
00635         }
00636         setAndStartPolyMovement(polynomial, exactflag, 2);
00637         
00638         //Polynomial 2 (speed)
00639         for (int i = 0; i < nOfMot; i++) {
00640                 if(!tooShortDistance[i]){
00641                         target[i] = static_cast<short>(p2_enc[i]);
00642                         time[i] = static_cast<short>(t2[i]);
00643                         pp0[i] = static_cast<short>(p1_enc[i]);
00644                         pp1[i] = static_cast<short>(64*(dir[i] * speed[i]));
00645                         pp2[i] = 0; 
00646                         pp3[i] = 0;
00647                 }
00648                 else{ 
00649                         target[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 2 / 5);
00650                         time[i] = static_cast<short>(maxtime / 6);
00651                         pp0[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] / 5);
00652                         pp1[i] = 0;
00653                         pp2[i] = 0; 
00654                         pp3[i] = 0; 
00655                 }
00656                 if(VDEBUG && (i == 0)){
00657                         std::cout << "pp axis " << i+1 << ":"\
00658                         "\t target: " << target[i] << \
00659                         "\t time: " << time[i] << \
00660                         "\tpp0: " << pp0[i] << \
00661                         ", pp1: " << pp1[i] << \
00662                         ", pp2: " << pp2[i] << \
00663                         ", pp3: " << pp3[i] << std::endl;
00664                 }
00665         }
00666         if (t.Elapsed())
00667                 return;
00668         polynomial.clear();
00669         for(int i = 0; i < nOfMot; ++i) {
00670                 polynomial.push_back(time[i]);
00671                 polynomial.push_back(target[i]);
00672                 polynomial.push_back(pp0[i]);
00673                 polynomial.push_back(pp1[i]);
00674                 polynomial.push_back(pp2[i]);
00675                 polynomial.push_back(pp3[i]);
00676         }
00677         setAndStartPolyMovement(polynomial, exactflag, 2);
00678         
00679         //Polynomial 3 (deceleration)
00680         for (int i = 0; i < nOfMot; i++) {
00681                 if(!tooShortDistance[i]){
00682                         target[i] = static_cast<short>(p3_enc[i]);
00683                         time[i] = static_cast<short>(t3[i]);
00684                         pp0[i] = static_cast<short>(p2_enc[i]);
00685                         pp1[i] = static_cast<short>(64*(dir[i] * speed[i]));
00686                         pp2[i] = static_cast<short>(1024*(-0.5 * dir[i] * amax));
00687                         pp3[i] = 0;
00688                 }
00689                 else{ 
00690                         target[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 3 / 5);
00691                         time[i] = static_cast<short>(maxtime / 6);
00692                         pp0[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 2 / 5);
00693                         pp1[i] = 0;
00694                         pp2[i] = 0; 
00695                         pp3[i] = 0; 
00696                 }
00697                 if(VDEBUG && (i == 0)){
00698                         std::cout << "pp axis " << i+1 << ":"\
00699                         "\t target: " << target[i] << \
00700                         "\t time: " << time[i] << \
00701                         "\tpp0: " << pp0[i] << \
00702                         ", pp1: " << pp1[i] << \
00703                         ", pp2: " << pp2[i] << \
00704                         ", pp3: " << pp3[i] << std::endl;
00705                 }
00706         }
00707         if (t.Elapsed())
00708                 return;
00709         polynomial.clear();
00710         for(int i = 0; i < nOfMot; ++i) {
00711                 polynomial.push_back(time[i]);
00712                 polynomial.push_back(target[i]);
00713                 polynomial.push_back(pp0[i]);
00714                 polynomial.push_back(pp1[i]);
00715                 polynomial.push_back(pp2[i]);
00716                 polynomial.push_back(pp3[i]);
00717         }
00718         setAndStartPolyMovement(polynomial, exactflag, 2);
00719         
00720         //Polynomial 4 (correction or padding if correction speed == 0)
00721         for (int i = 0; i < nOfMot; i++) {
00722                 if(!tooShortDistance[i]){
00723                         target[i] = static_cast<short>(p4_enc[i]);
00724                         time[i] = static_cast<short>(t4[i]);
00725                         pp0[i] = static_cast<short>(p3_enc[i]);
00726                         pp1[i] = static_cast<short>(64*(dir[i] * corrspeed[i]));
00727                         pp2[i] = 0; 
00728                         pp3[i] = 0;
00729                 }
00730                 else{ 
00731                         target[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 4 / 5);
00732                         time[i] = static_cast<short>(maxtime / 6);
00733                         pp0[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 3 / 5);
00734                         pp1[i] = 0;
00735                         pp2[i] = 0; 
00736                         pp3[i] = 0; 
00737                 }
00738                 if(VDEBUG && (i == 0)){
00739                         std::cout << "pp axis " << i+1 << ":"\
00740                         "\t target: " << target[i] << \
00741                         "\t time: " << time[i] << \
00742                         "\tpp0: " << pp0[i] << \
00743                         ", pp1: " << pp1[i] << \
00744                         ", pp2: " << pp2[i] << \
00745                         ", pp3: " << pp3[i] << std::endl;
00746                 }
00747         }
00748         if (t.Elapsed())
00749                 return;
00750         polynomial.clear();
00751         for(int i = 0; i < nOfMot; ++i) {
00752                 polynomial.push_back(time[i]);
00753                 polynomial.push_back(target[i]);
00754                 polynomial.push_back(pp0[i]);
00755                 polynomial.push_back(pp1[i]);
00756                 polynomial.push_back(pp2[i]);
00757                 polynomial.push_back(pp3[i]);
00758         }
00759         setAndStartPolyMovement(polynomial, exactflag, 2);
00760         
00761         //Polynomial 5 (deceleration or padding if correction speed == 0)
00762         for (int i = 0; i < nOfMot; i++) {
00763                 if(!tooShortDistance[i]){
00764                         target[i] = static_cast<short>(p5_enc[i]);
00765                         time[i] = static_cast<short>(t5[i]);
00766                         pp0[i] = static_cast<short>(p4_enc[i]);
00767                         pp1[i] = static_cast<short>(64*(dir[i] * corrspeed[i]));
00768                         if (corrspeed[i] != 0) {
00769                                 pp2[i] = static_cast<short>(1024*(-0.5 * dir[i] * amax));
00770                         } else {
00771                                 pp2[i] = 0;
00772                         }
00773                         pp3[i] = 0;
00774                 }
00775                 else{ 
00776                         target[i] = static_cast<short>(target_enc[i]);
00777                         time[i] = static_cast<short>(maxtime / 6);
00778                         pp0[i] = static_cast<short>(start_enc[i] + dir[i] * distance[i] * 4 / 5);
00779                         pp1[i] = 0;
00780                         pp2[i] = 0; 
00781                         pp3[i] = 0; 
00782                 }
00783                 if(VDEBUG && (i == 0)){
00784                         std::cout << "pp axis " << i+1 << ":"\
00785                         "\t target: " << target[i] << \
00786                         "\t time: " << time[i] << \
00787                         "\tpp0: " << pp0[i] << \
00788                         ", pp1: " << pp1[i] << \
00789                         ", pp2: " << pp2[i] << \
00790                         ", pp3: " << pp3[i] << std::endl;
00791                 }
00792         }
00793         if (t.Elapsed())
00794                 return;
00795         polynomial.clear();
00796         for(int i = 0; i < nOfMot; ++i) {
00797                 polynomial.push_back(time[i]);
00798                 polynomial.push_back(target[i]);
00799                 polynomial.push_back(pp0[i]);
00800                 polynomial.push_back(pp1[i]);
00801                 polynomial.push_back(pp2[i]);
00802                 polynomial.push_back(pp3[i]);
00803         }
00804         setAndStartPolyMovement(polynomial, exactflag, 2);
00805         
00806         //Polynomial 6 (padding)
00807         for (int i = 0; i < nOfMot; i++) {
00808                 if(!tooShortDistance[i]){
00809                         target[i] = static_cast<short>(target_enc[i]);
00810                         time[i] = static_cast<short>(t6[i]);
00811                         pp0[i] = static_cast<short>(p5_enc[i]);
00812                         pp1[i] = 0;
00813                         pp2[i] = 0;
00814                         pp3[i] = 0;
00815                 }
00816                 else{ 
00817                         target[i] = static_cast<short>(target_enc[i]);
00818                         time[i] = static_cast<short>(maxtime - (maxtime / 6) * 5);
00819                         pp0[i] = static_cast<short>(target_enc[i]);
00820                         pp1[i] = 0;
00821                         pp2[i] = 0; 
00822                         pp3[i] = 0; 
00823                 }
00824                 if(VDEBUG && (i == 0)){
00825                         std::cout << "pp axis " << i+1 << ":"\
00826                         "\t target: " << target[i] << \
00827                         "\t time: " << time[i] << \
00828                         "\tpp0: " << pp0[i] << \
00829                         ", pp1: " << pp1[i] << \
00830                         ", pp2: " << pp2[i] << \
00831                         ", pp3: " << pp3[i] << std::endl;
00832                 }
00833         }
00834         if (t.Elapsed())
00835                 return;
00836         polynomial.clear();
00837         for(int i = 0; i < nOfMot; ++i) {
00838                 polynomial.push_back(time[i]);
00839                 polynomial.push_back(target[i]);
00840                 polynomial.push_back(pp0[i]);
00841                 polynomial.push_back(pp1[i]);
00842                 polynomial.push_back(pp2[i]);
00843                 polynomial.push_back(pp3[i]);
00844         }
00845         setAndStartPolyMovement(polynomial, exactflag, 1); // no next & start movement
00846         
00847         // wait for end of linear movement
00848         if(wait){
00849                 waitFor(MSF_NLINMOV, timeout);
00850         }
00851 }
00853 void CLMBase::movLM(double X, double Y, double Z,
00854                     double Al, double Be, double Ga,
00855                     bool exactflag, double vmax, bool wait, int tolerance, long timeout) {
00856 
00857         double arr_tarpos[6] = {X, Y, Z, Al, Be, Ga};
00858         // target position in cartesian units
00859         double arr_actpos[6];
00860         // current position in cartesian units, NO REFRESH, SINCE ALREADY DONE IN moveRobotLinearTo()
00861         getCoordinates(arr_actpos[0], arr_actpos[1], arr_actpos[2], arr_actpos[3], arr_actpos[4], arr_actpos[5], false);
00862 
00863         movLM2P(arr_actpos[0], arr_actpos[1], arr_actpos[2], arr_actpos[3], arr_actpos[4], arr_actpos[5],
00864                 arr_tarpos[0], arr_tarpos[1], arr_tarpos[2], arr_tarpos[3], arr_tarpos[4], arr_tarpos[5],
00865                 exactflag, vmax, wait, tolerance, timeout);
00866 
00867 }
00868 
00869 void CLMBase::moveRobotLinearTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached, int waitTimeout) {
00870         // refresh encoders to make sure we use the right actual position
00871         base->recvMPS();
00872 
00873         movLM(x, y, z, phi, theta, psi, _activatePositionController, _maximumVelocity, waitUntilReached, 100, waitTimeout);
00874 }
00875 
00876 void CLMBase::moveRobotLinearTo(std::vector<double> coordinates, bool waitUntilReached, int waitTimeout) {
00877         moveRobotLinearTo( coordinates.at(0), coordinates.at(1), coordinates.at(2), coordinates.at(3), coordinates.at(4), coordinates.at(5), waitUntilReached, waitTimeout);
00878 }
00879 
00880 void CLMBase::moveRobotTo(double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached, int waitTimeout) {
00881 
00882         // TODO: remove call of old implementation
00883 //      std::cout << "moveRobotTo(): calling old implementation." << std::endl;
00884 //      CikBase::moveRobotTo(x, y, z, phi, theta, psi, waitUntilReached, waitTimeout);
00885 //      return;
00886         
00887         // current position in cartesian units (with encoder refresh)
00888         double cp[6];
00889         getCoordinates(cp[0], cp[1], cp[2], cp[3], cp[4], cp[5], true);
00890 
00891         movP2P(cp[0], cp[1], cp[2], cp[3], cp[4], cp[5], x, y, z, phi, theta, psi,
00892                 _activatePositionController, _maximumVelocity, waitUntilReached, waitTimeout);
00893 }
00894 
00895 void CLMBase::moveRobotTo(std::vector<double> coordinates, bool waitUntilReached, int waitTimeout) {
00896         moveRobotTo( coordinates.at(0), coordinates.at(1), coordinates.at(2), coordinates.at(3), coordinates.at(4), coordinates.at(5), waitUntilReached, waitTimeout);  
00897 }
00898 
00899 // set maximum linear velocity in mm/s
00900 void CLMBase::setMaximumLinearVelocity(double maximumVelocity) {
00901         if (maximumVelocity < 1)
00902                 maximumVelocity = 1;
00903         if (maximumVelocity > 300)
00904                 maximumVelocity = 300;
00905         _maximumVelocity = maximumVelocity;
00906 }
00907 double CLMBase::getMaximumLinearVelocity() const {
00908         return _maximumVelocity;
00909 }
00910 
00911 void CLMBase::setActivatePositionController(bool activate) {
00912         _activatePositionController = activate;
00913 }
00914 bool CLMBase::getActivatePositionController() {
00915         return _activatePositionController;
00916 }
00917 
00918 


kni
Author(s): Martin Günther
autogenerated on Thu Jun 6 2019 21:42:34