MinJerkTrajectoryFactory.cpp

Go to the documentation of this file.

#include <robodyn_controllers/MinJerkTrajectoryFactory.h>
#include <robodyn_controllers/MinJerkTrajectory.h>
#include "nasa_common_logging/Logger.h"

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

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

boost::shared_ptr<SynchedCartesianTrajectory> MinJerkSynchedCartesianTrajectoryFactory::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 << "MinJerkCartesianTrajectoryFactory::getTrajectory() - number of start poses and goal poses don't match";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.MinJerkCartesianTrajectoryFactory", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

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

    for (unsigned int i = 0; i < startPoses.size(); ++i)
    {
        trajPtrs[i].reset(new MinJerkCartesianTrajectory(*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;
}