trajectory.hpp
Go to the documentation of this file.
1 
8 /*****************************************************************************
9 ** Ifdefs
10 *****************************************************************************/
11 
12 #ifndef ECL_MANIPULATORS_TRAJECTORY_HPP_
13 #define ECL_MANIPULATORS_TRAJECTORY_HPP_
14 
15 /*****************************************************************************
16 ** Includes
17 *****************************************************************************/
18 
19 #include <iostream>
20 #include <cmath>
21 #include <sstream>
22 #include <string>
23 #include <vector>
24 #include "types.hpp"
25 #include "waypoint.hpp"
31 #include <ecl/concepts/streams.hpp>
36 
37 /*****************************************************************************
38 ** Namespaces
39 *****************************************************************************/
40 
41 namespace ecl {
42 
43 /*****************************************************************************
44 ** Interface [Trajectory][PrimaryTemplate]
45 *****************************************************************************/
56 template <enum ManipulatorAngleType Type = JointAngles>
57 class Trajectory {
58  private:
65  Trajectory () {};
66  virtual ~Trajectory () {};
67 };
68 
69 /*****************************************************************************
70 ** Interface [Trajectory][JointAngles]
71 *****************************************************************************/
85 template <>
87  public:
93  Trajectory(const char* name_identifier = "") :
94  name(name_identifier)
95  {}
102  Trajectory(const unsigned int& dimension, const char* name_identifier = "") :
103  name(name_identifier),
104  spline_functions(dimension),
105  max_accelerations(Array<double>::Constant(dimension,0.0))
106  {}
107 
113  virtual ~Trajectory() { clearSplines(); waypoints.clear(); }
117  void clear() { clearSplines(); waypoints.clear(); }
123  unsigned int size() const { return waypoints.size(); }
124 
131  unsigned int dimension() const { return max_accelerations.size(); }
132 
140  void redimension(const unsigned int &dim) {
141  clear();
142  spline_functions.resize(dim);
143  max_accelerations = Array<double>::Constant(dim,0.0);
144  }
145 
157  void append(const WayPoint<JointAngles> &waypoint) {
158  ecl_assert_throw(dimension() == waypoint.dimension(), StandardException(LOC, InvalidInputError, "Mismatched trajectory and waypoint dimensions.") );
159  waypoints.push_back(waypoint);
160  }
161 
171  return waypoints.back();
172  }
173 
182  const WayPoint<JointAngles>& last() const {
183  return waypoints.back();
184  }
195  const WayPoint<JointAngles>& waypoint(const unsigned int &index) {
196  ecl_assert_throw(index < waypoints.size(), StandardException(LOC, OutOfRangeError, "The index used exceeds the number of waypoints stored in the trajectory."));
197  return waypoints[index];
198  }
199 
221  void tensionSplineInterpolation(const double &tension);
243  void linearSplineInterpolation();
244 
252  double operator ()(const unsigned int& joint, const double& time );
260  double derivative(const unsigned int& joint, const double& time );
268  double dderivative(const unsigned int& joint, const double& time );
269 
270  /*********************
271  ** Accessors
272  **********************/
286  Array<double>& maxAccelerations() { return max_accelerations; };
295  void maxAccelerations(const double &max_acceleration) {
296  for ( unsigned int i = 0; i < dimension(); ++i ) {
297  max_accelerations[i] = max_acceleration;
298  }
299  }
305  const Array<double>& maxAccelerations() const { return max_accelerations; };
306 
307  /******************************************
308  ** Streaming
309  *******************************************/
321  template <typename OutputStream>
322  friend OutputStream& operator << (OutputStream &ostream, Trajectory<JointAngles> &trajectory);
323 
324  /******************************************
325  ** Parameters
326  *******************************************/
334  const double& duration() const { return trajectory_duration; }
337  private:
338  std::vector< WayPoint<JointAngles> > waypoints;
342 
348 public:
349  void clearSplines();
354  Array<SmoothLinearSpline> generateLinearSplines();
361  Array<TensionSpline> generateTensionSplines(const double& tension, const double initial_time = 0.0);
367  bool validateWaypoints(unsigned int min_no_waypoints);
373  bool initialiseWaypointDurations();
381  void updateDuration();
382 };
383 
384 /*****************************************************************************
385 ** Implementation [Trajectory][Streaming]
386 *****************************************************************************/
387 
388 template <typename OutputStream>
389 OutputStream& operator << (OutputStream &ostream, Trajectory<JointAngles> &trajectory) {
390 
392 
393  for (unsigned int i = 0; i < trajectory.waypoints.size(); ++i ) {
394  ostream << "Waypoint " << i << "\n";
395  ostream << trajectory.waypoints[i] << "\n";
396  }
397  ostream.flush();
398  return ostream;
399 }
400 
401 }; // namespace ecl
402 
403 #endif /* ECL_MANIPULATORS_TRAJECTORY_HPP_ */
Array< double > max_accelerations
Definition: trajectory.hpp:340
WayPoint< JointAngles > & last()
Get the last waypoint in the trajectory.
Definition: trajectory.hpp:170
const WayPoint< JointAngles > & waypoint(const unsigned int &index)
Get a handle to the specified waypoint.
Definition: trajectory.hpp:195
Trajectory(const unsigned int &dimension, const char *name_identifier="")
Configures the joint dimension, and also (optionally) the trajectory name.
Definition: trajectory.hpp:102
void append(const WayPoint< JointAngles > &waypoint)
Appends a waypoint to the trajectory specification.
Definition: trajectory.hpp:157
unsigned int size() const
Number of waypoints in the trajectory.
Definition: trajectory.hpp:123
#define LOC
Trajectory()
Unreachable constructor.
Definition: trajectory.hpp:65
Array< double > & maxAccelerations()
Handle to the max accelerations array, use to initialise the polynomial.
Definition: trajectory.hpp:286
Waypoint definitions for various manipulator contexts.
Type definitions for manipulators.
unsigned int dimension() const
Dimension, or degrees of freedom for the trajectory.
Definition: trajectory.hpp:131
Array< std::vector< GenericSplineFunction * > > spline_functions
Definition: trajectory.hpp:339
virtual ~Trajectory()
Definition: trajectory.hpp:66
#define ecl_assert_throw(expression, exception)
InvalidInputError
Parameter< std::string > name
String name identifier.
Definition: trajectory.hpp:335
OutOfRangeError
void redimension(const unsigned int &dim)
Reset the dimension, or degrees of freedom for the trajectory.
Definition: trajectory.hpp:140
virtual ~Trajectory()
Default destructor.
Definition: trajectory.hpp:113
const double & duration() const
Total trajectory duration (in seconds).
Definition: trajectory.hpp:334
Trajectory(const char *name_identifier="")
Default constructor, only (optionally) configures the trajectory name.
Definition: trajectory.hpp:93
unsigned int dimension() const
Return the waypoint dimension.
Definition: waypoint.hpp:160
#define ecl_compile_time_concept_check(Model)
static ConstantArray< Type, Size > Constant(const Type &value)
Primary template for manipulator trajectories.
Definition: trajectory.hpp:57
WayPoint specialisation with joint (motor) angle storage format.
Definition: waypoint.hpp:73
const Array< double > & maxAccelerations() const
Non-modifiable handle to the max accelerations array.
Definition: trajectory.hpp:305
Joint angle representations are being used.
Definition: types.hpp:33
const WayPoint< JointAngles > & last() const
Const version of the last waypoint in the trajectory.
Definition: trajectory.hpp:182
void clear()
Clear the waypoints and splines.
Definition: trajectory.hpp:117
void maxAccelerations(const double &max_acceleration)
Configure the entire trajectory with a uniform max acceleration bound.
Definition: trajectory.hpp:295
std::vector< WayPoint< JointAngles > > waypoints
Definition: trajectory.hpp:338


ecl_manipulators
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:09:10