quintic_spline_segment.h
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 // Copyright (c) 2008, Willow Garage, Inc.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright notice,
8 // this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of PAL Robotics S.L. nor the names of its
13 // contributors may be used to endorse or promote products derived from
14 // this software without specific prior written permission.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 // POSSIBILITY OF SUCH DAMAGE.
28 
30 
31 #pragma once
32 
33 
34 #include <array>
35 #include <iterator>
36 #include <stdexcept>
38 
39 namespace trajectory_interface
40 {
41 
47 template<class ScalarType>
49 {
50 public:
51  typedef ScalarType Scalar;
52  typedef Scalar Time;
54 
62  : coefs_(),
63  duration_(static_cast<Scalar>(0)),
64  start_time_(static_cast<Scalar>(0)),
65  time_from_start_(static_cast<Scalar>(0))
66  {}
67 
74  QuinticSplineSegment(const Time& start_time,
75  const State& start_state,
76  const Time& end_time,
77  const State& end_state)
78  {
79  init(start_time, start_state, end_time, end_state);
80  }
81 
102  void init(const Time& start_time,
103  const State& start_state,
104  const Time& end_time,
105  const State& end_state);
106 
116  void sample(const Time& time, State& state) const
117  {
118  // Resize state data. Should be a no-op if appropriately sized
119  state.position.resize(coefs_.size());
120  state.velocity.resize(coefs_.size());
121  state.acceleration.resize(coefs_.size());
122 
123  // Sample each dimension
124  typedef typename std::vector<SplineCoefficients>::const_iterator ConstIterator;
125  for(ConstIterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
126  {
127  const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
128  sampleWithTimeBounds(*coefs_it,
129  duration_, (time - start_time_),
130  state.position[id], state.velocity[id], state.acceleration[id]);
131  }
132  }
133 
135  Time startTime() const {return start_time_;}
136 
138  Time endTime() const {return start_time_ + duration_;}
139 
142 
144  unsigned int size() const {return coefs_.size();}
145 
146 private:
147  typedef std::array<Scalar, 6> SplineCoefficients;
148 
153  std::vector<SplineCoefficients> coefs_;
157 
158  // These methods are borrowed from the previous controller's implementation
159  // TODO: Clean their implementation, use the Horner algorithm for more numerically stable polynomial evaluation
160  static void generatePowers(int n, const Scalar& x, Scalar* powers);
161 
162  static void computeCoefficients(const Scalar& start_pos,
163  const Scalar& end_pos,
164  const Scalar& time,
165  SplineCoefficients& coefficients);
166 
167  static void computeCoefficients(const Scalar& start_pos, const Scalar& start_vel,
168  const Scalar& end_pos, const Scalar& end_vel,
169  const Scalar& time,
170  SplineCoefficients& coefficients);
171 
172  static void computeCoefficients(const Scalar& start_pos, const Scalar& start_vel, const Scalar& start_acc,
173  const Scalar& end_pos, const Scalar& end_vel, const Scalar& end_acc,
174  const Scalar& time,
175  SplineCoefficients& coefficients);
176 
177  static void sample(const SplineCoefficients& coefficients, const Scalar& time,
178  Scalar& position, Scalar& velocity, Scalar& acceleration);
179 
180  static void sampleWithTimeBounds(const SplineCoefficients& coefficients, const Scalar& duration, const Scalar& time,
181  Scalar& position, Scalar& velocity, Scalar& acceleration);
182 };
183 
184 template<class ScalarType>
186  const State& start_state,
187  const Time& end_time,
188  const State& end_state)
189 {
190  // Preconditions
191  if (end_time < start_time)
192  {
193  throw(std::invalid_argument("Quintic spline segment can't be constructed: end_time < start_time."));
194  }
195  if (start_state.position.empty() || end_state.position.empty())
196  {
197  throw(std::invalid_argument("Quintic spline segment can't be constructed: Endpoint positions can't be empty."));
198  }
199  if (start_state.position.size() != end_state.position.size())
200  {
201  throw(std::invalid_argument("Quintic spline segment can't be constructed: Endpoint positions size mismatch."));
202  }
203 
204  const unsigned int dim = start_state.position.size();
205  const bool has_velocity = !start_state.velocity.empty() && !end_state.velocity.empty();
206  const bool has_acceleration = !start_state.acceleration.empty() && !end_state.acceleration.empty();
207 
208  if (has_velocity && dim != start_state.velocity.size())
209  {
210  throw(std::invalid_argument("Quintic spline segment can't be constructed: Start state velocity size mismatch."));
211  }
212  if (has_velocity && dim != end_state.velocity.size())
213  {
214  throw(std::invalid_argument("Quintic spline segment can't be constructed: End state velocity size mismatch."));
215  }
216  if (has_acceleration && dim!= start_state.acceleration.size())
217  {
218  throw(std::invalid_argument("Quintic spline segment can't be constructed: Start state acceleration size mismatch."));
219  }
220  if (has_acceleration && dim != end_state.acceleration.size())
221  {
222  throw(std::invalid_argument("Quintic spline segment can't be constructed: End state acceleratios size mismatch."));
223  }
224 
225  // Time data
226  start_time_ = start_time;
227  duration_ = end_time - start_time;
228  time_from_start_ = start_state.time_from_start;
229 
230  // Spline coefficients
231  coefs_.resize(dim);
232 
233  typedef typename std::vector<SplineCoefficients>::iterator Iterator;
234  if (!has_velocity)
235  {
236  // Linear interpolation
237  for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
238  {
239  const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
240 
241  computeCoefficients(start_state.position[id],
242  end_state.position[id],
243  duration_,
244  *coefs_it);
245  }
246  }
247  else if (!has_acceleration)
248  {
249  // Cubic interpolation
250  for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
251  {
252  const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
253 
254  computeCoefficients(start_state.position[id], start_state.velocity[id],
255  end_state.position[id], end_state.velocity[id],
256  duration_,
257  *coefs_it);
258  }
259  }
260  else
261  {
262  // Quintic interpolation
263  for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
264  {
265  const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
266 
267  computeCoefficients(start_state.position[id], start_state.velocity[id], start_state.acceleration[id],
268  end_state.position[id], end_state.velocity[id], end_state.acceleration[id],
269  duration_,
270  *coefs_it);
271  }
272  }
273 }
274 
275 template<class ScalarType>
277 {
278  powers[0] = 1.0;
279  for (int i=1; i<=n; ++i)
280  {
281  powers[i] = powers[i-1]*x;
282  }
283 }
284 
285 template<class ScalarType>
287 computeCoefficients(const Scalar& start_pos,
288  const Scalar& end_pos,
289  const Scalar& time,
290  SplineCoefficients& coefficients)
291 {
292  coefficients[0] = start_pos;
293  coefficients[1] = (time == 0.0) ? 0.0 : (end_pos - start_pos) / time;
294  coefficients[2] = 0.0;
295  coefficients[3] = 0.0;
296  coefficients[4] = 0.0;
297  coefficients[5] = 0.0;
298 }
299 
300 template<class ScalarType>
302 computeCoefficients(const Scalar& start_pos, const Scalar& start_vel,
303  const Scalar& end_pos, const Scalar& end_vel,
304  const Scalar& time,
305  SplineCoefficients& coefficients)
306 {
307  if (time == 0.0)
308  {
309  coefficients[0] = start_pos;
310  coefficients[1] = start_vel;
311  coefficients[2] = 0.0;
312  coefficients[3] = 0.0;
313  }
314  else
315  {
316  Scalar T[4];
317  generatePowers(3, time, T);
318 
319  coefficients[0] = start_pos;
320  coefficients[1] = start_vel;
321  coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
322  coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
323  }
324  coefficients[4] = 0.0;
325  coefficients[5] = 0.0;
326 }
327 
328 template<class ScalarType>
330 computeCoefficients(const Scalar& start_pos, const Scalar& start_vel, const Scalar& start_acc,
331  const Scalar& end_pos, const Scalar& end_vel, const Scalar& end_acc,
332  const Scalar& time,
333  SplineCoefficients& coefficients)
334 {
335  if (time == 0.0)
336  {
337  coefficients[0] = start_pos;
338  coefficients[1] = start_vel;
339  coefficients[2] = 0.5*start_acc;
340  coefficients[3] = 0.0;
341  coefficients[4] = 0.0;
342  coefficients[5] = 0.0;
343  }
344  else
345  {
346  Scalar T[6];
347  generatePowers(5, time, T);
348 
349  coefficients[0] = start_pos;
350  coefficients[1] = start_vel;
351  coefficients[2] = 0.5*start_acc;
352  coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
353  12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
354  coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
355  16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
356  coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
357  6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
358  }
359 }
360 
361 template<class ScalarType>
363 sample(const SplineCoefficients& coefficients, const Scalar& time,
364  Scalar& position, Scalar& velocity, Scalar& acceleration)
365 {
366  // create powers of time:
367  Scalar t[6];
368  generatePowers(5, time, t);
369 
370  position = t[0]*coefficients[0] +
371  t[1]*coefficients[1] +
372  t[2]*coefficients[2] +
373  t[3]*coefficients[3] +
374  t[4]*coefficients[4] +
375  t[5]*coefficients[5];
376 
377  velocity = t[0]*coefficients[1] +
378  2.0*t[1]*coefficients[2] +
379  3.0*t[2]*coefficients[3] +
380  4.0*t[3]*coefficients[4] +
381  5.0*t[4]*coefficients[5];
382 
383  acceleration = 2.0*t[0]*coefficients[2] +
384  6.0*t[1]*coefficients[3] +
385  12.0*t[2]*coefficients[4] +
386  20.0*t[3]*coefficients[5];
387 }
388 
389 template<class ScalarType>
391 sampleWithTimeBounds(const SplineCoefficients& coefficients, const Scalar& duration, const Scalar& time,
392  Scalar& position, Scalar& velocity, Scalar& acceleration)
393 {
394  if (time < 0)
395  {
396  Scalar _;
397  sample(coefficients, 0.0, position, _, _);
398  velocity = 0;
399  acceleration = 0;
400  }
401  else if (time > duration)
402  {
403  Scalar _;
404  sample(coefficients, duration, position, _, _);
405  velocity = 0;
406  acceleration = 0;
407  }
408  else
409  {
410  sample(coefficients, time,
411  position, velocity, acceleration);
412  }
413 }
414 
415 } // namespace
trajectory_interface
Definition: pos_vel_acc_state.h:36
trajectory_interface::QuinticSplineSegment::computeCoefficients
static void computeCoefficients(const Scalar &start_pos, const Scalar &end_pos, const Scalar &time, SplineCoefficients &coefficients)
Definition: quintic_spline_segment.h:287
trajectory_interface::QuinticSplineSegment::sampleWithTimeBounds
static void sampleWithTimeBounds(const SplineCoefficients &coefficients, const Scalar &duration, const Scalar &time, Scalar &position, Scalar &velocity, Scalar &acceleration)
Definition: quintic_spline_segment.h:391
trajectory_interface::QuinticSplineSegment::endTime
Time endTime() const
Definition: quintic_spline_segment.h:138
trajectory_interface::sample
Trajectory::const_iterator sample(const Trajectory &trajectory, const typename Trajectory::value_type::Time &time, typename Trajectory::value_type::State &state)
Sample a trajectory at a specified time.
Definition: trajectory_interface.h:138
pos_vel_acc_state.h
trajectory_interface::QuinticSplineSegment::duration_
Time duration_
Definition: quintic_spline_segment.h:154
trajectory_interface::PosVelAccState::position
std::vector< Scalar > position
Definition: pos_vel_acc_state.h:70
trajectory_interface::QuinticSplineSegment::init
void init(const Time &start_time, const State &start_state, const Time &end_time, const State &end_state)
Initialize segment from start and end states (boundary conditions).
Definition: quintic_spline_segment.h:185
trajectory_interface::QuinticSplineSegment::timeFromStart
Time timeFromStart() const
Definition: quintic_spline_segment.h:141
trajectory_interface::QuinticSplineSegment::time_from_start_
Time time_from_start_
Definition: quintic_spline_segment.h:156
trajectory_interface::PosVelAccState::time_from_start
Scalar time_from_start
Definition: pos_vel_acc_state.h:73
trajectory_interface::QuinticSplineSegment::SplineCoefficients
std::array< Scalar, 6 > SplineCoefficients
Definition: quintic_spline_segment.h:147
trajectory_interface::PosVelAccState::velocity
std::vector< Scalar > velocity
Definition: pos_vel_acc_state.h:71
trajectory_interface::QuinticSplineSegment::size
unsigned int size() const
Definition: quintic_spline_segment.h:144
trajectory_interface::QuinticSplineSegment::coefs_
std::vector< SplineCoefficients > coefs_
Definition: quintic_spline_segment.h:153
trajectory_interface::PosVelAccState
Multi-dof trajectory state containing position, velocity and acceleration data.
Definition: pos_vel_acc_state.h:43
trajectory_interface::QuinticSplineSegment::Time
Scalar Time
Definition: quintic_spline_segment.h:52
trajectory_interface::QuinticSplineSegment::generatePowers
static void generatePowers(int n, const Scalar &x, Scalar *powers)
Definition: quintic_spline_segment.h:276
trajectory_interface::PosVelAccState::acceleration
std::vector< Scalar > acceleration
Definition: pos_vel_acc_state.h:72
trajectory_interface::QuinticSplineSegment::State
PosVelAccState< Scalar > State
Definition: quintic_spline_segment.h:53
trajectory_interface::QuinticSplineSegment::startTime
Time startTime() const
Definition: quintic_spline_segment.h:135
trajectory_interface::QuinticSplineSegment::Scalar
ScalarType Scalar
Definition: quintic_spline_segment.h:51
trajectory_interface::QuinticSplineSegment::start_time_
Time start_time_
Definition: quintic_spline_segment.h:155
trajectory_interface::QuinticSplineSegment::sample
void sample(const Time &time, State &state) const
Sample the segment at a specified time.
Definition: quintic_spline_segment.h:116
trajectory_interface::QuinticSplineSegment::QuinticSplineSegment
QuinticSplineSegment()
Creates an empty segment.
Definition: quintic_spline_segment.h:61
trajectory_interface::QuinticSplineSegment::QuinticSplineSegment
QuinticSplineSegment(const Time &start_time, const State &start_state, const Time &end_time, const State &end_state)
Construct segment from start and end states (boundary conditions).
Definition: quintic_spline_segment.h:74
trajectory_interface::QuinticSplineSegment
Class representing a multi-dimensional quintic spline segment with a start and end time.
Definition: quintic_spline_segment.h:48


joint_trajectory_controller
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Fri May 24 2024 02:41:24