TrajectoryManagerBenchmark.cpp

Go to the documentation of this file.

#include <iostream>

#include <hayai/hayai.hpp>

#include "robodyn_controllers/TrajectoryManager.h"
#include "robodyn_controllers/RosMsgTreeFk.h"
#include "r2_msgs/JointCapability.h"
#include <ros/package.h>

class TrajectoryManagerBasicFixture : 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");

        trajMan.initialize(packagePath + "/test/urdf/r2c_full_body_dynamics.urdf.xml", 0.03);

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

        poseSettings.maxLinearAcceleration = 0.05;
        poseSettings.maxLinearVelocity = 0.1;
        poseSettings.maxRotationalAcceleration = 0.05;
        poseSettings.maxRotationalVelocity = 0.1;
        trajMan.setPoseSettings(poseSettings);

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

        jointSettings.joint_names             = jointStatesMsg.name;
        jointSettings.jointVelocityLimits     = std::vector<double>(jointStatesMsg.name.size(), 0.1);
        jointSettings.jointAccelerationLimits = std::vector<double>(jointStatesMsg.name.size(), 0.05);
        trajMan.setJointSettings(jointSettings);

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

        inertiaInMsg.name = jointStatesMsg.name;
        inertiaInMsg.position.resize(inertiaInMsg.name.size());

        // 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] = 0.;
            // 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.;
            jointCommandMsg.proportionalGain[i]             = 1000.;
            jointCommandMsg.derivativeGain[i]               = 50.;
            jointCommandMsg.positionLoopTorqueLimit[i]      = 20.;

            inertiaInMsg.position[i] = 50.*(static_cast<double>(rand() % 10001) / 10000.) + 1.;
        }

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

        jointTraj.joint_names = jointCommandStateMsg.name;
        jointTraj.points.resize(1);
        jointTraj.points[0].positions = jointCommandStateMsg.position;
        jointTraj.points[0].time_from_start = ros::Duration(100.);

        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];
        cartTraj.points[0].time_from_start = ros::Duration(100.);

        jointCapabilitiesMsg.name = jointStatesMsg.name;
        jointCapabilitiesMsg.positionLimitMax.resize(jointCapabilitiesMsg.name.size(), 1.57);
        jointCapabilitiesMsg.positionLimitMin.resize(jointCapabilitiesMsg.name.size(), -1.57);
        jointCapabilitiesMsg.torqueLimit.resize(jointCapabilitiesMsg.name.size(), 100.);
    }

    virtual void TearDown()
    {
    }

    class MyTrajMan : public TrajectoryManager
    {
    public:
        MyTrajMan() : TrajectoryManager("TrajManBenchmark") {}
    protected:
        virtual void writeJointState(const sensor_msgs::JointState& jointState_out) {}
        virtual void writeJointControl(const r2_msgs::JointControlDataArray& jointControl_out) {}
        virtual void writeStatus(const actionlib_msgs::GoalStatusArray& goalStatusMsg_out) {}
    };

    MyTrajMan trajMan;

    sensor_msgs::JointState                      jointStatesMsg, jointCommandStateMsg;
    r2_msgs::JointControlDataArray   jointStatusMsg;
    RosMsgTreeFk                                 treeFk;
    r2_msgs::JointCommand            jointCommandMsg;
    r2_msgs::PoseState               poseCommandMsg, poseStateMsg;
    r2_msgs::JointCapability         jointCapabilitiesMsg;
    trajectory_msgs::JointTrajectory             jointTraj;
    r2_msgs::PoseTrajectory          cartTraj;
    r2_msgs::StringArray             baseMsg;
    sensor_msgs::JointState                      inertiaInMsg;
    r2_msgs::ControllerJointSettings jointSettings;
    r2_msgs::ControllerPoseSettings  poseSettings;
};

class TrajectoryManagerJointUpdateFixture : public TrajectoryManagerBasicFixture
{
protected:
    virtual void SetUp()
    {
        TrajectoryManagerBasicFixture::SetUp();
        trajMan.addJointWaypoints(jointTraj, jointStatesMsg, jointStatesMsg, jointStatesMsg);
    }

    virtual void TearDown()
    {
    }
};

class TrajectoryManagerCartUpdateFixture : public TrajectoryManagerBasicFixture
{
protected:
    virtual void SetUp()
    {
        TrajectoryManagerBasicFixture::SetUp();
        trajMan.addCartesianWaypoints(cartTraj, jointStatesMsg, jointStatesMsg, jointStatesMsg, poseStateMsg, poseStateMsg);
        trajMan.setJointCapabilities(jointCapabilitiesMsg);
        trajMan.updateInertia(inertiaInMsg);
        trajMan.setIkParameters(5E-12, .000032, 50, .1);
    }

    virtual void TearDown()
    {
    }
};

BENCHMARK_F(TrajectoryManagerBasicFixture, BasicUpdateTest, 10, 1000)
{
    jointCommandStateMsg = jointCommandStateMsg;
    jointStatesMsg       = jointStatesMsg;
    trajMan.setJointCapabilities(jointCapabilitiesMsg);
    trajMan.updateInertia(inertiaInMsg);
    trajMan.updateTrajectories(jointCommandMsg, jointStatusMsg, jointStatesMsg, jointStatesMsg);
}

BENCHMARK_F(TrajectoryManagerJointUpdateFixture, JointUpdateTest, 10, 1000)
{
    jointCommandStateMsg = jointCommandStateMsg;
    jointStatesMsg       = jointStatesMsg;
    trajMan.setJointCapabilities(jointCapabilitiesMsg);
    trajMan.updateInertia(inertiaInMsg);
    trajMan.updateTrajectories(jointCommandMsg, jointStatusMsg, jointStatesMsg, jointStatesMsg);
}

BENCHMARK_F(TrajectoryManagerCartUpdateFixture, CartUpdateTest, 10, 1000)
{
    jointCommandStateMsg = jointCommandStateMsg;
    jointStatesMsg       = jointStatesMsg;
    trajMan.setJointCapabilities(jointCapabilitiesMsg);
    trajMan.updateInertia(inertiaInMsg);
    trajMan.updateTrajectories(jointCommandMsg, jointStatusMsg, jointStatesMsg, jointStatesMsg);
}

BENCHMARK_F(TrajectoryManagerBasicFixture, AddJointTrajTest, 10, 100)
{
    jointCommandStateMsg   = jointCommandStateMsg;
    jointStatesMsg         = jointStatesMsg;
    jointTraj.header.stamp = ros::Time::now();
    trajMan.addJointWaypoints(jointTraj, jointStatesMsg, jointStatesMsg, jointStatesMsg);
    trajMan.setJointCapabilities(jointCapabilitiesMsg);
    trajMan.updateInertia(inertiaInMsg);
    trajMan.updateTrajectories(jointCommandMsg, jointStatusMsg, jointStatesMsg, jointStatesMsg);
}

BENCHMARK_F(TrajectoryManagerBasicFixture, AddCartTrajTest, 10, 100)
{
    jointCommandStateMsg = jointCommandStateMsg;
    jointStatesMsg       = jointStatesMsg;
    trajMan.setIkParameters(5E-12, .000032, 50, .1);
    cartTraj.header.stamp = ros::Time::now();
    trajMan.addCartesianWaypoints(cartTraj, jointStatesMsg, jointStatesMsg, jointStatesMsg, poseStateMsg, poseStateMsg);
    trajMan.setJointCapabilities(jointCapabilitiesMsg);
    trajMan.updateInertia(inertiaInMsg);
    trajMan.updateTrajectories(jointCommandMsg, jointStatusMsg, jointStatesMsg, jointStatesMsg);
}

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