InvDynIntegratedTest.cpp

Go to the documentation of this file.

#include <iostream>

#include <gtest/gtest.h>

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

#include <ros/package.h>

#include <ros/package.h>
#include <kdl/treefksolverpos_recursive.hpp>


class InvDynIntegratedTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        ros::Time::init();
    }

    virtual void TearDown()
    {
    }

    KdlTreeId                        id;
    RosMsgTreeId                     rmt;
    JointDynamicsData                jd;
    sensor_msgs::JointState          actualState;
    sensor_msgs::JointState          traj1;
    r2_msgs::WrenchState wrenchState;

};

TEST_F(InvDynIntegratedTest, SimpleDynTest)
{
    std::string packagePath = ros::package::getPath("robodyn_controllers");
    id.loadFromFile(packagePath + "/test/urdf/SimpleDynRobot.xml");
    jd.InitializeMaps(id.getTree());

    actualState.name.push_back("joint1");
    actualState.position.push_back(0.0);

    traj1.name.push_back("joint1");
    traj1.position.push_back(1.0);
    traj1.velocity.push_back(0.0);
    traj1.effort.push_back(0.0);

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

    sensor_msgs::JointState tauFF, Hv;

    // Finally start the test
    // Actual state populated only
    std::cout << "Actual state only" << std::endl;

    rmt.setJointData(actualState, traj1, wrenchState, jd);

    id.treeRecursiveNewtonEuler(jd, "link1", "null", velIn, accelIn, forceOut, inertiaOut);
    std::cout << "forceOut: " << forceOut.force.data[0] << " " << forceOut.force.data[1] << " " << forceOut.force.data[2] << " " << forceOut.torque.data[0] << " " << forceOut.torque.data[1] << " " << forceOut.torque.data[2] << std::endl;
    std::cout << "inertiaOut: " << inertiaOut.getMass() << std::endl;

    rmt.getJointCommand(jd, tauFF);

    rmt.getJointInertias(jd, Hv);

    for (unsigned int i = 0; i < tauFF.name.size(); i++)
    {
        std::cout << "Tau name: " << tauFF.name[i] << " tau: " << tauFF.effort[i] << std::endl;
    }

    for (unsigned int i = 0; i < Hv.name.size(); i++)
    {
        std::cout << "Hv name: " << Hv.name[i] << " tau: " << Hv.position[i] << std::endl;
    }

    // One trajectory populated
    std::cout << "No trajectory with gravity" << std::endl;

    accelIn = KDL::Twist(KDL::Vector(0, 9.8, 0), KDL::Vector::Zero());

    rmt.setJointData(actualState, traj1, wrenchState, jd);

    id.treeRecursiveNewtonEuler(jd, "link1", "null", velIn, accelIn, forceOut, inertiaOut);
    std::cout << "forceOut: " << forceOut.force.data[0] << " " << forceOut.force.data[1] << " " << forceOut.force.data[2] << " " << forceOut.torque.data[0] << " " << forceOut.torque.data[1] << " " << forceOut.torque.data[2] << std::endl;
    std::cout << "inertiaOut: " << inertiaOut.getMass() << std::endl;

    rmt.getJointCommand(jd, tauFF);

    rmt.getJointInertias(jd, Hv);

    for (unsigned int i = 0; i < tauFF.name.size(); i++)
    {
        std::cout << "Tau name: " << tauFF.name[i] << " tau: " << tauFF.effort[i] << std::endl;
    }

    for (unsigned int i = 0; i < Hv.name.size(); i++)
    {
        std::cout << "Hv name: " << Hv.name[i] << " tau: " << Hv.position[i] << std::endl;
    }

    // One trajectory with gravity
    std::cout << "One trajectory with gravity" << std::endl;

    traj1.velocity.clear();
    traj1.effort.clear();
    traj1.velocity.push_back(1.0);
    traj1.effort.push_back(1.0);

    rmt.setJointData(actualState, traj1, wrenchState, jd);

    id.treeRecursiveNewtonEuler(jd, "link1", "null", velIn, accelIn, forceOut, inertiaOut);
    std::cout << "forceOut: " << forceOut.force.data[0] << " " << forceOut.force.data[1] << " " << forceOut.force.data[2] << std::endl;
    std::cout << "inertiaOut: " << inertiaOut.getMass() << std::endl;
    rmt.getJointCommand(jd, tauFF);

    rmt.getJointInertias(jd, Hv);

    for (unsigned int i = 0; i < tauFF.name.size(); i++)
    {
        std::cout << "Tau name: " << tauFF.name[i] << " tau: " << tauFF.effort[i] << std::endl;
    }

    for (unsigned int i = 0; i < Hv.name.size(); i++)
    {
        std::cout << "Hv name: " << Hv.name[i] << " tau: " << Hv.position[i] << std::endl;
    }

}

