waypoint.hpp

Go to the documentation of this file.

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

#ifndef ECL_MANIPULATORS_WAYPOINT_HPP_
#define ECL_MANIPULATORS_WAYPOINT_HPP_

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

#include <string>
#include "types.hpp"
#include <ecl/containers/array.hpp>
#include <ecl/utilities/parameter.hpp>

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

namespace ecl {

/*****************************************************************************
** Class [WayPoint<ManipulatorAngleType>]
*****************************************************************************/
template<enum ManipulatorAngleType Type = JointAngles>
class WayPoint {
    private:
        WayPoint() {};
};

/*****************************************************************************
** Class [WayPoint<unsigned int N,JointAngles>]
*****************************************************************************/
template <>
class WayPoint<JointAngles>
{
    public:
        typedef Array<double> JointDataArray; 
        /******************************************
        ** Constructors
        *******************************************/
        WayPoint() :
            name(""),
            duration(0.0),
            rates_configured(false),
            accelerations_configured(false)
        {}

        WayPoint(const unsigned int &dimension) :
            name(""),
            duration(0.0),
            rates_configured(false),
            accelerations_configured(false),
            angle_positions(JointDataArray::Constant(dimension,0.0)),
            angle_rates(JointDataArray::Constant(dimension,0.0)),
            angle_accelerations(JointDataArray::Constant(dimension,0.0)),
            nominal_rates(JointDataArray::Constant(dimension,1.00))
        {}

        WayPoint( const WayPoint<JointAngles> &waypoint) :
                name(waypoint.name()),
                        duration(waypoint.duration()),
                        rates_configured(waypoint.rates_configured()),
                        accelerations_configured(waypoint.accelerations_configured()),
                        angle_positions(waypoint.angle_positions),
                        angle_rates(waypoint.angle_rates),
                        angle_accelerations(waypoint.angle_accelerations),
                        nominal_rates(waypoint.nominal_rates)
                {}

        virtual ~WayPoint() {}

        /******************************************
        ** Configuration
        *******************************************/
        unsigned int dimension() const { return angle_positions.size(); }

        void redimension(const unsigned int &dimension) {
                angle_positions = JointDataArray::Constant(dimension,0.0);
                angle_rates = JointDataArray::Constant(dimension,0.0);
                angle_accelerations = JointDataArray::Constant(dimension,0.0);
                nominal_rates = JointDataArray::Constant(dimension,0.0);
        }
        Array<double>& angles() { return angle_positions; }
        const Array<double>& angles() const { return angle_positions; }

        Array<double>& rates() {
            rates_configured(true);
            return angle_rates;
        }
        const Array<double>& rates() const{
            return angle_rates;
        }

        Array<double>& accelerations() {
            accelerations_configured(true);
            return angle_accelerations;
        }
        const Array<double>& accelerations() const {
            return angle_accelerations;
        }

        Array<double>& nominalRates() { return nominal_rates; }
        void nominalRates(double nom_rate) {
                for ( unsigned int i = 0; i < nominal_rates.size(); ++i) {
                        nominal_rates[i] = nom_rate;
                }
        }

        bool operator==(const WayPoint<JointAngles>& other) const {
                for ( unsigned int i = 0; i < angle_positions.size(); ++i ) {
                        if ( angle_positions[i] != other.angle_positions[i] ) {
                                return false;
                        }
                }
                return true;
        }

        bool operator!=(const WayPoint<JointAngles>& other) const {
                for ( unsigned int i = 0; i < angle_positions.size(); ++i ) {
                        if ( angle_positions[i] != other.angle_positions[i] ) {
                                return true;
                        }
                }
                return false;
        }

        bool approx(const WayPoint<JointAngles> &other, const double& epsilon = 0.01) {
                for ( unsigned int i = 0; i < angle_positions.size(); ++i ) {
                        if ( std::fabs(angle_positions[i] - other.angle_positions[i]) > epsilon ) {
                                return false;
                        }
                }
                return true;
        }

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

        /******************************************
        ** Parameters
        *******************************************/
        Parameter<std::string> name; 
        Parameter<double> duration; 
        Parameter<bool> rates_configured; 
        Parameter<bool> accelerations_configured; 
    private:
        JointDataArray angle_positions;
        JointDataArray angle_rates; // this is rates *at* the point, currently unused by ycs stuff, but might be useful
        JointDataArray angle_accelerations;
        JointDataArray nominal_rates; // this is rough rates between points (i.e. slopes)
};

/*****************************************************************************
** Implementation [WayPoint][Streaming]
*****************************************************************************/

template <typename OutputStream>
OutputStream& operator << (OutputStream &ostream, WayPoint<JointAngles> &waypoint) {

        Format< Array<double> > format; format.precision(2); format.width(6);
    if ( waypoint.name() != "" ) {
        ostream << "  Name                 : " << waypoint.name() << "\n";
    }
    ostream << "  Angles               : " << format(waypoint.angle_positions) << "\n";
    if (waypoint.rates_configured()) {
        ostream << "  Rates                : " << format(waypoint.angle_rates) << "\n";
    }
    if (waypoint.accelerations_configured()) {
        ostream << "  Accelerations        : " << format(waypoint.angle_accelerations) << "\n";
    }
    ostream << "  Nominal Rates        : " << format(waypoint.nominal_rates) << "\n";
    ostream.flush();
    return ostream;
}

/*****************************************************************************
** Typedefs
*****************************************************************************/

//typedef Waypoint<JointAngles> JointAngleWaypoint; /**< @brief Convenient typedef for joint angle waypoints. **/

}; // namespace ecl

#endif /* ECL_MANIPULATORS_WAYPOINT_HPP_ */