spline_functions.cpp
Go to the documentation of this file.
00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2010  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * spline_functions.cpp
00020  *
00021  *  Created on: 28.01.2011
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #include <katana/spline_functions.h>
00026 
00027 namespace katana
00028 {
00029 inline void generatePowers(int n, double x, double* powers)
00030 {
00031   powers[0] = 1.0;
00032   for (int i = 1; i <= n; i++)
00033   {
00034     powers[i] = powers[i - 1] * x;
00035   }
00036 }
00037 
00038 void sampleCubicSpline(const std::vector<double>& coefficients, double time, double& position, double& velocity,
00039                        double& acceleration)
00040 {
00041   // create powers of time:
00042   double t[4];
00043   generatePowers(3, time, t);
00044 
00045   position = t[0] * coefficients[0] + t[1] * coefficients[1] + t[2] * coefficients[2] + t[3] * coefficients[3];
00046   velocity = t[0] * coefficients[1] + 2.0 * t[1] * coefficients[2] + 3.0 * t[2] * coefficients[3];
00047   acceleration = 2.0 * t[0] * coefficients[2] + 6.0 * t[1] * coefficients[3];
00048 }
00049 
00050 void getCubicSplineCoefficients(double start_pos, double start_vel, double end_pos, double end_vel, double time,
00051                                 std::vector<double>& coefficients)
00052 {
00053   coefficients.resize(4);
00054 
00055   if (time == 0.0)
00056   {
00057     coefficients[0] = end_pos;
00058     coefficients[1] = end_vel;
00059     coefficients[2] = 0.0;
00060     coefficients[3] = 0.0;
00061   }
00062   else
00063   {
00064     double T[4];
00065     generatePowers(3, time, T);
00066 
00067     coefficients[0] = start_pos;
00068     coefficients[1] = start_vel;
00069     coefficients[2] = (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2];
00070     coefficients[3] = (2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3];
00071   }
00072 }
00073 
00074 // copied from KNI
00075 void splineCoefficients(int steps, double *timearray, double *encoderarray, double *arr_p1, double *arr_p2,
00076                         double *arr_p3, double *arr_p4)
00077 {
00078 
00079   int i, j; // counter variables
00080 
00081   // calculate time differences between points and b-coefficients
00082   double deltatime[steps];
00083   double b[steps];
00084   for (i = 0; i < steps; i++)
00085   {
00086     deltatime[i] = timearray[i + 1] - timearray[i];
00087     b[i] = 1.0 / deltatime[i];
00088   }
00089 
00090   // calculate a-coefficients
00091   double a[steps - 1];
00092   for (i = 0; i < (steps - 1); i++)
00093   {
00094     a[i] = (2 / deltatime[i]) + (2 / deltatime[i + 1]);
00095   }
00096 
00097   // build up the right hand side of the linear system
00098   double c[steps];
00099   double d[steps + 1];
00100   d[0] = 0; // f_1' and f_n' equal zero
00101   d[steps] = 0;
00102   for (i = 0; i < steps; i++)
00103   {
00104     c[i] = (encoderarray[i + 1] - encoderarray[i]) / (deltatime[i] * deltatime[i]);
00105   }
00106   for (i = 0; i < (steps - 1); i++)
00107   {
00108     d[i + 1] = 3 * (c[i] + c[i + 1]);
00109   }
00110 
00111   // compose A * f' = d
00112   double Alin[steps - 1][steps]; // last column of Alin is right hand side
00113 
00114   // fill with zeros
00115   for (i = 0; i < (steps - 1); i++)
00116   {
00117     for (j = 0; j < steps; j++)
00118     {
00119       Alin[i][j] = 0.0;
00120     }
00121   }
00122   // insert values
00123   for (i = 0; i < (steps - 1); i++)
00124   {
00125     if (i == 0)
00126     {
00127       Alin[0][0] = a[0];
00128       Alin[0][1] = b[1];
00129       Alin[0][steps - 1] = d[1];
00130     }
00131     else
00132     {
00133       Alin[i][i - 1] = b[i];
00134       Alin[i][i] = a[i];
00135       Alin[i][i + 1] = b[i + 1];
00136       Alin[i][steps - 1] = d[i + 1];
00137     }
00138   }
00139 
00140   // solve linear equation
00141   boost::numeric::ublas::matrix<double> ublas_A(steps - 1, steps - 1);
00142   boost::numeric::ublas::matrix<double> ublas_b(steps - 1, 1);
00143   for (i = 0; i < (steps - 1); i++)
00144   {
00145     for (j = 0; j < (steps - 1); j++)
00146     {
00147       ublas_A(i, j) = Alin[i][j];
00148     }
00149     ublas_b(i, 0) = Alin[i][steps - 1];
00150   }
00151   boost::numeric::ublas::permutation_matrix<unsigned int> piv(steps - 1);
00152   lu_factorize(ublas_A, piv);
00153   lu_substitute(ublas_A, piv, ublas_b);
00154 
00155   // save result in derivatives array
00156   double derivatives[steps + 1];
00157   derivatives[0] = 0;
00158   for (i = 0; i < (steps - 1); i++)
00159   {
00160     derivatives[i + 1] = ublas_b(i, 0);
00161   }
00162   derivatives[steps] = 0;
00163   // build the hermite polynom with difference scheme
00164   // Q(t) = a0 + (b0 + (c0 + d0 * t) * (t - 1)) * t = a0 + (b0 - c0) * t +
00165   //   (c0 - d0) * t^2 + d0 * t^3 = p0 + p1 * t + p2 * t^2 + p3 * t^3
00166   double a0, b0, c0, d0;
00167   for (i = 0; i < steps; i++)
00168   {
00169     a0 = encoderarray[i];
00170     b0 = encoderarray[i + 1] - a0;
00171     c0 = b0 - deltatime[i] * derivatives[i];
00172     d0 = deltatime[i] * (derivatives[i + 1] + derivatives[i]) - 2 * b0;
00173     arr_p1[i] = a0;
00174     arr_p2[i] = b0 - c0;
00175     arr_p3[i] = c0 - d0;
00176     arr_p4[i] = d0;
00177 
00178     // added MG: normalize to segment duration (of course we could do some simplifications here)
00179     arr_p2[i] = arr_p2[i] / deltatime[i];
00180     arr_p3[i] = arr_p3[i] / pow(deltatime[i], 2);
00181     arr_p4[i] = arr_p4[i] / pow(deltatime[i], 3);
00182   }
00183 }
00184 
00185 void sampleSplineWithTimeBounds(const std::vector<double>& coefficients, double duration, double time,
00186                                 double& position, double& velocity, double& acceleration)
00187 {
00188   if (time < 0)
00189   {
00190     double _;
00191     sampleCubicSpline(coefficients, 0.0, position, _, _);
00192     velocity = 0;
00193     acceleration = 0;
00194   }
00195   else if (time > duration)
00196   {
00197     double _;
00198     sampleCubicSpline(coefficients, duration, position, _, _);
00199     velocity = 0;
00200     acceleration = 0;
00201   }
00202   else
00203   {
00204     sampleCubicSpline(coefficients, time, position, velocity, acceleration);
00205   }
00206 }
00207 
00208 } // namespace katana


katana
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:35