$search
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