TrajectoryMonitorBenchmark.cpp

Go to the documentation of this file.

#include <iostream>

#include <hayai/hayai.hpp>

#include "robodyn_controllers/TrajectoryMonitor.h"
#include "robodyn_controllers/RosMsgTreeFk.h"
#include <ros/package.h>

class TrajectoryMonitorBasicFixture : public hayai::Fixture
{
protected:
    virtual void SetUp()
    {
        ros::Time::init();

        std::string packagePath = ros::package::getPath("robodyn_controllers");
        treeFk.loadFromFile(packagePath + "/test/urdf/r2c_full_body_dynamics.urdf.xml");

        baseMsg.data.resize(1);
        baseMsg.data[0] = "r2/robot_world";
        trajMon.setBases(baseMsg);

        jointStatesMsg.name.push_back("/r2/left_leg/joint0");
        jointStatesMsg.name.push_back("/r2/left_leg/joint1");
        jointStatesMsg.name.push_back("/r2/left_leg/joint2");
        jointStatesMsg.name.push_back("/r2/left_leg/joint3");
        jointStatesMsg.name.push_back("/r2/left_leg/joint4");
        jointStatesMsg.name.push_back("/r2/left_leg/joint5");
        jointStatesMsg.name.push_back("/r2/left_leg/joint6");
        jointStatesMsg.name.push_back("/r2/right_leg/joint0");
        jointStatesMsg.name.push_back("/r2/right_leg/joint1");
        jointStatesMsg.name.push_back("/r2/right_leg/joint2");
        jointStatesMsg.name.push_back("/r2/right_leg/joint3");
        jointStatesMsg.name.push_back("/r2/right_leg/joint4");
        jointStatesMsg.name.push_back("/r2/right_leg/joint5");
        jointStatesMsg.name.push_back("/r2/right_leg/joint6");
        jointStatesMsg.name.push_back("/r2/waist/joint0");
        jointStatesMsg.name.push_back("/r2/left_arm/joint0");
        jointStatesMsg.name.push_back("/r2/left_arm/joint1");
        jointStatesMsg.name.push_back("/r2/left_arm/joint2");
        jointStatesMsg.name.push_back("/r2/left_arm/joint3");
        jointStatesMsg.name.push_back("/r2/left_arm/joint4");
        jointStatesMsg.name.push_back("/r2/left_arm/joint5");
        jointStatesMsg.name.push_back("/r2/left_arm/joint6");
        jointStatesMsg.name.push_back("/r2/right_arm/joint0");
        jointStatesMsg.name.push_back("/r2/right_arm/joint1");
        jointStatesMsg.name.push_back("/r2/right_arm/joint2");
        jointStatesMsg.name.push_back("/r2/right_arm/joint3");
        jointStatesMsg.name.push_back("/r2/right_arm/joint4");
        jointStatesMsg.name.push_back("/r2/right_arm/joint5");
        jointStatesMsg.name.push_back("/r2/right_arm/joint6");
        jointStatesMsg.name.push_back("/r2/neck/joint0");
        jointStatesMsg.name.push_back("/r2/neck/joint1");
        jointStatesMsg.name.push_back("/r2/neck/joint2");

        jointStatesMsg.position.resize(jointStatesMsg.name.size(), 0.);
        jointStatesMsg.velocity.resize(jointStatesMsg.name.size(), 0.);
        jointStatesMsg.effort.resize(jointStatesMsg.name.size(), 0.);

        jointStatusMsg.joint = jointStatesMsg.name;
        jointStatusMsg.data.resize(jointStatusMsg.joint.size());

        jointCommandMsg.name = jointStatesMsg.name;
        jointCommandMsg.desiredPosition.resize(jointCommandMsg.name.size(), 0.);
        jointCommandMsg.desiredPositionVelocityLimit.resize(jointCommandMsg.name.size(), 0.);
        jointCommandMsg.proportionalGain.resize(jointCommandMsg.name.size(), 0.);
        jointCommandMsg.derivativeGain.resize(jointCommandMsg.name.size(), 0.);
        jointCommandMsg.positionLoopTorqueLimit.resize(jointCommandMsg.name.size(), 0.);

        // add some random data
        for (unsigned int i = 0; i < jointStatesMsg.name.size(); ++i)
        {
            // position between -90 and 90 deg
            jointStatesMsg.position[i] = 3.1416 * (static_cast<double>(rand() % 10001) / 10000.) - 1.5708;
            // velocity between -1 and 1 rad/s
            jointStatesMsg.velocity[i] = 2.*(static_cast<double>(rand() % 10001) / 10000.) - 1.;
            // effort between -100 and 100 N/m
            jointStatesMsg.effort[i]   = 200.*(static_cast<double>(rand() % 10001) / 10000.) - 100.;

            jointStatusMsg.data[i].coeffState.state = rand() % 2;

            jointCommandMsg.desiredPosition[i] = jointStatesMsg.position[i];

            if (rand() % 5 == 0)
            {
                jointCommandMsg.desiredPosition[i] += 0.2 * (static_cast<double>(rand() % 10001) / 10000.) - 0.1;
            }

            jointCommandMsg.desiredPositionVelocityLimit[i] = 0.2 * (static_cast<double>(rand() % 10001) / 10000.) - 0.1;
            jointCommandMsg.proportionalGain[i]             = 1000.;
            jointCommandMsg.derivativeGain[i]               = 50.;
            jointCommandMsg.positionLoopTorqueLimit[i]      = 20.;
        }

        jointCommandStateMsg.name     = jointCommandMsg.name;
        jointCommandStateMsg.position = jointCommandMsg.desiredPosition;
        jointCommandStateMsg.velocity = jointCommandMsg.desiredPositionVelocityLimit;
        jointCommandStateMsg.effort   = jointCommandMsg.feedForwardTorque;
        treeFk.getPoseState(jointCommandStateMsg, poseCommandMsg);

        jointTraj.joint_names = jointCommandStateMsg.name;
        jointTraj.points.resize(1);
        jointTraj.points[0].positions = jointCommandStateMsg.position;

        unsigned int nodeIndex = rand() % poseCommandMsg.name.size();
        cartTraj.nodes.resize(1);
        cartTraj.nodes[0]  = poseCommandMsg.name[nodeIndex];
        cartTraj.refFrames = cartTraj.nodes;
        cartTraj.points.resize(1);
        cartTraj.points[0].positions.resize(1);
        cartTraj.points[0].positions[0] = poseCommandMsg.positions[nodeIndex];
    }