TEST_F(InvDynIntegratedTest, BranchingTest)
{
    std::string packagePath = ros::package::getPath("robodyn_controllers");
    id.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml");
    jd.InitializeMaps(id.getTree());

    actualState.name.push_back("joint0");
    actualState.name.push_back("joint1");
    actualState.name.push_back("joint2");
    actualState.name.push_back("joint3");

    actualState.position.push_back(0.0);
    actualState.position.push_back(0.0);
    actualState.position.push_back(5.0);
    actualState.position.push_back(5.0);

    traj1.name.push_back("joint0");
    traj1.position.push_back(0.0);
    traj1.velocity.push_back(0.0);
    traj1.effort.push_back(0.0);

    traj1.name.push_back("joint1");
    traj1.position.push_back(1.0);
    traj1.velocity.push_back(1.0);
    traj1.effort.push_back(1.0);

    traj1.name.push_back("joint2");
    traj1.name.push_back("joint3");

    traj1.position.push_back(2.0);
    traj1.position.push_back(2.0);

    traj1.velocity.push_back(2.0);
    traj1.velocity.push_back(2.0);

    traj1.effort.push_back(2.0);
    traj1.effort.push_back(2.0);

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

    sensor_msgs::JointState tauFF, Hv;

    // Finally start the test
    // Actual state populated only
    std::cout << "Trajectory no gravity" << std::endl;

    rmt.setJointData(actualState, traj1, wrenchState, jd);

    id.treeRecursiveNewtonEuler(jd, "linkb", "null", velIn, accelIn, forceOut, inertiaOut);
    std::cout << "forceOut: " << forceOut.force.data[0] << " " << forceOut.force.data[1] << " " << forceOut.force.data[2] << std::endl;
    std::cout << "inertiaOut: " << inertiaOut.getMass() << std::endl;

    rmt.getJointCommand(jd, tauFF);

    rmt.getJointInertias(jd, Hv);

    for (unsigned int i = 0; i < tauFF.name.size(); i++)
    {
        std::cout << "Tau name: " << tauFF.name[i] << " tau: " << tauFF.effort[i] << std::endl;
    }

    for (unsigned int i = 0; i < Hv.name.size(); i++)
    {
        std::cout << "Hv name: " << Hv.name[i] << " tau: " << Hv.position[i] << std::endl;
    }

    // Two trajectories with gravity
    std::cout << "Trajectory with gravity" << std::endl;

    accelIn = KDL::Twist(KDL::Vector(0, 9.8, 0), KDL::Vector::Zero());

    rmt.setJointData(actualState, traj1, wrenchState, jd);

    id.treeRecursiveNewtonEuler(jd, "linkb", "null", velIn, accelIn, forceOut, inertiaOut);
    std::cout << "forceOut: " << forceOut.force.data[0] << " " << forceOut.force.data[1] << " " << forceOut.force.data[2] << std::endl;
    std::cout << "inertiaOut: " << inertiaOut.getMass() << std::endl;

    rmt.getJointCommand(jd, tauFF);

    rmt.getJointInertias(jd, Hv);

    for (unsigned int i = 0; i < tauFF.name.size(); i++)
    {
        std::cout << "Tau name: " << tauFF.name[i] << " tau: " << tauFF.effort[i] << std::endl;
    }

    for (unsigned int i = 0; i < Hv.name.size(); i++)
    {
        std::cout << "Hv name: " << Hv.name[i] << " tau: " << Hv.position[i] << std::endl;
    }

}

