TrapezoidalVelocityTrajectoryFactory.cpp

Go to the documentation of this file.

#include <robodyn_controllers/TrapezoidalVelocityTrajectoryFactory.h>

boost::shared_ptr<JointTrajectory> TrapezoidalVelocityJointTrajectoryFactory::getTrajectory(const KDL::JntArrayAcc& startPose,
        const KDL::JntArrayAcc& goalPose, double timeToFinish) const
{
    boost::shared_ptr<JointTrajectory> traj(new TrapezoidalVelocityJointTrajectory(*this, startPose, goalPose, timeToFinish));
    return traj;
}

boost::shared_ptr<CartesianTrajectory> TrapezoidalVelocityCartesianTrajectoryFactory::getTrajectory(const KDL::FrameAcc& startPose,
        const KDL::FrameAcc& goalPose, double timeToFinish) const
{
    boost::shared_ptr<CartesianTrajectory> traj(new TrapezoidalVelocityCartesianTrajectory(*this, startPose, goalPose, timeToFinish));
    return traj;
}

boost::shared_ptr<SynchedCartesianTrajectory> TrapezoidalVelocitySynchedCartesianTrajectoryFactory::getTrajectory(const std::vector<KDL::FrameAcc>& startPoses,
        const std::vector<KDL::FrameAcc>& goalPoses, double timeToFinish) const
{
    if (startPoses.size() != goalPoses.size())
    {
        std::stringstream err;
        err << "TrapezoidalVelocityCartesianTrajectoryFactory::getTrajectory() - number of start poses and goal poses don't match";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.TrapezoidalVelocityCartesianTrajectoryFactory", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    std::vector<boost::shared_ptr<TrapezoidalVelocityCartesianTrajectory> > trajPtrs(startPoses.size());
    double duration = 0.;

    for (unsigned int i = 0; i < startPoses.size(); ++i)
    {
        trajPtrs[i].reset(new TrapezoidalVelocityCartesianTrajectory(*this, startPoses[i], goalPoses[i], timeToFinish));

        if (trajPtrs[i]->getDuration() > duration)
        {
            duration = trajPtrs[i]->getDuration();
        }
    }

    boost::shared_ptr<SynchedCartesianTrajectory> synchedTrajPtrs(new SynchedCartesianTrajectory);

    for (unsigned int i = 0; i < startPoses.size(); ++i)
    {
        trajPtrs[i]->setDuration(duration);
        synchedTrajPtrs->push_back(trajPtrs[i]);
    }

    return synchedTrajPtrs;
}