TrajectoryFactory.h
Go to the documentation of this file.
00001 
00008 #ifndef TRAJECTORY_FACTORY_H
00009 #define TRAJECTORY_FACTORY_H
00010 
00011 #include <robodyn_controllers/MotionLimiter.h>
00012 #include <kdl/frameacc.hpp>
00013 #include <kdl/jntarrayacc.hpp>
00014 #include <vector>
00015 #include <boost/shared_ptr.hpp>
00016 #include <robodyn_controllers/Trajectory.h>
00017 
00018 template <class P, template <class> class trajType = Trajectory>
00019 class TrajectoryFactory
00020 {
00021 public:
00022     TrajectoryFactory() {}
00023     virtual ~TrajectoryFactory() {}
00024 
00031     virtual boost::shared_ptr<trajType<P> > getTrajectory(const typename trajType<P>::poseType& startPose,
00032             const typename trajType<P>::poseType& goalPose, double durationTarget = -1.) const = 0;
00033 };
00034 
00035 template <class P, template <class> class trajType = Trajectory>
00036 class TrajectorySequenceFactory
00037 {
00038 public:
00039     typedef TrajectoryFactory<P, trajType> factoryType;
00040 
00041     TrajectorySequenceFactory() {}
00042 
00043     virtual ~TrajectorySequenceFactory() {}
00044 
00045     void setTrajectoryFactory(boost::shared_ptr<const TrajectoryFactory<P, trajType> > trajFactory_in)
00046     {
00047         trajFactory = trajFactory_in;
00048     }
00049 
00056     virtual boost::shared_ptr<TrajectorySequence<P, trajType> > getTrajectory(const typename trajType<P>::poseType& startPose,
00057             const std::vector<typename trajType<P>::poseType>& goalPoses,
00058             std::vector<double> durationTargets = std::vector<double>()) const;
00059 
00060 private:
00061     boost::shared_ptr<const TrajectoryFactory<P, trajType> > trajFactory;
00062 };
00063 
00064 template <template <class> class trajType = Trajectory>
00065 class JointTrajectoryFactory : public TrajectoryFactory<KDL::JntArrayAcc, trajType>, public JointMotionLimiter
00066 {
00067 };
00068 
00069 template <template <class> class trajType = Trajectory>
00070 class JointTrajectorySequenceFactory : public TrajectorySequenceFactory<KDL::JntArrayAcc, trajType>
00071 {
00072 };
00073 
00074 template <template <class> class trajType = Trajectory>
00075 class CartesianTrajectoryFactory : public TrajectoryFactory<KDL::FrameAcc, trajType>, public CartesianMotionLimiter
00076 {
00077 };
00078 
00079 template <template <class> class trajType = Trajectory>
00080 class CartesianTrajectorySequenceFactory : public TrajectorySequenceFactory<KDL::FrameAcc, trajType>
00081 {
00082 };
00083 
00084 typedef CartesianTrajectoryFactory<SynchedTrajectory> SynchedCartesianTrajectoryFactory;
00085 typedef CartesianTrajectorySequenceFactory<SynchedTrajectory> SynchedCartesianTrajectorySequenceFactory;
00086 
00087 template <class P, template <class> class trajType>
00088 boost::shared_ptr<TrajectorySequence<P, trajType> > TrajectorySequenceFactory<P, trajType>::getTrajectory(const typename trajType<P>::poseType& startPose,
00089         const std::vector<typename trajType<P>::poseType>& goalPoses,
00090         std::vector<double> durationTargets) const
00091 {
00092     boost::shared_ptr<TrajectorySequence<P, trajType> > trajSeq(new TrajectorySequence<P, trajType>);
00093     double durTarget;
00094     typename trajType<P>::poseType currStart = startPose;
00095 
00096     for (unsigned int i = 0; i < goalPoses.size(); ++i)
00097     {
00098         if (durationTargets.size() > i)
00099         {
00100             durTarget = durationTargets[i];
00101         }
00102         else
00103         {
00104             durTarget = -1.;
00105         }
00106 
00107         trajSeq->push_back(trajFactory->getTrajectory(currStart, goalPoses[i], durTarget));
00108         currStart = goalPoses[i];
00109     }
00110 
00111     return trajSeq;
00112 }
00113 
00114 #endif


robodyn_controllers
Author(s):
autogenerated on Sat Jun 8 2019 20:20:53