TreeIdBenchmark.cpp

Go to the documentation of this file.

#include <iostream>

#include <hayai/hayai.hpp>

#include "robodyn_controllers/RosMsgTreeId.h"
#include "robodyn_controllers/KdlTreeId.h"
#include "robodyn_controllers/JointDynamicsData.h"
#include <ros/package.h>

#include <kdl/treefksolverpos_recursive.hpp>


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

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

        baseFrame = "/r2/right_leg/gripper/tip";

        jd.InitializeMaps(id.getTree());

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

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

        trajStateMsg.name = actualStateMsg.name;
        trajStateMsg.position.resize(trajStateMsg.name.size(), 0.);
        trajStateMsg.velocity.resize(trajStateMsg.name.size(), 0.);
        trajStateMsg.effort.resize(trajStateMsg.name.size(), 0.);

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

            // acceleration between -1 and 1 rad/s^2
            trajStateMsg.effort[i]     = 2.*(static_cast<double>(rand() % 10001) / 10000.) - 1.;
            // velocity based on actual and acceleration and rate of 30Hz
            trajStateMsg.velocity[i]   = actualStateMsg.velocity[i] + trajStateMsg.effort[i] / 30.;
            // position based on actual and velocity and rate of 30Hz
            trajStateMsg.position[i]   = actualStateMsg.position[i] + trajStateMsg.velocity[i] / 30.;
        }

        gravity_kdl   = KDL::Vector(0, 0, -9.81);
        gravity_frame = "/r2/imu";
    }

    virtual void TearDown()
    {
    }

    KdlTreeId   id;
    std::string baseFrame;

    RosMsgTreeId                     rmt;
    sensor_msgs::JointState          actualStateMsg;
    sensor_msgs::JointState          trajStateMsg;
    r2_msgs::WrenchState forceMsg;
    JointDynamicsData                jd;

    KDL::Vector gravity_kdl;
    std::string gravity_frame;

    KDL::Twist            velIn, accelIn;
    KDL::Wrench           forceOut;
    KDL::RigidBodyInertia inertiaOut;

    sensor_msgs::JointState          jointTorqueMsg;
    sensor_msgs::JointState          jointInertiaMsg;
    r2_msgs::WrenchState segForceMsg;
};

BENCHMARK_F(TreeIdBenchmark, IntegratedTest, 3, 1000)
{
    id.isBaseFrameInTree(baseFrame);
    id.isBaseFrameInTree(baseFrame); // run twice in update hook

    rmt.setJointData(actualStateMsg, trajStateMsg, forceMsg, jd);

    // Find gravity vector and do dynamics
    id.getAccelInBaseFrame(gravity_kdl, gravity_frame, jd, baseFrame, accelIn);

    // Gravity Comp
    id.treeRecursiveNewtonEuler(jd, baseFrame, "null", velIn, accelIn, forceOut, inertiaOut);
    rmt.getJointCommand(jd, jointTorqueMsg);

    // Inertia and force happens regardless
    rmt.getJointInertias(jd, jointInertiaMsg);
    rmt.getSegmentForces(jd, segForceMsg);
}

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