trajectory.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef ECL_MANIPULATORS_TRAJECTORY_HPP_
#define ECL_MANIPULATORS_TRAJECTORY_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <iostream>
#include <cmath>
#include <sstream>
#include <string>
#include <vector>
#include "types.hpp"
#include "waypoint.hpp"
#include <ecl/geometry/polynomial.hpp>
#include <ecl/geometry/function_math.hpp>
#include <ecl/geometry/smooth_linear_spline.hpp>
#include <ecl/geometry/spline_function.hpp>
#include <ecl/geometry/tension_spline.hpp>
#include <ecl/concepts/streams.hpp>
#include <ecl/exceptions/macros.hpp>
#include <ecl/exceptions/data_exception.hpp>
#include <ecl/exceptions/standard_exception.hpp>
#include <ecl/utilities/parameter.hpp>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace ecl {

/*****************************************************************************
** Interface [Trajectory][PrimaryTemplate]
*****************************************************************************/
template <enum ManipulatorAngleType Type = JointAngles>
class Trajectory {
    private:
        Trajectory () {};
        virtual ~Trajectory () {};
};

/*****************************************************************************
** Interface [Trajectory][JointAngles]
*****************************************************************************/
template <>
class Trajectory<JointAngles> {
    public:
        Trajectory(const char* name_identifier = "") :
            name(name_identifier)
        {}
        Trajectory(const unsigned int& dimension, const char* name_identifier = "") :
            name(name_identifier),
            spline_functions(dimension),
            max_accelerations(Array<double>::Constant(dimension,0.0))
        {}

        virtual ~Trajectory() { clearSplines(); waypoints.clear(); }
        void clear() { clearSplines(); waypoints.clear(); }
        unsigned int size() const { return waypoints.size(); }

        unsigned int dimension() const { return max_accelerations.size(); }

        void redimension(const unsigned int &dim) {
                clear();
                spline_functions.resize(dim);
                max_accelerations = Array<double>::Constant(dim,0.0);
        }

        void append(const WayPoint<JointAngles> &waypoint) {
                ecl_assert_throw(dimension() == waypoint.dimension(), StandardException(LOC, InvalidInputError, "Mismatched trajectory and waypoint dimensions.") );
            waypoints.push_back(waypoint);
        }

        WayPoint<JointAngles>& last() {
                return waypoints.back();
        }

        const WayPoint<JointAngles>& last() const {
                return waypoints.back();
        }
        const WayPoint<JointAngles>& waypoint(const unsigned int &index) {
                ecl_assert_throw(index < waypoints.size(), StandardException(LOC, OutOfRangeError, "The index used exceeds the number of waypoints stored in the trajectory."));
                return waypoints[index];
        }

        void tensionSplineInterpolation(const double &tension);
        void linearSplineInterpolation();

        double operator ()(const unsigned int& joint, const double& time );
        double  derivative(const unsigned int& joint, const double& time );
        double dderivative(const unsigned int& joint, const double& time );

        /*********************
        ** Accessors
        **********************/
        Array<double>& maxAccelerations() { return max_accelerations; };
        void maxAccelerations(const double &max_acceleration) {
                for ( unsigned int i = 0; i < dimension(); ++i ) {
                        max_accelerations[i] = max_acceleration;
                }
        }
        const Array<double>& maxAccelerations() const { return max_accelerations; };

        /******************************************
        ** Streaming
        *******************************************/
        template <typename OutputStream>
        friend OutputStream& operator << (OutputStream &ostream, Trajectory<JointAngles> &trajectory);

        /******************************************
        ** Parameters
        *******************************************/
        const double& duration() const { return trajectory_duration; }
        Parameter<std::string> name; 
    private:
        std::vector< WayPoint<JointAngles> > waypoints;
        Array< std::vector<GenericSplineFunction*> > spline_functions;
        Array< double> max_accelerations;
        double trajectory_duration;

public:
        void clearSplines();
        Array<SmoothLinearSpline> generateLinearSplines();
        Array<TensionSpline> generateTensionSplines(const double& tension, const double initial_time = 0.0);
        bool validateWaypoints(unsigned int min_no_waypoints);
        bool initialiseWaypointDurations();
        void updateDuration();
};

/*****************************************************************************
** Implementation [Trajectory][Streaming]
*****************************************************************************/

template <typename OutputStream>
OutputStream& operator << (OutputStream &ostream, Trajectory<JointAngles> &trajectory) {

        ecl_compile_time_concept_check(StreamConcept<OutputStream>);

    for (unsigned int i = 0; i < trajectory.waypoints.size(); ++i ) {
        ostream << "Waypoint " << i << "\n";
        ostream << trajectory.waypoints[i] << "\n";
    }
    ostream.flush();
    return ostream;
}

}; // namespace ecl

#endif /* ECL_MANIPULATORS_TRAJECTORY_HPP_ */