00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00028
00030
00031 #ifndef TRAJECTORY_INTERFACE_QUINTIC_SPLINE_SEGMENT_H
00032 #define TRAJECTORY_INTERFACE_QUINTIC_SPLINE_SEGMENT_H
00033
00034 #include <iterator>
00035 #include <stdexcept>
00036
00037 #include <boost/array.hpp>
00038 #include <boost/foreach.hpp>
00039
00040 #include <trajectory_interface/pos_vel_acc_state.h>
00041
00042 namespace trajectory_interface
00043 {
00044
00050 template<class ScalarType>
00051 class QuinticSplineSegment
00052 {
00053 public:
00054 typedef ScalarType Scalar;
00055 typedef Scalar Time;
00056 typedef PosVelAccState<Scalar> State;
00057
00064 QuinticSplineSegment()
00065 : coefs_(),
00066 duration_(static_cast<Scalar>(0)),
00067 start_time_(static_cast<Scalar>(0))
00068 {}
00069
00076 QuinticSplineSegment(const Time& start_time,
00077 const State& start_state,
00078 const Time& end_time,
00079 const State& end_state)
00080 {
00081 init(start_time, start_state, end_time, end_state);
00082 }
00083
00104 void init(const Time& start_time,
00105 const State& start_state,
00106 const Time& end_time,
00107 const State& end_state);
00108
00118 void sample(const Time& time, State& state) const
00119 {
00120
00121 state.position.resize(coefs_.size());
00122 state.velocity.resize(coefs_.size());
00123 state.acceleration.resize(coefs_.size());
00124
00125
00126 typedef typename std::vector<SplineCoefficients>::const_iterator ConstIterator;
00127 for(ConstIterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
00128 {
00129 const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
00130 sampleWithTimeBounds(*coefs_it,
00131 duration_, (time - start_time_),
00132 state.position[id], state.velocity[id], state.acceleration[id]);
00133 }
00134 }
00135
00137 Time startTime() const {return start_time_;}
00138
00140 Time endTime() const {return start_time_ + duration_;}
00141
00143 unsigned int size() const {return coefs_.size();}
00144
00145 private:
00146 typedef boost::array<Scalar, 6> SplineCoefficients;
00147
00152 std::vector<SplineCoefficients> coefs_;
00153 Time duration_;
00154 Time start_time_;
00155
00156
00157
00158 static void generatePowers(int n, const Scalar& x, Scalar* powers);
00159
00160 static void computeCoefficients(const Scalar& start_pos,
00161 const Scalar& end_pos,
00162 const Scalar& time,
00163 SplineCoefficients& coefficients);
00164
00165 static void computeCoefficients(const Scalar& start_pos, const Scalar& start_vel,
00166 const Scalar& end_pos, const Scalar& end_vel,
00167 const Scalar& time,
00168 SplineCoefficients& coefficients);
00169
00170 static void computeCoefficients(const Scalar& start_pos, const Scalar& start_vel, const Scalar& start_acc,
00171 const Scalar& end_pos, const Scalar& end_vel, const Scalar& end_acc,
00172 const Scalar& time,
00173 SplineCoefficients& coefficients);
00174
00175 static void sample(const SplineCoefficients& coefficients, const Scalar& time,
00176 Scalar& position, Scalar& velocity, Scalar& acceleration);
00177
00178 static void sampleWithTimeBounds(const SplineCoefficients& coefficients, const Scalar& duration, const Scalar& time,
00179 Scalar& position, Scalar& velocity, Scalar& acceleration);
00180 };
00181
00182 template<class ScalarType>
00183 void QuinticSplineSegment<ScalarType>::init(const Time& start_time,
00184 const State& start_state,
00185 const Time& end_time,
00186 const State& end_state)
00187 {
00188
00189 if (end_time < start_time)
00190 {
00191 throw(std::invalid_argument("Quintic spline segment can't be constructed: end_time < start_time."));
00192 }
00193 if (start_state.position.empty() || end_state.position.empty())
00194 {
00195 throw(std::invalid_argument("Quintic spline segment can't be constructed: Endpoint positions can't be empty."));
00196 }
00197 if (start_state.position.size() != end_state.position.size())
00198 {
00199 throw(std::invalid_argument("Quintic spline segment can't be constructed: Endpoint positions size mismatch."));
00200 }
00201
00202 const unsigned int dim = start_state.position.size();
00203 const bool has_velocity = !start_state.velocity.empty() && !end_state.velocity.empty();
00204 const bool has_acceleration = !start_state.acceleration.empty() && !end_state.acceleration.empty();
00205
00206 if (has_velocity && dim != start_state.velocity.size())
00207 {
00208 throw(std::invalid_argument("Quintic spline segment can't be constructed: Start state velocity size mismatch."));
00209 }
00210 if (has_velocity && dim != end_state.velocity.size())
00211 {
00212 throw(std::invalid_argument("Quintic spline segment can't be constructed: End state velocity size mismatch."));
00213 }
00214 if (has_acceleration && dim!= start_state.acceleration.size())
00215 {
00216 throw(std::invalid_argument("Quintic spline segment can't be constructed: Start state acceleration size mismatch."));
00217 }
00218 if (has_acceleration && dim != end_state.acceleration.size())
00219 {
00220 throw(std::invalid_argument("Quintic spline segment can't be constructed: End state acceleratios size mismatch."));
00221 }
00222
00223
00224 start_time_ = start_time;
00225 duration_ = end_time - start_time;
00226
00227
00228 coefs_.resize(dim);
00229
00230 typedef typename std::vector<SplineCoefficients>::iterator Iterator;
00231 if (!has_velocity)
00232 {
00233
00234 for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
00235 {
00236 const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
00237
00238 computeCoefficients(start_state.position[id],
00239 end_state.position[id],
00240 duration_,
00241 *coefs_it);
00242 }
00243 }
00244 else if (!has_acceleration)
00245 {
00246
00247 for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
00248 {
00249 const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
00250
00251 computeCoefficients(start_state.position[id], start_state.velocity[id],
00252 end_state.position[id], end_state.velocity[id],
00253 duration_,
00254 *coefs_it);
00255 }
00256 }
00257 else
00258 {
00259
00260 for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
00261 {
00262 const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
00263
00264 computeCoefficients(start_state.position[id], start_state.velocity[id], start_state.acceleration[id],
00265 end_state.position[id], end_state.velocity[id], end_state.acceleration[id],
00266 duration_,
00267 *coefs_it);
00268 }
00269 }
00270 }
00271
00272 template<class ScalarType>
00273 inline void QuinticSplineSegment<ScalarType>::generatePowers(int n, const Scalar& x, Scalar* powers)
00274 {
00275 powers[0] = 1.0;
00276 for (int i=1; i<=n; ++i)
00277 {
00278 powers[i] = powers[i-1]*x;
00279 }
00280 }
00281
00282 template<class ScalarType>
00283 void QuinticSplineSegment<ScalarType>::
00284 computeCoefficients(const Scalar& start_pos,
00285 const Scalar& end_pos,
00286 const Scalar& time,
00287 SplineCoefficients& coefficients)
00288 {
00289 coefficients[0] = start_pos;
00290 coefficients[1] = (time == 0.0) ? 0.0 : (end_pos - start_pos) / time;
00291 coefficients[2] = 0.0;
00292 coefficients[3] = 0.0;
00293 coefficients[4] = 0.0;
00294 coefficients[5] = 0.0;
00295 }
00296
00297 template<class ScalarType>
00298 void QuinticSplineSegment<ScalarType>::
00299 computeCoefficients(const Scalar& start_pos, const Scalar& start_vel,
00300 const Scalar& end_pos, const Scalar& end_vel,
00301 const Scalar& time,
00302 SplineCoefficients& coefficients)
00303 {
00304 if (time == 0.0)
00305 {
00306 coefficients[0] = start_pos;
00307 coefficients[1] = start_vel;
00308 coefficients[2] = 0.0;
00309 coefficients[3] = 0.0;
00310 }
00311 else
00312 {
00313 Scalar T[4];
00314 generatePowers(3, time, T);
00315
00316 coefficients[0] = start_pos;
00317 coefficients[1] = start_vel;
00318 coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
00319 coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
00320 }
00321 coefficients[4] = 0.0;
00322 coefficients[5] = 0.0;
00323 }
00324
00325 template<class ScalarType>
00326 void QuinticSplineSegment<ScalarType>::
00327 computeCoefficients(const Scalar& start_pos, const Scalar& start_vel, const Scalar& start_acc,
00328 const Scalar& end_pos, const Scalar& end_vel, const Scalar& end_acc,
00329 const Scalar& time,
00330 SplineCoefficients& coefficients)
00331 {
00332 if (time == 0.0)
00333 {
00334 coefficients[0] = start_pos;
00335 coefficients[1] = start_vel;
00336 coefficients[2] = 0.5*start_acc;
00337 coefficients[3] = 0.0;
00338 coefficients[4] = 0.0;
00339 coefficients[5] = 0.0;
00340 }
00341 else
00342 {
00343 Scalar T[6];
00344 generatePowers(5, time, T);
00345
00346 coefficients[0] = start_pos;
00347 coefficients[1] = start_vel;
00348 coefficients[2] = 0.5*start_acc;
00349 coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
00350 12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
00351 coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
00352 16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
00353 coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
00354 6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
00355 }
00356 }
00357
00358 template<class ScalarType>
00359 void QuinticSplineSegment<ScalarType>::
00360 sample(const SplineCoefficients& coefficients, const Scalar& time,
00361 Scalar& position, Scalar& velocity, Scalar& acceleration)
00362 {
00363
00364 Scalar t[6];
00365 generatePowers(5, time, t);
00366
00367 position = t[0]*coefficients[0] +
00368 t[1]*coefficients[1] +
00369 t[2]*coefficients[2] +
00370 t[3]*coefficients[3] +
00371 t[4]*coefficients[4] +
00372 t[5]*coefficients[5];
00373
00374 velocity = t[0]*coefficients[1] +
00375 2.0*t[1]*coefficients[2] +
00376 3.0*t[2]*coefficients[3] +
00377 4.0*t[3]*coefficients[4] +
00378 5.0*t[4]*coefficients[5];
00379
00380 acceleration = 2.0*t[0]*coefficients[2] +
00381 6.0*t[1]*coefficients[3] +
00382 12.0*t[2]*coefficients[4] +
00383 20.0*t[3]*coefficients[5];
00384 }
00385
00386 template<class ScalarType>
00387 void QuinticSplineSegment<ScalarType>::
00388 sampleWithTimeBounds(const SplineCoefficients& coefficients, const Scalar& duration, const Scalar& time,
00389 Scalar& position, Scalar& velocity, Scalar& acceleration)
00390 {
00391 if (time < 0)
00392 {
00393 Scalar _;
00394 sample(coefficients, 0.0, position, _, _);
00395 velocity = 0;
00396 acceleration = 0;
00397 }
00398 else if (time > duration)
00399 {
00400 Scalar _;
00401 sample(coefficients, duration, position, _, _);
00402 velocity = 0;
00403 acceleration = 0;
00404 }
00405 else
00406 {
00407 sample(coefficients, time,
00408 position, velocity, acceleration);
00409 }
00410 }
00411
00412 }
00413
00414 #endif // header guard