test_ddp_planar.cpp

Go to the documentation of this file.

#include <vector>
#include <Eigen/Core>
#include <gtest/gtest.h>


#include <mpc/types.h>
#include <mpc/mpc.h>

#include <mpc/examples/euclidean.h>

using namespace MPC;

void dummyDyn(const VectorT &x, const VectorT &u, VectorT &xN) { 
        return;
}

void dummyDynD(const VectorT &x, const VectorT &u, Deriv &d) { 
        return;
}

float dummyCost(const VectorT &x, const VectorT &u) {
        return 0.;
}

void dummyCostD(const VectorT &x, const VectorT &u, Deriv &d) {
        return;
}

TEST(MPC, DDPInitTest) {
        bool init_success = DDP_init(&dummyDyn, &dummyDynD, &dummyCost, &dummyCostD);
        ASSERT_TRUE(init_success);
}

TEST(MPC, DDPeuclidean2DTest) {
        VectorT goal(VectorT::Zero(2));
        VectorT x0(VectorT::Zero(2));
        MatrixT u0(MatrixT::Random(2, 100));
        // setup start and goal
        x0(0) = 2.;
        goal(0) = 2.;
        goal(1) = 4.;
        euclidean::setGoal(goal);
        // init ddp
        bool init_success = DDP_init(&euclidean::dynamics, &euclidean::dynamicsD, &euclidean::cost, &euclidean::costD);
        ASSERT_TRUE(init_success);
        // and run the algorithm
        DDPParams p;
        Trajectory res;
        MPC::DDP(x0, u0, p, 100, res);
        //std::cout << res.x.transpose() << std::endl;
        ASSERT_NEAR((res.x.col(res.x.cols()-1) - goal).norm(), 0., 5e-4); 
}

TEST(MPC, DDPeuclideanWithObstaclesTest) {
        VectorT goal(VectorT::Zero(2));
        VectorT otmp(VectorT::Zero(2));
        VectorT x0(VectorT::Zero(2));
        MatrixT u0(MatrixT::Random(2, 100));
        u0.array() = u0.array().abs();

        // setup start and goal
        goal(0) = 0.;
        goal(1) = 4.;
        euclidean::setGoal(goal);
        euclidean::EuclideanObstacles &obs = euclidean::getObstacles();
        obs.clear();
        
        otmp(0) = 1.;
        otmp(1) = 2.;
        obs.push_back(otmp);
        otmp(0) = 2.;
        otmp(1) = 1.;
        obs.push_back(otmp);
        otmp(0) = 3.;
        otmp(1) = 2.;
        obs.push_back(otmp);
        otmp(0) = 1.5;
        otmp(1) = 2.;
        obs.push_back(otmp);
        otmp(0) = 2.;
        otmp(1) = 3.;
        obs.push_back(otmp);
        otmp(0) = 2.5;
        otmp(1) = 2.;
        obs.push_back(otmp);
        // init ddp
        bool init_success = DDP_init(&euclidean::dynamics, &euclidean::dynamicsD, &euclidean::cost, &euclidean::costD);
        ASSERT_TRUE(init_success);
        // and run the algorithm
        DDPParams p;
        Trajectory res;
        MPC::DDP(x0, u0, p, 200, res);
        //std::cout << res.x.transpose() << std::endl;
        ASSERT_NEAR((res.x.col(res.x.cols()-1) - goal).norm(), 0., 1e-3); 
}

TEST(MPC, DDPeuclidean3DWithObstaclesTest) {
        VectorT goal(VectorT::Zero(3));
        VectorT otmp(VectorT::Zero(3));
        VectorT x0(VectorT::Zero(3));
        MatrixT u0(MatrixT::Random(3, 100));
        u0.array() = u0.array().abs();

        // setup start and goal
        goal(0) = 0.;
        goal(1) = 4.;
        goal(2) = 1.;
        euclidean::setGoal(goal);
        euclidean::EuclideanObstacles &obs = euclidean::getObstacles();
        obs.clear();

        otmp(0) = 1.;
        otmp(1) = 2.;
        otmp(2) = 0.;
        obs.push_back(otmp);
        otmp(0) = 2.;
        otmp(1) = 1.;
        otmp(2) = 1.;
        obs.push_back(otmp);
        otmp(0) = 3.;
        otmp(1) = 2.;
        otmp(2) = 0.;
        obs.push_back(otmp);
        otmp(0) = 1.5;
        otmp(1) = 2.;
        otmp(2) = 1.;
        obs.push_back(otmp);
        otmp(0) = 2.;
        otmp(1) = 3.;
        otmp(2) = 0.;
        obs.push_back(otmp);
        otmp(0) = 2.5;
        otmp(1) = 2.;
        otmp(2) = 1.;
        obs.push_back(otmp);
        // init ddp
        bool init_success = DDP_init(&euclidean::dynamics, &euclidean::dynamicsD, &euclidean::cost, &euclidean::costD);
        ASSERT_TRUE(init_success);
        // and run the algorithm
        DDPParams p;
        Trajectory res;
        MPC::DDP(x0, u0, p, 200, res);
        //std::cout << res.x.transpose() << std::endl;
        ASSERT_NEAR((res.x.col(res.x.cols()-1) - goal).norm(), 0., 1e-3); 
}


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