TrajectoryFactory.h

Go to the documentation of this file.

#ifndef TRAJECTORY_FACTORY_H
#define TRAJECTORY_FACTORY_H

#include <robodyn_controllers/MotionLimiter.h>
#include <kdl/frameacc.hpp>
#include <kdl/jntarrayacc.hpp>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <robodyn_controllers/Trajectory.h>

template <class P, template <class> class trajType = Trajectory>
class TrajectoryFactory
{
public:
    TrajectoryFactory() {}
    virtual ~TrajectoryFactory() {}

    virtual boost::shared_ptr<trajType<P> > getTrajectory(const typename trajType<P>::poseType& startPose,
            const typename trajType<P>::poseType& goalPose, double durationTarget = -1.) const = 0;
};

template <class P, template <class> class trajType = Trajectory>
class TrajectorySequenceFactory
{
public:
    typedef TrajectoryFactory<P, trajType> factoryType;

    TrajectorySequenceFactory() {}

    virtual ~TrajectorySequenceFactory() {}

    void setTrajectoryFactory(boost::shared_ptr<const TrajectoryFactory<P, trajType> > trajFactory_in)
    {
        trajFactory = trajFactory_in;
    }

    virtual boost::shared_ptr<TrajectorySequence<P, trajType> > getTrajectory(const typename trajType<P>::poseType& startPose,
            const std::vector<typename trajType<P>::poseType>& goalPoses,
            std::vector<double> durationTargets = std::vector<double>()) const;

private:
    boost::shared_ptr<const TrajectoryFactory<P, trajType> > trajFactory;
};

template <template <class> class trajType = Trajectory>
class JointTrajectoryFactory : public TrajectoryFactory<KDL::JntArrayAcc, trajType>, public JointMotionLimiter
{
};

template <template <class> class trajType = Trajectory>
class JointTrajectorySequenceFactory : public TrajectorySequenceFactory<KDL::JntArrayAcc, trajType>
{
};

template <template <class> class trajType = Trajectory>
class CartesianTrajectoryFactory : public TrajectoryFactory<KDL::FrameAcc, trajType>, public CartesianMotionLimiter
{
};

template <template <class> class trajType = Trajectory>
class CartesianTrajectorySequenceFactory : public TrajectorySequenceFactory<KDL::FrameAcc, trajType>
{
};

typedef CartesianTrajectoryFactory<SynchedTrajectory> SynchedCartesianTrajectoryFactory;
typedef CartesianTrajectorySequenceFactory<SynchedTrajectory> SynchedCartesianTrajectorySequenceFactory;

template <class P, template <class> class trajType>
boost::shared_ptr<TrajectorySequence<P, trajType> > TrajectorySequenceFactory<P, trajType>::getTrajectory(const typename trajType<P>::poseType& startPose,
        const std::vector<typename trajType<P>::poseType>& goalPoses,
        std::vector<double> durationTargets) const
{
    boost::shared_ptr<TrajectorySequence<P, trajType> > trajSeq(new TrajectorySequence<P, trajType>);
    double durTarget;
    typename trajType<P>::poseType currStart = startPose;

    for (unsigned int i = 0; i < goalPoses.size(); ++i)
    {
        if (durationTargets.size() > i)
        {
            durTarget = durationTargets[i];
        }
        else
        {
            durTarget = -1.;
        }

        trajSeq->push_back(trajFactory->getTrajectory(currStart, goalPoses[i], durTarget));
        currStart = goalPoses[i];
    }

    return trajSeq;
}

#endif