MinJerkTrajectory_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/MinJerkTrajectoryFactory.h"
#include <tf/tf.h>

class TrajectoryGeneratorTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        ctg = new MinJerkCartesianTrajectoryFactory();
        jtg = new MinJerkJointTrajectoryFactory();
    }

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

    MinJerkCartesianTrajectoryFactory*     ctg; // pointer for easy destruction between tests
    MinJerkJointTrajectoryFactory*         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(TrajectoryGeneratorTest, minJerkCartGetSetTest)
{
    // 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);

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

TEST_F(TrajectoryGeneratorTest, minJerkCartTrajTest)
{
    const double linVel = 0.05;
    const double rotVel = 0.3;

    ctg->setVelocityLimits(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);
    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;

    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         x, y, z, w;
    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);

    for (double time = timeStep; time < ctptr->getDuration(); time += timeStep)
    {
        // check lin velocity
        testPose2 = testPose;
        ctptr->getPose(time, testPose);
        EXPECT_LE((testPose.p.p - testPose2.p.p).Norm() / timeStep, linVel);
        EXPECT_LE(testPose.p.v.Norm(), linVel);

        // 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);
        EXPECT_LE(testPose.M.w.Norm(), rotVel);

        // 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;
    }

    // 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(TrajectoryGeneratorTest, minJerkCartZeroVelTrajTest)
{
    const double linVel = 0.05;
    const double rotVel = 0.3;

    ctg->setVelocityLimits(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(TrajectoryGeneratorTest, minJerkJointGetSetTest)
{
    std::vector<double> jntLimits(3);
    jntLimits.at(0) = .1;
    jntLimits.at(1) = .2;
    jntLimits.at(2) = .3;

    // check set/get methods
    jtg->setLimits(jntLimits, std::vector<double>(3));
    EXPECT_EQ(jntLimits, jtg->getVelocityLimits());

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

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

TEST_F(TrajectoryGeneratorTest, minJerkJointTrajTest)
{
    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(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;

    jtg->setLimits(jntLimits, std::vector<double>(5));

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

    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);

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

//        std::cout << testPose.q(0) << ", " << testPose.qdot(0) << ", " << testPose.qdotdot(0) << "; " << (testPose.q(0) - testPose2.q(0))/timeStep << ", " << (testPose.qdot(0) - testPose2.qdot(0))/timeStep << std::endl;
        for (unsigned int j = 0; j < jntStart.q.rows(); ++j)
        {
            // check velocity
            EXPECT_LE((testPose.q(j) - testPose2.q(j)) / timeStep, jntLimits.at(j));

            // check distance along path
            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;
        }
    }
}

TEST_F(TrajectoryGeneratorTest, minJerkJointZeroVelTrajTest)
{
    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, std::vector<double>(5));

    // 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, std::vector<double>(5));
    EXPECT_THROW(jtg->getTrajectory(jntStart, jntEnd), std::runtime_error);
}

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