TrapezoidalVelocityTrajectory_Test.cpp

Go to the documentation of this file.

/*
 * TrajectoryGenerator_Test.cpp
 *
 *  Created on: Aug 16, 2012
 *      Author: Ross Taylor
 */

#include <iostream>
#include <gtest/gtest.h>
#include "robodyn_controllers/TrapezoidalVelocityTrajectoryFactory.h"
#include <tf/tf.h>
#include <fstream>

bool outputFlag = false;

class TrapVelTrajectoryGeneratorTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        ctg = new TrapezoidalVelocityCartesianTrajectoryFactory();
        jtg = new TrapezoidalVelocityJointTrajectoryFactory();
    }

    virtual void TearDown()
    {
        // delete stuff, etc.
        delete ctg;
        delete jtg;
    }

    TrapezoidalVelocityCartesianTrajectoryFactory* ctg; // pointer for easy destruction between tests
    TrapezoidalVelocityJointTrajectoryFactory*     jtg; // pointer for easy destruction between tests
    boost::shared_ptr<CartesianTrajectory>         ctptr;
    boost::shared_ptr<JointTrajectory>             jtptr;
};

// TEST_F uses SetUp/TearDown in Test above
// second argument describes the test (make descriptive)
TEST_F(TrapVelTrajectoryGeneratorTest, TrapezoidalVelocityCartGetSetTest)
{
    // EXPECT_EQ (test equality)
    // EXPECT_FLOAT_EQ (test equality, with roundoff allowed)
    // EXPECT_TRUE
    // EXPECT_FALSE
    // EXPECT_THROW
    // google google test for more

    const double linVel = 0.05;
    const double rotVel = 0.3;

    // check set/get methods
    EXPECT_THROW(ctg->setVelocityLimits(-linVel, rotVel), std::invalid_argument);
    EXPECT_THROW(ctg->setVelocityLimits(linVel, -rotVel), std::invalid_argument);
    EXPECT_THROW(ctg->setAccelerationLimits(-linVel, rotVel), std::invalid_argument);
    EXPECT_THROW(ctg->setAccelerationLimits(linVel, -rotVel), std::invalid_argument);

    ctg->setVelocityLimits(linVel, rotVel);
    ctg->setAccelerationLimits(linVel, rotVel);
    EXPECT_EQ(linVel, ctg->getLinearVelocityLimit());
    EXPECT_EQ(rotVel, ctg->getRotationalVelocityLimit());
    EXPECT_EQ(linVel, ctg->getLinearAccelerationLimit());
    EXPECT_EQ(rotVel, ctg->getRotationalAccelerationLimit());
}