    virtual void TearDown()
    {
    }

    class MyTrajMon : public TrajectoryMonitor
    {
    public:
        MyTrajMon() : TrajectoryMonitor("TrajMonBenchmark") {}
    protected:
        virtual void writeCartesianWaypoints(const r2_msgs::PoseTrajectoryReplan& trajectory) {}
        virtual void writeJointWaypoints(const r2_msgs::JointTrajectoryReplan& trajectory) {}
        virtual void writeBases(const r2_msgs::StringArray& basesMsg) {}
        virtual void writeStatus(const actionlib_msgs::GoalStatusArray& goalStatusMsg_out) {}
    };

    MyTrajMon trajMon;
    sensor_msgs::JointState jointStatesMsg,     jointCommandStateMsg;
    r2_msgs::JointControlDataArray  jointStatusMsg;
    r2_msgs::JointCommand           jointCommandMsg;
    RosMsgTreeFk                                treeFk;
    r2_msgs::PoseState              poseCommandMsg;
    r2_msgs::JointCapability        jointCapabilitiesMsg;
    trajectory_msgs::JointTrajectory            jointTraj;
    r2_msgs::PoseTrajectory         cartTraj;
    r2_msgs::StringArray            baseMsg;
};

class TrajectoryMonitorUpdateFixture : public TrajectoryMonitorBasicFixture
{
protected:
    virtual void SetUp()
    {
        TrajectoryMonitorBasicFixture::SetUp();
        trajMon.addJointWaypoints(jointTraj);
        trajMon.addCartesianWaypoints(cartTraj, poseCommandMsg);
    }

    virtual void TearDown()
    {
    }
};

BENCHMARK_F(TrajectoryMonitorBasicFixture, AddTrajTest, 10, 1000)
{
    trajMon.addJointWaypoints(jointTraj);
    trajMon.addCartesianWaypoints(cartTraj, poseCommandMsg);
}

BENCHMARK_F(TrajectoryMonitorUpdateFixture, UpdateTest, 10, 1000)
{
    trajMon.updateMonitor(jointCommandMsg, poseCommandMsg);
    trajMon.evaluateReplanTrigger(jointStatesMsg, jointCommandMsg, jointStatusMsg, jointCapabilitiesMsg);
    trajMon.replan();
}

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