trajectory.hpp
Go to the documentation of this file.
00001 
00008 /*****************************************************************************
00009 ** Ifdefs
00010 *****************************************************************************/
00011 
00012 #ifndef ECL_MANIPULATORS_TRAJECTORY_HPP_
00013 #define ECL_MANIPULATORS_TRAJECTORY_HPP_
00014 
00015 /*****************************************************************************
00016 ** Includes
00017 *****************************************************************************/
00018 
00019 #include <iostream>
00020 #include <cmath>
00021 #include <sstream>
00022 #include <string>
00023 #include <vector>
00024 #include "types.hpp"
00025 #include "waypoint.hpp"
00026 #include <ecl/geometry/polynomial.hpp>
00027 #include <ecl/geometry/function_math.hpp>
00028 #include <ecl/geometry/smooth_linear_spline.hpp>
00029 #include <ecl/geometry/spline_function.hpp>
00030 #include <ecl/geometry/tension_spline.hpp>
00031 #include <ecl/concepts/streams.hpp>
00032 #include <ecl/exceptions/macros.hpp>
00033 #include <ecl/exceptions/data_exception.hpp>
00034 #include <ecl/exceptions/standard_exception.hpp>
00035 #include <ecl/utilities/parameter.hpp>
00036 
00037 /*****************************************************************************
00038 ** Namespaces
00039 *****************************************************************************/
00040 
00041 namespace ecl {
00042 
00043 /*****************************************************************************
00044 ** Interface [Trajectory][PrimaryTemplate]
00045 *****************************************************************************/
00056 template <enum ManipulatorAngleType Type = JointAngles>
00057 class Trajectory {
00058     private:
00065         Trajectory () {};
00066         virtual ~Trajectory () {};
00067 };
00068 
00069 /*****************************************************************************
00070 ** Interface [Trajectory][JointAngles]
00071 *****************************************************************************/
00085 template <>
00086 class Trajectory<JointAngles> {
00087     public:
00093         Trajectory(const char* name_identifier = "") :
00094             name(name_identifier)
00095         {}
00102         Trajectory(const unsigned int& dimension, const char* name_identifier = "") :
00103             name(name_identifier),
00104             spline_functions(dimension),
00105             max_accelerations(Array<double>::Constant(dimension,0.0))
00106         {}
00107 
00113         virtual ~Trajectory() { clearSplines(); waypoints.clear(); }
00117         void clear() { clearSplines(); waypoints.clear(); }
00123         unsigned int size() const { return waypoints.size(); }
00124 
00131         unsigned int dimension() const { return max_accelerations.size(); }
00132 
00140         void redimension(const unsigned int &dim) {
00141                 clear();
00142                 spline_functions.resize(dim);
00143                 max_accelerations = Array<double>::Constant(dim,0.0);
00144         }
00145 
00157         void append(const WayPoint<JointAngles> &waypoint) ecl_assert_throw_decl(StandardException) {
00158                 ecl_assert_throw(dimension() == waypoint.dimension(), StandardException(LOC, InvalidInputError, "Mismatched trajectory and waypoint dimensions.") );
00159             waypoints.push_back(waypoint);
00160         }
00161 
00170         WayPoint<JointAngles>& last() {
00171                 return waypoints.back();
00172         }
00173 
00182         const WayPoint<JointAngles>& last() const {
00183                 return waypoints.back();
00184         }
00195         const WayPoint<JointAngles>& waypoint(const unsigned int &index) ecl_assert_throw_decl(StandardException) {
00196                 ecl_assert_throw(index < waypoints.size(), StandardException(LOC, OutOfRangeError, "The index used exceeds the number of waypoints stored in the trajectory."));
00197                 return waypoints[index];
00198         }
00199 
00221         void tensionSplineInterpolation(const double &tension) ecl_assert_throw_decl(StandardException);
00243         void linearSplineInterpolation() throw(StandardException);
00244 
00252         double operator ()(const unsigned int& joint, const double& time ) ecl_assert_throw_decl(StandardException);
00260         double  derivative(const unsigned int& joint, const double& time ) ecl_assert_throw_decl(StandardException);
00268         double dderivative(const unsigned int& joint, const double& time ) ecl_assert_throw_decl(StandardException);
00269 
00270         /*********************
00271         ** Accessors
00272         **********************/
00286         Array<double>& maxAccelerations() { return max_accelerations; };
00295         void maxAccelerations(const double &max_acceleration) {
00296                 for ( unsigned int i = 0; i < dimension(); ++i ) {
00297                         max_accelerations[i] = max_acceleration;
00298                 }
00299         }
00305         const Array<double>& maxAccelerations() const { return max_accelerations; };
00306 
00307         /******************************************
00308         ** Streaming
00309         *******************************************/
00321         template <typename OutputStream>
00322         friend OutputStream& operator << (OutputStream &ostream, Trajectory<JointAngles> &trajectory);
00323 
00324         /******************************************
00325         ** Parameters
00326         *******************************************/
00334         const double& duration() const { return trajectory_duration; }
00335         Parameter<std::string> name; 
00337     private:
00338         std::vector< WayPoint<JointAngles> > waypoints;
00339         Array< std::vector<GenericSplineFunction*> > spline_functions;
00340         Array< double> max_accelerations;
00341         double trajectory_duration;
00342 
00348 public:
00349         void clearSplines();
00354         Array<SmoothLinearSpline> generateLinearSplines() throw(DataException<int>);
00361         Array<TensionSpline> generateTensionSplines(const double& tension, const double initial_time = 0.0);
00367         bool validateWaypoints(unsigned int min_no_waypoints);
00373         bool initialiseWaypointDurations();
00381         void updateDuration();
00382 };
00383 
00384 /*****************************************************************************
00385 ** Implementation [Trajectory][Streaming]
00386 *****************************************************************************/
00387 
00388 template <typename OutputStream>
00389 OutputStream& operator << (OutputStream &ostream, Trajectory<JointAngles> &trajectory) {
00390 
00391         ecl_compile_time_concept_check(StreamConcept<OutputStream>);
00392 
00393     for (unsigned int i = 0; i < trajectory.waypoints.size(); ++i ) {
00394         ostream << "Waypoint " << i << "\n";
00395         ostream << trajectory.waypoints[i] << "\n";
00396     }
00397     ostream.flush();
00398     return ostream;
00399 }
00400 
00401 }; // namespace ecl
00402 
00403 #endif /* ECL_MANIPULATORS_TRAJECTORY_HPP_ */


ecl_manipulators
Author(s): Daniel Stonier
autogenerated on Sun Oct 5 2014 23:35:27