TEST_F(TrapVelTrajectoryGeneratorTest, TrapezoidalVelocityCartTrajTest)
{
    const double linVel = 0.045;
    const double rotVel = 0.31;

    ctg->setVelocityLimits(linVel, rotVel);
    ctg->setAccelerationLimits(linVel, rotVel);

    // check trajectory
    KDL::FrameAcc currPose, desPose, startTest, endTest, testPose, testPose2;
    currPose.p.p.x(0.);
    currPose.p.p.y(0.);
    currPose.p.p.z(0.);
    currPose.M.R = KDL::Rotation::Quaternion(0, 0, 0, 1);
    currPose.M.w.x(0.);
    desPose.p.p.x(1.);
    desPose.p.p.y(2.);
    desPose.p.p.z(3.);
    desPose.M.R = currPose.M.R;
    desPose.M.R.DoRotX(.3);
    desPose.M.R.DoRotY(0.);
    desPose.M.R.DoRotZ(0.);
    double x, y, z, w;
    desPose.M.R.GetQuaternion(x, y, z, w);
    std::cout << "x,y,z,w: " << x << ", " << y << ", " << z << ", " << w << std::endl;

    ctptr = ctg->getTrajectory(currPose, desPose);
    ctptr->getPose(ctptr->getDuration(), endTest);
    EXPECT_EQ(desPose.GetFrame(), endTest.GetFrame());
    ctptr->getPose(0., startTest);
    EXPECT_EQ(currPose.GetFrame(), startTest.GetFrame());

    double prevLinDist = 0.;
    double totLinDist  = (endTest.p.p - startTest.p.p).Norm();
    startTest.M.R.GetQuaternion(x, y, z, w);
    tf::Quaternion startQuat(x, y, z, w);
    endTest.M.R.GetQuaternion(x, y, z, w);
    tf::Quaternion endQuat(x, y, z, w);
    double totRotDist  = std::fabs(endQuat.angleShortestPath(startQuat));
    double prevRotDist = 0.;

    double timeStep = 0.01;
    ctptr->getPose(0., testPose);
    std::ofstream file;

    if (outputFlag)
    {
        file.open("testCart.csv");
        file << 0.;
        file << ", " << testPose.p.p.x();
        file << ", " << testPose.p.p.y();
        file << ", " << testPose.p.p.z();
        file << ", " << startQuat.x() << ", " << startQuat.y() << ", " << startQuat.z() << ", " << startQuat.w();
        file << std::endl;
    }

    for (double time = timeStep; time < ctptr->getDuration(); time += timeStep)
    {
        // check lin velocity
        testPose2 = testPose;
        ctptr->getPose(time, testPose);

        if (outputFlag)
        {
            file << time;
            file << ", " << testPose.p.p.x();
            file << ", " << testPose.p.p.y();
            file << ", " << testPose.p.p.z();
        }

        EXPECT_LE((testPose.p.p - testPose2.p.p).Norm() / timeStep, sqrt(linVel * linVel * 3));

        // check rot velocity
        testPose2.M.R.GetQuaternion(x, y, z, w);
        tf::Quaternion quat1(x, y, z, w);
        testPose.M.R.GetQuaternion(x, y, z, w);
        tf::Quaternion quat2(x, y, z, w);
        EXPECT_LE(std::fabs(quat2.angleShortestPath(quat1) / timeStep), rotVel);

        if (outputFlag)
        {
            file << ", " << quat2.x() << ", " << quat2.y() << ", " << quat2.z() << ", " << quat2.w();
            file << std::endl;
        }

        // check distance along path
        double linDist = (testPose.p.p - startTest.p.p).Norm();
        EXPECT_GE(linDist, prevLinDist);

        if (totLinDist > 0)
        {
            EXPECT_GE(linDist / totLinDist, 0);
            EXPECT_LE(linDist / totLinDist, 1);
        }

        double linDistToEnd = (endTest.p.p - testPose.p.p).Norm();
        EXPECT_LE(totLinDist - linDistToEnd - linDist, 0.0001);
        prevLinDist = linDist;

        // check angle along path
        double rotDist = std::fabs(quat2.angleShortestPath(startQuat));
        EXPECT_GE(rotDist, prevRotDist);

        if (totRotDist > 0)
        {
            EXPECT_GE(rotDist / totRotDist, 0);
            EXPECT_LE(rotDist / totRotDist, 1);
        }

        double rotDistToEnd = std::fabs(quat2.angleShortestPath(endQuat));
        EXPECT_LE(totRotDist - rotDistToEnd - rotDist, 0.0001);
        prevRotDist = rotDist;
    }

    ctptr->getPose(ctptr->getDuration(), testPose);

    if (outputFlag)
    {
        file << ctptr->getDuration();
        file << ", " << testPose.p.p.x();
        file << ", " << testPose.p.p.y();
        file << ", " << testPose.p.p.z();
        file << ", " << endQuat.x() << ", " << endQuat.y() << ", " << endQuat.z() << ", " << endQuat.w();
        file << std::endl;
        file.close();
    }

    // give a longer time and make sure it takes longer
    double timeToFinish = ctptr->getDuration() * 2.;
    ctptr = ctg->getTrajectory(currPose, desPose, timeToFinish);
    EXPECT_EQ(timeToFinish, ctptr->getDuration());
}

TEST_F(TrapVelTrajectoryGeneratorTest, TrapezoidalVelocityCartZeroVelTrajTest)
{
    const double linVel = 0.05;
    const double rotVel = 0.3;

    ctg->setVelocityLimits(0., rotVel);
    ctg->setAccelerationLimits(0., rotVel);

    // check trajectory
    KDL::FrameAcc currPose, desPose;
    currPose.p.p.x(0.);
    currPose.p.p.y(0.);
    currPose.p.p.z(0.);
    currPose.M.R = KDL::Rotation::Quaternion(0, 0, 0, 1);
    desPose.p.p.x(1.);
    desPose.p.p.y(2.);
    desPose.p.p.z(3.);
    desPose.M.R = currPose.M.R;
    desPose.M.R.DoRotX(.3);
    desPose.M.R.DoRotY(.2);
    desPose.M.R.DoRotZ(.1);
    std::vector<KDL::FrameAcc> traj;

    EXPECT_THROW(ctg->getTrajectory(currPose, desPose), std::runtime_error);

    ctg->setVelocityLimits(linVel, 0.);

    EXPECT_THROW(ctg->getTrajectory(currPose, desPose), std::runtime_error);
}

