RosMsgGainCalculatorBenchmark.cpp

Go to the documentation of this file.

#include <hayai/hayai.hpp>
#include <ros/package.h>
#include "robodyn_controllers/RosMsgGainCalculator.h"

class RosMsgGainCalculatorBenchmark : public hayai::Fixture
{
protected:
    virtual void SetUp()
    {
        yankLim = 1.0;
        yankLimWindup = 0.05;

        std::string packagePath = ros::package::getPath("robodyn_controllers");
        rmg.loadFromFile(packagePath + "/test/urdf/r2c_full_body.urdf.xml");
        rmg.getJointNames(jointNames);

        rmg.yl.setRateLimit(yankLim);
        rmg.ylWindup.setRateLimit(yankLimWindup);

        dd.joint_names.resize(jointNames.size());
        dd.naturalFreq.resize(jointNames.size());
        dd.dampingRatio.resize(jointNames.size());
        dd.windupLimit.resize(jointNames.size());
        inertia.name.resize(jointNames.size());
        inertia.position.resize(jointNames.size());

        for (unsigned int i = 0; i < jointNames.size(); ++i)
        {
            rmg.jointGainsData[jointNames[i]].maxK   = 30;
            rmg.jointGainsData[jointNames[i]].maxD   = 30;
            rmg.jointGainsData[jointNames[i]].scaleI = 0.01;
            rmg.jointGainsData[jointNames[i]].minD   = 5.0;
            dd.joint_names[i]                        = jointNames[i];
            dd.naturalFreq[i]                        = 1000.;
            dd.dampingRatio[i]                       = 1.5;
            dd.windupLimit[i]                        = 5.;
            inertia.name[i]                          = jointNames[i];
            inertia.position[i]                      = static_cast<double>(rand() % 10001) / 1000.;
        }

        yankLim = 20.0;
        rmg.yl.setRateLimit(yankLim);

        yankLimWindup = 2.0;
        rmg.ylWindup.setRateLimit(yankLimWindup);
    }

    virtual void TearDown()
    {
    }

    RosMsgGainCalculator       rmg;
    r2_msgs::Gains dd, dg;
    double                     scaleI, minD, yankLim, yankLimWindup;
    sensor_msgs::JointState    inertia;
    std::vector<std::string>   jointNames;
};

BENCHMARK_F(RosMsgGainCalculatorBenchmark, FindDesiredDynBenchmark, 3, 1000)
{
    rmg.storeGainInformation(dd);
    rmg.findDesiredDynamics(inertia, dg);
}

int main(int argc, char** argv)
{
    hayai::Benchmarker::RunAllTests();
    return 0;
}