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