TEST_F(TrapVelTrajectoryGeneratorTest, TrapezoidalVelocityJointGetSetTest)
{
    std::vector<double> jntLimits(3);
    jntLimits.at(0) = .1;
    jntLimits.at(1) = .2;
    jntLimits.at(2) = .3;

    // check set/get methods
    jtg->setLimits(jntLimits, jntLimits);
    EXPECT_EQ(jntLimits, jtg->getVelocityLimits());
    EXPECT_EQ(jntLimits, jtg->getAccelerationLimits());

    for (unsigned int i = 0; i < jntLimits.size(); ++i)
    {
        EXPECT_EQ(jntLimits.at(i), jtg->getVelocityLimit(i));
        EXPECT_EQ(jntLimits.at(i), jtg->getAccelerationLimit(i));
    }

    EXPECT_ANY_THROW(jtg->getVelocityLimit(jntLimits.size())); // one past the end
    EXPECT_ANY_THROW(jtg->getAccelerationLimit(jntLimits.size())); // one past the end
}

TEST_F(TrapVelTrajectoryGeneratorTest, TrapezoidalVelocityJointTrajTest)
{
    double eps = std::numeric_limits<float>::epsilon();

    std::vector<double> jntLimits(5);
    jntLimits.at(0) = .1;
    jntLimits.at(1) = .2;
    jntLimits.at(2) = .3;
    jntLimits.at(3) = .15;
    jntLimits.at(4) = .25;

    KDL::JntArrayAcc jntStart(5);
    jntStart.q(0)    = 0;
    jntStart.q(1)    = 0;
    jntStart.q(2)    = 0;
    jntStart.q(3)    = 0;
    jntStart.q(4)    = 0;
    jntStart.qdot(0) = 0;
    jntStart.qdot(1) = -.01;
    jntStart.qdot(2) = .03;
    jntStart.qdot(3) = -.05;
    jntStart.qdot(4) = 0;

    KDL::JntArrayAcc jntEnd(5);
    jntEnd.q(0)    = 15.*3.14 / 180;
    jntEnd.q(1)    = -20.*3.14 / 180;
    jntEnd.q(2)    = 30.*3.14 / 180;
    jntEnd.q(3)    = -25.*3.14 / 180;
    jntEnd.q(4)    = 10.*3.14 / 180;
    jntEnd.qdot(0) = 0;
    jntEnd.qdot(1) = .01;
    jntEnd.qdot(2) = .03;
    jntEnd.qdot(3) = 0;
    jntEnd.qdot(4) = .02;

    jtg->setLimits(jntLimits, jntLimits);

    // check trajectory
    KDL::JntArrayAcc startTest, endTest, testPose, testPose2;
    jtptr = jtg->getTrajectory(jntStart, jntEnd);
    jtptr->getPose(0., startTest);
    jtptr->getPose(jtptr->getDuration(), endTest);

    for (unsigned int i = 0; i < endTest.q.rows(); ++i)
    {
        EXPECT_FLOAT_EQ(jntEnd.q(i), endTest.q(i));
        EXPECT_FLOAT_EQ(jntStart.q(i), startTest.q(i));
    }

    KDL::JntArray prevDist(5);
    KDL::JntArray totDist(jntStart.q.rows());

    for (unsigned int i = 0; i < totDist.rows(); ++i)
    {
        totDist(i) = std::fabs(endTest.q(i) - startTest.q(i));
    }

    double timeStep = 0.01;
    jtptr->getPose(0., testPose);

    std::ofstream file;

    if (outputFlag)
    {
        file.open("test.csv");
        file << 0.;

        for (unsigned int i = 0; i < testPose.q.rows(); ++i)
        {
            file << ", " << testPose.q(i) << ", " << testPose.qdot(i) << ", " << testPose.qdotdot(i);
        }

        file << std::endl;
    }

    for (double time = timeStep; time < jtptr->getDuration(); time += timeStep)
    {
        testPose2 = testPose;
        jtptr->getPose(time, testPose);

        if (outputFlag)
        {
            file << time;

            for (unsigned int i = 0; i < testPose.q.rows(); ++i)
            {
                file << ", " << testPose.q(i) << ", " << testPose.qdot(i) << ", " << testPose.qdotdot(i);
            }

            file << std::endl;
        }

        for (unsigned int j = 0; j < jntStart.q.rows(); ++j)
        {
            // check velocity
            EXPECT_LE(fabs(testPose.q(j) - testPose2.q(j)) / timeStep, jntLimits.at(j) + eps);

            // check distance along path
            if (jntStart.qdot(j) == 0. && jntEnd.qdot(j) == 0.)
            {
                double dist = std::fabs(testPose.q(j) - jntStart.q(j));
                EXPECT_GE(dist, prevDist(j));

                if (totDist(j) > 0)
                {
                    EXPECT_GE(dist / totDist(j), 0);
                    EXPECT_LE(dist / totDist(j), 1);
                }

                double distToEnd = std::fabs(endTest.q(j) - testPose.q(j));
                EXPECT_FLOAT_EQ(dist, totDist(j) - distToEnd);
                prevDist(j) = dist;
            }
        }
    }

    jtptr->getPose(jtptr->getDuration(), testPose);

    if (outputFlag)
    {
        file << jtptr->getDuration();

        for (unsigned int i = 0; i < testPose.q.rows(); ++i)
        {
            file << ", " << testPose.q(i) << ", " << testPose.qdot(i) << ", " << testPose.qdotdot(i);
        }

        file << std::endl;
        file.close();
    }
}