TEST_F(InvDynIntegratedTest, R2LegsTest)
{
    std::string packagePath = ros::package::getPath("robodyn_controllers");
    id.loadFromFile(packagePath + "/test/urdf/r2c_legs_control.urdf.xml");
    jd.InitializeMaps(id.getTree());

    actualState.name.push_back("/r2/left_leg/joint0");
    actualState.name.push_back("/r2/left_leg/joint1");
    actualState.name.push_back("/r2/left_leg/joint2");
    actualState.name.push_back("/r2/left_leg/joint3");
    actualState.name.push_back("/r2/left_leg/joint4");
    actualState.name.push_back("/r2/left_leg/joint5");
    actualState.name.push_back("/r2/left_leg/joint6");
    actualState.name.push_back("/r2/right_leg/joint0");
    actualState.name.push_back("/r2/right_leg/joint1");
    actualState.name.push_back("/r2/right_leg/joint2");
    actualState.name.push_back("/r2/right_leg/joint3");
    actualState.name.push_back("/r2/right_leg/joint4");
    actualState.name.push_back("/r2/right_leg/joint5");
    actualState.name.push_back("/r2/right_leg/joint6");

    actualState.position.resize(actualState.name.size(), 0.);
    actualState.position[3] = 90 * 3.14159 / 180;
    actualState.position[8] = -90 * 3.14159 / 180;

    actualState.velocity.resize(actualState.name.size(), 0.);
    actualState.effort.resize(actualState.name.size(), 0.);

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

    sensor_msgs::JointState tauFF;

    accelIn = KDL::Twist(KDL::Vector(0, 0, 9.81), KDL::Vector::Zero());

    rmt.setJointData(actualState, actualState, wrenchState, jd);

    id.treeRecursiveNewtonEuler(jd, "r2/robot_world", "null", velIn, accelIn, forceOut, inertiaOut);
    std::cout << "forceOut: " << forceOut.force.data[0] << " " << forceOut.force.data[1] << " " << forceOut.force.data[2] << std::endl;
    std::cout << "inertiaOut: " << inertiaOut.getMass() << std::endl;

    rmt.getJointCommand(jd, tauFF);

    for (unsigned int i = 0; i < tauFF.name.size(); i++)
    {
        std::cout << "Tau name: " << tauFF.name[i] << " tau: " << tauFF.effort[i] << std::endl;
    }

    // check
    //EXPECT_LT(fabs(tauFF.effort[3] - 70.058), .7);
    //EXPECT_LT(fabs(tauFF.effort[8] + 148.358), 1.5);
}

