MultiLoopController_Test.cpp

Go to the documentation of this file.

#include <iostream>
#include <gtest/gtest.h>
#include "robodyn_utilities/MultiLoopController.h"
#include <ros/package.h>
#include <stdexcept>

class MultiLoopControllerTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        mlc.setLoopRate(100);
        mlc.setTorqueLoopParameters(0, 0, 0, 0, true);
        mlc.setPositionLoopParameters(0, 10, 100);
        mlc.setVelocityLoopParameters(-10, 10, 10, 10, 5);
        mlc.setCurrentLoopParameters(-10, 10, 10);
        mlc.setHardwareParameters(0, 0, 1, 20, 0.1, 0, 1, 50000, 0.000002, 0.0000002);
    }

    virtual void TearDown()
    {
    }

    MultiLoopController mlc;
};

TEST_F(MultiLoopControllerTest, setBadParams)
{
    EXPECT_THROW(mlc.setLoopRate(0),                                            std::runtime_error);
    EXPECT_THROW(mlc.setPositionLoopParameters(10, 0, 100),                     std::runtime_error);
    EXPECT_THROW(mlc.setTorqueLoopParameters(10, 0, 100, 0),                    std::runtime_error);
    EXPECT_THROW(mlc.setVelocityLoopParameters(10, -10, 10, 10, 5),             std::runtime_error);
    EXPECT_THROW(mlc.setVelocityLoopParameters(-10, 10, 10, 10, -5),            std::runtime_error);
    EXPECT_THROW(mlc.setCurrentLoopParameters(10, -10, 10),                     std::runtime_error);
    EXPECT_THROW(mlc.setCurrentLoopParameters(-10, 10, -10),                    std::runtime_error);
    EXPECT_THROW(mlc.setHardwareParameters(0, 0, 0, 1, 1, 0, 1, 200, .01, .01), std::runtime_error);
    EXPECT_THROW(mlc.setHardwareParameters(0, 0, 1, 1, 1, 0, 0, 200, .01, .01), std::runtime_error);
    EXPECT_THROW(mlc.setHardwareParameters(0, 0, 1, 1, 1, 0, 1, .5, 1, .01),    std::runtime_error);
}

TEST_F(MultiLoopControllerTest, badInput)
{
    EXPECT_THROW(mlc.currentLoop(1, 1, 0), std::runtime_error);
}

TEST_F(MultiLoopControllerTest, currentLoop)
{
    mlc.setCurrentLoopParameters(0, 0, 0);
    EXPECT_EQ(0, mlc.currentLoop(0, 0, 48));

    // motcom limit
    mlc.setCurrentLoopParameters(-1000., 1000., 200.);
    EXPECT_EQ(200, mlc.currentLoop(1000, 1000, 48));

    // current limit
    mlc.reset();
    mlc.setCurrentLoopParameters(-1., 1., 200.);
    EXPECT_EQ(102, mlc.currentLoop(1000, 0, 50));
}

TEST_F(MultiLoopControllerTest, zeros)
{
    mlc.setPositionLoopParameters(0, 100, 0);
    mlc.setVelocityLoopParameters(0, 0, 0, 0, 0);
    mlc.setCurrentLoopParameters(0, 0, 0);

    double vel;
    EXPECT_EQ(0, mlc.update(0, 50, 0, 48, vel));

    mlc.setPositionLoopParameters(5, 70, 0);
    mlc.setVelocityLoopParameters(0, 0, 0, 0, 0);
    mlc.setCurrentLoopParameters(-.25, .25, 200);

    EXPECT_EQ(0, mlc.update(0, 50, 0, 48, vel));
}

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