TEST_F(TrapVelTrajectoryGeneratorTest, TrapezoidalVelocityJointZeroVelTrajTest)
{
    std::vector<double> jntLimits(5);
    jntLimits.at(0) = .1;
    jntLimits.at(1) = .2;
    jntLimits.at(2) = .3;
    jntLimits.at(3) = .15;
    jntLimits.at(4) = .25;

    KDL::JntArrayAcc jntStart(5);
    jntStart.q(0) = 0;
    jntStart.q(1) = 0;
    jntStart.q(2) = 0;
    jntStart.q(3) = 0;
    jntStart.q(4) = 0;

    KDL::JntArrayAcc jntEnd(4);
    jntEnd.q(0) = 15.*3.14 / 180;
    jntEnd.q(1) = 20.*3.14 / 180;
    jntEnd.q(2) = 30.*3.14 / 180;
    jntEnd.q(3) = 25.*3.14 / 180;

    jtg->setLimits(jntLimits, jntLimits);

    // mismatched lengths
    EXPECT_THROW(jtg->getTrajectory(jntStart, jntEnd), std::runtime_error);

    // zero vel
    jntEnd.resize(5);
    jntEnd.q(0)     = 15.*3.14 / 180;
    jntEnd.q(1)     = 20.*3.14 / 180;
    jntEnd.q(2)     = 30.*3.14 / 180;
    jntEnd.q(3)     = 25.*3.14 / 180;
    jntEnd.q(4)     = 10.*3.14 / 180;
    jntLimits.at(3) = 0.;
    jtg->setLimits(jntLimits, jntLimits);
    EXPECT_THROW(jtg->getTrajectory(jntStart, jntEnd), std::runtime_error);
}