TEST_F(InvDynIntegratedTest, R2LegsArgos)
{
    std::string packagePath = ros::package::getPath("robodyn_controllers");
    id.loadFromFile(packagePath + "/test/urdf/r2c_legs_control_wUpperMass.urdf.xml");
    jd.InitializeMaps(id.getTree());

    actualState.name.push_back("/r2/left_leg/joint0");
    actualState.name.push_back("/r2/left_leg/joint1");
    actualState.name.push_back("/r2/left_leg/joint2");
    actualState.name.push_back("/r2/left_leg/joint3");
    actualState.name.push_back("/r2/left_leg/joint4");
    actualState.name.push_back("/r2/left_leg/joint5");
    actualState.name.push_back("/r2/left_leg/joint6");
    actualState.name.push_back("/r2/right_leg/joint0");
    actualState.name.push_back("/r2/right_leg/joint1");
    actualState.name.push_back("/r2/right_leg/joint2");
    actualState.name.push_back("/r2/right_leg/joint3");
    actualState.name.push_back("/r2/right_leg/joint4");
    actualState.name.push_back("/r2/right_leg/joint5");
    actualState.name.push_back("/r2/right_leg/joint6");

    actualState.position.resize(actualState.name.size(), 0.);
    actualState.position[1] = -90 * 3.14159 / 180;
    actualState.position[8] = -90 * 3.14159 / 180;

    actualState.velocity.resize(actualState.name.size(), 0.);
    actualState.effort.resize(actualState.name.size(), 0.);

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

    sensor_msgs::JointState tauFF;

    // trajectory and force
//    wrenchState.name.push_back("r2/robot_world");
//    geometry_msgs::Vector3 init;
//    init.x = 0.0; init.y = 0.0; init.z = 9.81*(71.43+146);
//    geometry_msgs::Wrench initW;
//    initW.force = init;  initW.torque = geometry_msgs::Vector3();
//    wrenchState.wrench.push_back(initW);

    r2_msgs::WrenchState emptyForceMsg;
    sensor_msgs::JointState          emptyTrajMsg;
    rmt.setJointData(actualState, actualState, wrenchState, jd);

    std::string baseFrame("r2/right_leg/gripper/tip");

    id.getAccelInBaseFrame(KDL::Vector(0, 0, 9.81), "r2/robot_world", jd, baseFrame, accelIn);
    std::cout << "gravity in base frame: " << accelIn.vel.x() << ", " << accelIn.vel.y() << ", " << accelIn.vel.z() << std::endl;
    id.treeRecursiveNewtonEuler(jd, baseFrame, "null", zeroTwist, zeroTwist, forceOut, inertiaOut);

    rmt.getJointCommand(jd, tauFF);

    for (unsigned int i = 0; i < tauFF.name.size(); i++)
    {
        std::cout << "Tau name: " << tauFF.name[i] << " tau: " << tauFF.effort[i] << std::endl;
    }

    // check
    //EXPECT_LT(fabs(tauFF.effort[1] + 148.358), 1.5);
    //EXPECT_EQ(0., tauFF.effort[8]);
}

TEST_F(InvDynIntegratedTest, R2FullBody)
{
    std::string packagePath = ros::package::getPath("robodyn_controllers");
    id.loadFromFile(packagePath + "/test/urdf/r2c_full_body_dynamics.urdf.xml");
    jd.InitializeMaps(id.getTree());

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

    actualState.position.resize(actualState.name.size(), 0.);
    actualState.position[1] = -90 * 3.14159 / 180;
    actualState.position[8] = -90 * 3.14159 / 180;

    actualState.velocity.resize(actualState.name.size(), 0.);
    actualState.effort.resize(actualState.name.size(), 0.);

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

    sensor_msgs::JointState tauFF;

    // trajectory and force
//    wrenchState.name.push_back("r2/robot_world");
//    geometry_msgs::Vector3 init;
//    init.x = 0.0; init.y = 0.0; init.z = 9.81*(71.43+146);
//    geometry_msgs::Wrench initW;
//    initW.force = init;  initW.torque = geometry_msgs::Vector3();
//    wrenchState.wrench.push_back(initW);

    r2_msgs::WrenchState              emptyForceMsg;
    sensor_msgs::JointState          emptyTrajMsg;
    rmt.setJointData(actualState, actualState, wrenchState, jd);

    std::string baseFrame("r2/right_leg/gripper/tip");

    id.getAccelInBaseFrame(KDL::Vector(0, 0, 9.81), "r2/robot_world", jd, baseFrame, accelIn);
    std::cout << "gravity in base frame: " << accelIn.vel.x() << ", " << accelIn.vel.y() << ", " << accelIn.vel.z() << std::endl;
    // Gravity Comp
    id.treeRecursiveNewtonEuler(jd, baseFrame, "null", velIn, accelIn, forceOut, inertiaOut);

    rmt.getJointCommand(jd, tauFF);

    for (unsigned int i = 0; i < tauFF.name.size(); i++)
    {
        std::cout << "Tau name: " << tauFF.name[i] << " tau: " << tauFF.effort[i] << std::endl;
    }

    // check
    //EXPECT_LT(fabs(tauFF.effort[6] + 148.358), 1.5);
    //EXPECT_EQ(0., tauFF.effort[22]);
}

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