Trajectory.h

Go to the documentation of this file.

#ifndef TRAJECTORY_H
#define TRAJECTORY_H

#include <kdl/frameacc.hpp>
#include <kdl/jntarrayacc.hpp>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <stdexcept>
#include "nasa_common_logging/Logger.h"

template <class P>
class Trajectory
{
public:
    typedef P poseType;

    Trajectory() : duration(0.) {}
    virtual ~Trajectory() {}

    inline double getDuration() const {return duration;}

    virtual void getPose(double time, poseType& pose) = 0;

    virtual void setDuration(double duration_in);

private:
    double duration;
};

typedef Trajectory<KDL::JntArrayAcc> JointTrajectory;
typedef Trajectory<KDL::FrameAcc> CartesianTrajectory;

template <class P>
class SynchedTrajectory : public Trajectory<std::vector<P> >
{
public:
    typedef Trajectory<std::vector<P> > baseType;

    SynchedTrajectory() {}
    virtual ~SynchedTrajectory() {}

    void push_back(boost::shared_ptr<Trajectory<P> > traj);

    void pop_back()
    {
        trajectories.pop_back();

        if (trajectories.empty()) { baseType::setDuration(0.); }
    }

    void clear()
    {
        trajectories.clear();
        baseType::setDuration(0.);
    }

    inline unsigned int size() {return trajectories.size();}

    inline boost::shared_ptr<const Trajectory<typename baseType::poseType> > operator[](unsigned int i)
    {return trajectories[i];}

    virtual void getPose(double time, typename baseType::poseType& pose);

private:
    typedef std::vector<boost::shared_ptr<Trajectory<P> > > trajectories_type;
    trajectories_type trajectories;
};

typedef SynchedTrajectory<KDL::FrameAcc> SynchedCartesianTrajectory;

template <class P, template <class> class trajType = Trajectory>
class TrajectorySequence : public trajType<P>
{
public:
    typedef trajType<P> baseType;

    TrajectorySequence() {}
    virtual ~TrajectorySequence() {}

    void push_back(boost::shared_ptr<baseType> traj)
    {
        trajectories.push_back(traj);
        baseType::setDuration(baseType::getDuration() + traj->getDuration());
    }

    void pop_back()
    {
        baseType::setDuration(baseType::getDuration() - trajectories.back()->getDuration());
        trajectories.pop_back();
    }

    void clear()
    {
        trajectories.clear();
        baseType::setDuration(0.);
    }

    virtual void getPose(double time, typename baseType::poseType& pose);

private:
    typedef std::vector<boost::shared_ptr<baseType> > trajectories_type;
    trajectories_type trajectories;
};

template <class P>
class SynchedTrajectorySequence : public TrajectorySequence<P, SynchedTrajectory> {};

typedef TrajectorySequence<KDL::JntArrayAcc> JointTrajectorySequence;
typedef TrajectorySequence<KDL::FrameAcc> CartesianTrajectorySequence;
typedef SynchedTrajectorySequence<KDL::FrameAcc> SynchedCartesianTrajectorySequence;

template <class P>
void Trajectory<P>::setDuration(double duration_in)
{
    if (duration_in >= 0)
    {
        duration = duration_in;
    }
    else
    {
        std::stringstream err;
        err << "Trajectory::setDuration() - duration must be greater than or equal to 0";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Trajectory", log4cpp::Priority::ERROR, err.str());
        throw std::invalid_argument(err.str());
    }
}

template <class P>
void SynchedTrajectory<P>::push_back(boost::shared_ptr<Trajectory<P> > traj)
{
    if (trajectories.empty())
    {
        baseType::setDuration(traj->getDuration());
    }
    else if (baseType::getDuration() != traj->getDuration())
    {
        std::stringstream err;
        err << "SynchedTrajectory::push_back() - durations of trajectories don't match";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.SynchedTrajectory", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    trajectories.push_back(traj);
}

template <class P>
void SynchedTrajectory<P>::getPose(double time, typename baseType::poseType& pose)
{
    pose.resize(trajectories.size());

    for (unsigned int i = 0; i < trajectories.size(); ++i)
    {
        trajectories[i]->getPose(time, pose[i]);
    }
}

template <class P, template <class> class trajType>
void TrajectorySequence<P, trajType>::getPose(double time, typename baseType::poseType& pose)
{
    if (trajectories.empty())
    {
        std::stringstream err;
        err << "TrajectorySequence::getPose() - trajectory empty";
        throw std::runtime_error(err.str());
        return;
    }

    typename trajectories_type::iterator it = trajectories.begin();

    while (time > (*it)->getDuration())
    {
        time -= (*it)->getDuration();
        ++it;

        if (it == trajectories.end())
        {
            throw std::runtime_error("Time exceeds trajectory duration");
        }
    }

    (*it)->getPose(time, pose);
}

#endif