TEST_F(TrapVelTrajectoryGeneratorTest, TrapezoidalVelocityUtilityDurationTest)
{
    TrapezoidalVelocityUtility tvu;
    double t1, t2, t3;

    tvu.setConstraints(0, 1.5, 0, 0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.5, t2);
    EXPECT_FLOAT_EQ(2.5, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.5, tvu.p(1.0));
    EXPECT_FLOAT_EQ(1.0, tvu.p(1.5));
    EXPECT_FLOAT_EQ(1.5, tvu.p(2.5));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(1.5));
    EXPECT_FLOAT_EQ(0.0, tvu.v(2.5));

    EXPECT_FLOAT_EQ(1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(1.5));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(2.5));

    tvu.setDuration(2.5);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.5, t2);
    EXPECT_FLOAT_EQ(2.5, t3);

    EXPECT_FLOAT_EQ(1.5, tvu.p(2.5));
    EXPECT_FLOAT_EQ(0, tvu.v(2.5));

    tvu.setConstraints(0, -1.5, 0, 0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.5, t2);
    EXPECT_FLOAT_EQ(2.5, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-0.5, tvu.p(1.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.p(1.5));
    EXPECT_FLOAT_EQ(-1.5, tvu.p(2.5));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(1.5));
    EXPECT_FLOAT_EQ(0.0, tvu.v(2.5));

    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(1.5));
    EXPECT_FLOAT_EQ(1.0, tvu.a(2.5));

    tvu.setConstraints(0, 1.5, 1.0, 0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0, t1);
    EXPECT_FLOAT_EQ(1.0, t2);
    EXPECT_FLOAT_EQ(2.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.p(1.0));
    EXPECT_FLOAT_EQ(1.5, tvu.p(2.0));

    EXPECT_FLOAT_EQ(1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(2.0));

    EXPECT_FLOAT_EQ(0.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(2.0));

    tvu.setConstraints(0, -1.5, -1.0, 0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0, t1);
    EXPECT_FLOAT_EQ(1.0, t2);
    EXPECT_FLOAT_EQ(2.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.p(1.0));
    EXPECT_FLOAT_EQ(-1.5, tvu.p(2.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(2.0));

    EXPECT_FLOAT_EQ(0.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(2.0));

    tvu.setConstraints(0, 1.5, -1.0, 0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(2.0, t1);
    EXPECT_FLOAT_EQ(3.0, t2);
    EXPECT_FLOAT_EQ(4.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.0, tvu.p(2.0));
    EXPECT_FLOAT_EQ(1.0, tvu.p(3.0));
    EXPECT_FLOAT_EQ(1.5, tvu.p(4.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(2.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(3.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(4.0));

    EXPECT_FLOAT_EQ(1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(2.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(3.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(4.0));

    tvu.setConstraints(0, -1.5, 1.0, 0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(2.0, t1);
    EXPECT_FLOAT_EQ(3.0, t2);
    EXPECT_FLOAT_EQ(4.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.0, tvu.p(2.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.p(3.0));
    EXPECT_FLOAT_EQ(-1.5, tvu.p(4.0));

    EXPECT_FLOAT_EQ(1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(2.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(3.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(4.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(2.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(3.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(4.0));

    tvu.setConstraints(0, 1.5, 0.0, 1.0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(2.0, t2);
    EXPECT_FLOAT_EQ(2.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.5, tvu.p(1.0));
    EXPECT_FLOAT_EQ(1.5, tvu.p(2.0));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(2.0));

    EXPECT_FLOAT_EQ(1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(2.0));

    tvu.setConstraints(0, -1.5, 0.0, -1.0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(2.0, t2);
    EXPECT_FLOAT_EQ(2.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-0.5, tvu.p(1.0));
    EXPECT_FLOAT_EQ(-1.5, tvu.p(2.0));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(2.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(2.0));

    tvu.setConstraints(0, 1.5, 0.0, -1.0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(2.0, t2);
    EXPECT_FLOAT_EQ(4.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.5, tvu.p(1.0));
    EXPECT_FLOAT_EQ(1.5, tvu.p(2.0));
    EXPECT_FLOAT_EQ(1.5, tvu.p(4.0));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(2.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(4.0));

    EXPECT_FLOAT_EQ(1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(2.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(4.0));

    tvu.setConstraints(0, -1.5, 0.0, 1.0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(2.0, t2);
    EXPECT_FLOAT_EQ(4.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-0.5, tvu.p(1.0));
    EXPECT_FLOAT_EQ(-1.5, tvu.p(2.0));
    EXPECT_FLOAT_EQ(-1.5, tvu.p(4.0));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(2.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(4.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(0.0, tvu.a(2.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(4.0));

    tvu.setConstraints(0, 1.5, 2.0, 1.0, 0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.0, t1);
    EXPECT_FLOAT_EQ(0.0, t2);
    EXPECT_FLOAT_EQ(1.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(1.5, tvu.p(1.0));

    EXPECT_FLOAT_EQ(2.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(1.0));

    EXPECT_FLOAT_EQ(0.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(1.0));

    tvu.setConstraints(0, 1.5, 1.0, 2.0, 0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.0, t2);
    EXPECT_FLOAT_EQ(1.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(1.5, tvu.p(1.0));

    EXPECT_FLOAT_EQ(1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(2.0, tvu.v(1.0));

    EXPECT_FLOAT_EQ(1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(1.0));

    tvu.setConstraints(0, -1.5, -1.0, -2.0, 0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.0, t2);
    EXPECT_FLOAT_EQ(1.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-1.5, tvu.p(1.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-2.0, tvu.v(1.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(1.0));

    tvu.setConstraints(0, -1.5, -2.0, -1.0, 0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.0, t1);
    EXPECT_FLOAT_EQ(0.0, t2);
    EXPECT_FLOAT_EQ(1.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-1.5, tvu.p(1.0));

    EXPECT_FLOAT_EQ(-2.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(1.0));

    EXPECT_FLOAT_EQ(0.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(1.0));


    tvu.setConstraints(0, 0.5, 0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.70710678, t1);
    EXPECT_FLOAT_EQ(0.70710678, t2);
    EXPECT_FLOAT_EQ(1.4142135623, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.25, tvu.p(0.70710678));
    EXPECT_FLOAT_EQ(0.5, tvu.p(1.4142135623));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(0.70710678, tvu.v(0.70710678));
    EXPECT_NEAR(0.0, tvu.v(1.4142135623), 1e-10);

    EXPECT_FLOAT_EQ(1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(0.70710678));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(1.4142135623));

    tvu.setConstraints(0, - 0.5, 0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.70710678, t1);
    EXPECT_FLOAT_EQ(0.70710678, t2);
    EXPECT_FLOAT_EQ(1.4142135623, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-0.25, tvu.p(0.70710678));
    EXPECT_FLOAT_EQ(-0.5, tvu.p(1.4142135623));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-0.70710678, tvu.v(0.70710678));
    EXPECT_NEAR(0.0, tvu.v(1.4142135623), 1e-10);

    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.70710678));
    EXPECT_FLOAT_EQ(1.0, tvu.a(1.4142135623));

    tvu.setConstraints(0, 0.5, 1.0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.0, t1);
    EXPECT_FLOAT_EQ(0.0, t2);
    EXPECT_FLOAT_EQ(1.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.5, tvu.p(1.0));

    EXPECT_FLOAT_EQ(1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(1.0));

    EXPECT_FLOAT_EQ(0.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(1.0));

    tvu.setConstraints(0, 0.5, -1.0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(2.0, t1);
    EXPECT_FLOAT_EQ(2.0, t2);
    EXPECT_FLOAT_EQ(3.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.0, tvu.p(2.0));
    EXPECT_FLOAT_EQ(0.5, tvu.p(3.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(2.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(3.0));

    EXPECT_FLOAT_EQ(1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(2.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(3.0));

    tvu.setConstraints(0, -0.5, -1.0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.0, t1);
    EXPECT_FLOAT_EQ(0.0, t2);
    EXPECT_FLOAT_EQ(1.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-0.5, tvu.p(1.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(1.0));

    EXPECT_FLOAT_EQ(0.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(1.0));

    tvu.setConstraints(0, -0.5, 1.0, 0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(2.0, t1);
    EXPECT_FLOAT_EQ(2.0, t2);
    EXPECT_FLOAT_EQ(3.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.0, tvu.p(2.0));
    EXPECT_FLOAT_EQ(-0.5, tvu.p(3.0));

    EXPECT_FLOAT_EQ(1.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(2.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(3.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(2.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(3.0));

    tvu.setConstraints(0, 0.5, 0.0, 1.0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.0, t2);
    EXPECT_FLOAT_EQ(1.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.5, tvu.p(1.0));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(1.0));

    EXPECT_FLOAT_EQ(1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(1.0));

    tvu.setConstraints(0, 0.5, 0, -1.0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.0, t2);
    EXPECT_FLOAT_EQ(3.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(0.5, tvu.p(1.0));
    EXPECT_FLOAT_EQ(0.5, tvu.p(3.0));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(3.0));

    EXPECT_FLOAT_EQ(1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(3.0));

    tvu.setConstraints(0, -0.5, 0.0, -1.0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.0, t2);
    EXPECT_FLOAT_EQ(1.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-0.5, tvu.p(1.0));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(1.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(1.0));

    tvu.setConstraints(0, -0.5, 0.0, 1.0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.0, t2);
    EXPECT_FLOAT_EQ(3.0, t3);

    EXPECT_FLOAT_EQ(0.0, tvu.p(0.0));
    EXPECT_FLOAT_EQ(-0.5, tvu.p(1.0));
    EXPECT_FLOAT_EQ(-0.5, tvu.p(3.0));

    EXPECT_FLOAT_EQ(0.0, tvu.v(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.v(1.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(3.0));

    EXPECT_FLOAT_EQ(-1.0, tvu.a(0.0));
    EXPECT_FLOAT_EQ(-1.0, tvu.a(1.0));
    EXPECT_FLOAT_EQ(1.0, tvu.a(3.0));

    tvu.setConstraints(0, 0.5, 0.5, 1.0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.56066017, t1);
    EXPECT_FLOAT_EQ(0.56066017, t2);
    EXPECT_FLOAT_EQ(0.62132034, t3);

    tvu.setConstraints(0, 0.5, 1.0, 0.5);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.06066017, t1);
    EXPECT_FLOAT_EQ(0.06066017, t2);
    EXPECT_FLOAT_EQ(0.62132034, t3);

    tvu.setConstraints(0, -0.5, -0.5, -1.0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.56066017, t1);
    EXPECT_FLOAT_EQ(0.56066017, t2);
    EXPECT_FLOAT_EQ(0.62132034, t3);

    tvu.setConstraints(0, -0.5, -1.0, -0.5);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.06066017, t1);
    EXPECT_FLOAT_EQ(0.06066017, t2);
    EXPECT_FLOAT_EQ(0.62132034, t3);

    tvu.setConstraints(0, -0.5, -1.0, -2.0);
    EXPECT_NO_THROW(tvu.duration(2.0, 1.0));
    EXPECT_THROW(tvu.duration(2.0, 1.0, false), std::runtime_error);
    EXPECT_NO_THROW(tvu.duration(2.0, 3.0, false));

    tvu.setConstraints(0, 0.5, 1.0, 2.0);
    EXPECT_NO_THROW(tvu.duration(2.0, 1.0));
    EXPECT_THROW(tvu.duration(2.0, 1.0, false), std::runtime_error);
    EXPECT_NO_THROW(tvu.duration(2.0, 3.0, false));

    tvu.setConstraints(0, 0.5, 1.0, 1.0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.22474487, t1);
    EXPECT_FLOAT_EQ(0.22474487, t2);
    EXPECT_FLOAT_EQ(0.44948974, t3);

    tvu.setConstraints(0.175526947621374, 0.12247641320582009, -0.6514631803290432, -0.5641836637951542);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_NEAR(0, t1, 1e-7);
    EXPECT_NEAR(0, t2, 1e-7);
    EXPECT_FLOAT_EQ(0.0872795, t3);

    tvu.setConstraints(0.512156, 0.512156, -5.48252e-07, 0);
    tvu.duration(1, 0.00872665);
    tvu.setDuration(3.74166);
    tvu.getTimes(t1, t2, t3);

    EXPECT_NEAR(6.2825e-5, t1, 1e-7);
    EXPECT_NEAR(3.74166, t2, 1e-7);
    EXPECT_NEAR(3.74166, t3, 1e-7);

    tvu.setConstraints(1.35736215114594, 1.15844341219664, -0.772499978542328, 0);
    tvu.duration(0.4, 1.5);
    tvu.getTimes(t1, t2, t3);

    EXPECT_NEAR(0, t1, 1e-7);
    EXPECT_NEAR(0, t2, 1e-7);
    EXPECT_NEAR(0.515, t3, 1e-7);

}

TEST_F(TrapVelTrajectoryGeneratorTest, TrapezoidalVelocityUtilitySetDurationTest)
{
    TrapezoidalVelocityUtility tvu;
    double t1, t2, t3;

    tvu.setConstraints(0, 1.5, 0, 0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.5, t2);
    EXPECT_FLOAT_EQ(2.5, t3);

    EXPECT_FLOAT_EQ(1.5, tvu.p(2.5));
    EXPECT_NEAR(0, tvu.v(2.5), 1e-10);

    tvu.setDuration(2.5);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.5, t2);
    EXPECT_FLOAT_EQ(2.5, t3);

    EXPECT_FLOAT_EQ(1.5, tvu.p(2.5));
    EXPECT_FLOAT_EQ(0, tvu.v(2.5));

    tvu.setDuration(5);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.32055053, t1);
    EXPECT_FLOAT_EQ(4.6794496, t2);
    EXPECT_FLOAT_EQ(5.0, t3);

    EXPECT_FLOAT_EQ(1.5, tvu.p(5));
    EXPECT_NEAR(0, tvu.v(5), 1e-10);

    EXPECT_THROW(tvu.setDuration(1), std::runtime_error);

    tvu.setConstraints(0, -1.5, 0, 0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(1.5, t2);
    EXPECT_FLOAT_EQ(2.5, t3);

    EXPECT_FLOAT_EQ(-1.5, tvu.p(2.5));
    EXPECT_FLOAT_EQ(0.0, tvu.v(2.5));

    tvu.setDuration(5);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.32055053, t1);
    EXPECT_FLOAT_EQ(4.6794496, t2);
    EXPECT_FLOAT_EQ(5.0, t3);

    EXPECT_FLOAT_EQ(-1.5, tvu.p(5));
    EXPECT_NEAR(0.0, tvu.v(5), 1e-10);

    tvu.setConstraints(0, 1.5, 1.0, 0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0, t1);
    EXPECT_FLOAT_EQ(1.0, t2);
    EXPECT_FLOAT_EQ(2.0, t3);

    tvu.setDuration(3);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.5, t1);
    EXPECT_FLOAT_EQ(2.5, t2);
    EXPECT_FLOAT_EQ(3.0, t3);

    EXPECT_FLOAT_EQ(0.375, tvu.p(0.5));
    EXPECT_FLOAT_EQ(0.5, tvu.v(0.5));

    EXPECT_FLOAT_EQ(1.375, tvu.p(2.5));
    EXPECT_FLOAT_EQ(0.5, tvu.v(1.5));

    EXPECT_FLOAT_EQ(1.5, tvu.p(3.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(3.0));

    tvu.setDuration(5);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.75, t1);
    EXPECT_FLOAT_EQ(4.75, t2);
    EXPECT_FLOAT_EQ(5.0, t3);

    EXPECT_FLOAT_EQ(0.46875, tvu.p(0.75));
    EXPECT_FLOAT_EQ(0.25, tvu.v(0.75));

    EXPECT_FLOAT_EQ(1.46875, tvu.p(4.75));
    EXPECT_FLOAT_EQ(0.25, tvu.v(0.75));

    EXPECT_FLOAT_EQ(1.5, tvu.p(5.0));
    EXPECT_FLOAT_EQ(0.0, tvu.v(5.0));

    tvu.setConstraints(0, 1.5, 0.0, 1.0, 0, 0);
    tvu.duration(1.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(1.0, t1);
    EXPECT_FLOAT_EQ(2.0, t2);
    EXPECT_FLOAT_EQ(2.0, t3);

    EXPECT_FLOAT_EQ(1.5, tvu.p(2.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(2.0));

    tvu.setDuration(5);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.25, t1);
    EXPECT_FLOAT_EQ(4.25, t2);
    EXPECT_FLOAT_EQ(5.0, t3);

    EXPECT_FLOAT_EQ(0.03125, tvu.p(0.25));
    EXPECT_FLOAT_EQ(0.25, tvu.v(0.25));

    EXPECT_FLOAT_EQ(1.03125, tvu.p(4.25));
    EXPECT_FLOAT_EQ(0.25, tvu.v(4.25));

    EXPECT_FLOAT_EQ(1.5, tvu.p(5.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(5.0));


    tvu.setConstraints(0, 0.5, 0.5, 1.0);
    tvu.duration(2.0, 1.0);
    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.56066017, t1);
    EXPECT_FLOAT_EQ(0.56066017, t2);
    EXPECT_FLOAT_EQ(0.62132034, t3);

    EXPECT_NO_THROW(tvu.setDuration(5));

    tvu.setConstraints(0, 0.5, 0.5, 1.0);
    tvu.duration(2.0, 1.25);

    EXPECT_NO_THROW(tvu.setDuration(5));

    tvu.getTimes(t1, t2, t3);

    EXPECT_FLOAT_EQ(0.4, t1);
    EXPECT_FLOAT_EQ(4.2, t2);
    EXPECT_FLOAT_EQ(5.0, t3);

    EXPECT_FLOAT_EQ(0.5, tvu.p(5.0));
    EXPECT_FLOAT_EQ(1.0, tvu.v(5.0));

    tvu.setConstraints(0, 0.5, 0.5, 0.866);
    tvu.duration(2.0, 1.0);

    EXPECT_NO_THROW(tvu.setDuration(5));

    tvu.getTimes(t1, t2, t3);

    EXPECT_NEAR(0.5, t1, 1e-5);
    EXPECT_NEAR(4.134, t2, 1e-5);
    EXPECT_FLOAT_EQ(5.0, t3);


    tvu.setConstraints(1.5, 1.5, 1.5, 1.5);
    tvu.duration(0.25, 0.5);
    EXPECT_THROW(tvu.setDuration(5), std::runtime_error);

    tvu.setConstraints(1.5, 1.5, 0.0, 0.0);
    tvu.duration(0.25, 0.5);
    EXPECT_NO_THROW(tvu.setDuration(5));

    tvu.getTimes(t1, t2, t3);
    EXPECT_FLOAT_EQ(0.0, t1);
    EXPECT_FLOAT_EQ(5.0, t2);
    EXPECT_FLOAT_EQ(5.0, t3);

}

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);

    for (int i = 0; i < argc; ++i)
    {
        std::cout << argv[i] << std::endl;

        if (strcmp(argv[i], "--outputTraj") == 0)
        {
            outputFlag = true;
        }
    }

    return RUN_ALL_TESTS();
}