kinematics_interface_test.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include <random>
#include <ros/ros.h>
#include <time.h>
#include <rdl_msgs/ChangeWrenchArrayFrameRequest.h>

#include "rdl_dynamics/Dynamics.h"
#include "rdl_dynamics/FrameOrientation.hpp"
#include "rdl_dynamics/Kinematics.h"
#include "rdl_dynamics/Model.h"
#include "rdl_dynamics/rdl_utils.h"
#include "rdl_msgs/ChangePoseFrame.h"
#include "rdl_msgs/ChangePointFrame.h"
#include "rdl_msgs/ChangeOrientationFrame.h"
#include "rdl_msgs/ChangePointArrayFrame.h"
#include "rdl_msgs/Change3DVectorFrame.h"
#include "rdl_msgs/Change3DVectorArrayFrame.h"
#include "rdl_msgs/ChangeTwistFrame.h"
#include "rdl_msgs/ChangeTwistArrayFrame.h"
#include "rdl_msgs/ChangeWrenchArrayFrame.h"
#include "rdl_msgs/ChangeWrenchFrame.h"
#include "rdl_msgs/GetTransform.h"
#include "rdl_msgs/GetTwist.h"
#include "rdl_msgs/GetBodyGravityWrench.h"
#include "rdl_msgs/GetRobotCenterOfMass.h"
#include "rdl_msgs/RobotState.h"
#include "rdl_urdfreader/urdfreader.h"

struct KinematicsInterfaceTest : public testing::Test
{
    KinematicsInterfaceTest()
    {
        srand(time(NULL));
        nh = ros::NodeHandle("kinematics_interface");
        p_nh = ros::NodeHandle("~");
        model.reset(new RobotDynamics::Model());
    }

    ~KinematicsInterfaceTest()
    {
    }

    void SetUp()
    {
    }

    void TearDown()
    {
    }

    RobotDynamics::Math::VectorNd q, qdot;
    ros::Publisher robot_state_publisher;
    rdl_msgs::RobotState robot_state_msg;
    std::map<std::string, std::string> joint_to_body_name_map;
    RobotDynamics::ModelPtr model;
    ros::NodeHandle p_nh;
    ros::NodeHandle nh;

    void randomizeStates()
    {
        for (int i = 0; i < q.size(); i++)
        {
            q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
        }

        RobotDynamics::Math::Quaternion orient;
        orient = model->GetQuaternion(2, q);
        orient.normalize();
        model->SetQuaternion(2, orient, q);

        for (int i = 0; i < qdot.size(); i++)
        {
            qdot[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
        }

        for (int i = 0; i < q.size(); i++)
        {
            robot_state_msg.position.push_back(q[i]);
        }

        for (int i = 0; i < qdot.size(); i++)
        {
            robot_state_msg.velocity.push_back(qdot[i]);
        }
    }

    void publishRobotState(unsigned int n_pubs = 10)
    {
        for (unsigned int i = 0; i < n_pubs; i++)
        {
            robot_state_publisher.publish(robot_state_msg);
            usleep(100);
        }
    }

    bool waitForSubscriber(unsigned int usec_timeout)
    {
        unsigned int total_usec = 0;
        while (robot_state_publisher.getNumSubscribers() == 0 && total_usec < usec_timeout)
        {
            total_usec += 100;
            usleep(100);
        }
    }
};

TEST_F(KinematicsInterfaceTest, change_pose_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    RobotDynamics::Math::Quaternion quat(1., 2., 3., 4.);
    quat.normalize();
    RobotDynamics::Math::FrameOrientation o(model->referenceFrameMap["test_link2"], quat);
    RobotDynamics::Math::FramePoint p(model->referenceFrameMap["test_link2"], 0.1, 0.2, 0.3);

    rdl_msgs::ChangePoseFrameRequest pose_msg_req;
    rdl_msgs::ChangePoseFrameResponse pose_msg_resp;
    pose_msg_req.new_reference_frame = "world";
    pose_msg_req.pose_in.header.frame_id = "test_link2";
    pose_msg_req.pose_in.pose.orientation.x = o.x();
    pose_msg_req.pose_in.pose.orientation.y = o.y();
    pose_msg_req.pose_in.pose.orientation.z = o.z();
    pose_msg_req.pose_in.pose.orientation.w = o.w();
    pose_msg_req.pose_in.pose.position.x = p.x();
    pose_msg_req.pose_in.pose.position.y = p.y();
    pose_msg_req.pose_in.pose.position.z = p.z();

    ros::ServiceClient pose_client = nh.serviceClient<rdl_msgs::ChangePoseFrame>("change_frame_pose");
    EXPECT_TRUE(pose_client.call(pose_msg_req, pose_msg_resp));

    EXPECT_TRUE(pose_msg_resp.success);
    if (!pose_msg_resp.success)
    {
        ROS_ERROR_STREAM("" << pose_msg_resp.report);
    }

    o.changeFrame(model->worldFrame);
    p.changeFrame(model->worldFrame);

    EXPECT_NEAR(o.x(), pose_msg_resp.pose_out.pose.orientation.x, 1.e-10);
    EXPECT_NEAR(o.y(), pose_msg_resp.pose_out.pose.orientation.y, 1.e-10);
    EXPECT_NEAR(o.z(), pose_msg_resp.pose_out.pose.orientation.z, 1.e-10);
    EXPECT_NEAR(o.w(), pose_msg_resp.pose_out.pose.orientation.w, 1.e-10);
    EXPECT_NEAR(p.x(), pose_msg_resp.pose_out.pose.position.x, 1.e-10);
    EXPECT_NEAR(p.y(), pose_msg_resp.pose_out.pose.position.y, 1.e-10);
    EXPECT_NEAR(p.z(), pose_msg_resp.pose_out.pose.position.z, 1.e-10);
    EXPECT_STREQ(pose_msg_resp.pose_out.header.frame_id.c_str(), "world");
}

TEST_F(KinematicsInterfaceTest, change_point_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    RobotDynamics::Math::FramePoint p(model->referenceFrameMap["test_link2"], 1., 2., 3.);

    rdl_msgs::ChangePointFrameRequest p_msg_req;
    rdl_msgs::ChangePointFrameResponse p_msg_resp;
    p_msg_req.new_reference_frame = "world";
    p_msg_req.point_in.header.frame_id = "test_link2";
    p_msg_req.point_in.point.x = p.x();
    p_msg_req.point_in.point.y = p.y();
    p_msg_req.point_in.point.z = p.z();

    ros::ServiceClient p_client = nh.serviceClient<rdl_msgs::ChangePointFrame>("change_frame_point");
    EXPECT_TRUE(p_client.call(p_msg_req, p_msg_resp));

    EXPECT_TRUE(p_msg_resp.success);
    if (!p_msg_resp.success)
    {
        ROS_ERROR_STREAM("" << p_msg_resp.report);
    }

    p.changeFrame(model->worldFrame);

    EXPECT_NEAR(p.x(), p_msg_resp.point_out.point.x, 1.e-10);
    EXPECT_NEAR(p.y(), p_msg_resp.point_out.point.y, 1.e-10);
    EXPECT_NEAR(p.z(), p_msg_resp.point_out.point.z, 1.e-10);
    EXPECT_STREQ(p.getReferenceFrame()->getName().c_str(), "world");
}

TEST_F(KinematicsInterfaceTest, change_orientation_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    RobotDynamics::Math::Quaternion quat(1., 2., 3., 4.);
    quat.normalize();
    RobotDynamics::Math::FrameOrientation o(model->referenceFrameMap["test_link2"], quat);

    rdl_msgs::ChangeOrientationFrameRequest o_msg_req;
    rdl_msgs::ChangeOrientationFrameResponse o_msg_resp;
    o_msg_req.new_reference_frame = "world";
    o_msg_req.orientation_in.header.frame_id = "test_link2";
    o_msg_req.orientation_in.quaternion.x = o.x();
    o_msg_req.orientation_in.quaternion.y = o.y();
    o_msg_req.orientation_in.quaternion.z = o.z();
    o_msg_req.orientation_in.quaternion.w = o.w();

    ros::ServiceClient o_client = nh.serviceClient<rdl_msgs::ChangeOrientationFrame>("change_frame_orientation");
    EXPECT_TRUE(o_client.call(o_msg_req, o_msg_resp));

    EXPECT_TRUE(o_msg_resp.success);
    if (!o_msg_resp.success)
    {
        ROS_ERROR_STREAM("" << o_msg_resp.report);
    }

    o.changeFrame(model->worldFrame);

    EXPECT_NEAR(o.x(), o_msg_resp.orientation_out.quaternion.x, 1.e-10);
    EXPECT_NEAR(o.y(), o_msg_resp.orientation_out.quaternion.y, 1.e-10);
    EXPECT_NEAR(o.z(), o_msg_resp.orientation_out.quaternion.z, 1.e-10);
    EXPECT_NEAR(o.w(), o_msg_resp.orientation_out.quaternion.w, 1.e-10);
    EXPECT_STREQ(o.getReferenceFrame()->getName().c_str(), "world");
}

TEST_F(KinematicsInterfaceTest, change_point_array_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    RobotDynamics::Math::FramePoint p(model->referenceFrameMap["test_link2"], 1., 2., 3.);
    RobotDynamics::Math::FramePoint p2(model->referenceFrameMap["test_link1"], 1., 2., 3.);

    rdl_msgs::ChangePointArrayFrameRequest p_msg_req;
    rdl_msgs::ChangePointArrayFrameResponse p_msg_resp;
    p_msg_req.new_reference_frame.push_back("world");
    geometry_msgs::PointStamped point;
    point.header.frame_id = p.getReferenceFrame()->getName();
    point.point.x = p.x();
    point.point.y = p.y();
    point.point.z = p.z();
    p_msg_req.point_array_in.push_back(point);

    p_msg_req.new_reference_frame.push_back("test_link2");
    point.header.frame_id = p2.getReferenceFrame()->getName();
    point.point.x = p2.x();
    point.point.y = p2.y();
    point.point.z = p2.z();
    p_msg_req.point_array_in.push_back(point);

    ros::ServiceClient p_client = nh.serviceClient<rdl_msgs::ChangePointArrayFrame>("change_frame_point_array");
    EXPECT_TRUE(p_client.call(p_msg_req, p_msg_resp));

    EXPECT_TRUE(p_msg_resp.success);
    if (!p_msg_resp.success)
    {
        ROS_ERROR_STREAM("" << p_msg_resp.report);
    }

    p.changeFrame(model->worldFrame);
    p2.changeFrame(model->referenceFrameMap["test_link2"]);

    EXPECT_NEAR(p.x(), p_msg_resp.point_array_out[0].point.x, 1.e-10);
    EXPECT_NEAR(p.y(), p_msg_resp.point_array_out[0].point.y, 1.e-10);
    EXPECT_NEAR(p.z(), p_msg_resp.point_array_out[0].point.z, 1.e-10);
    EXPECT_STREQ(p.getReferenceFrame()->getName().c_str(), p_msg_resp.point_array_out[0].header.frame_id.c_str());

    EXPECT_NEAR(p2.x(), p_msg_resp.point_array_out[1].point.x, 1.e-10);
    EXPECT_NEAR(p2.y(), p_msg_resp.point_array_out[1].point.y, 1.e-10);
    EXPECT_NEAR(p2.z(), p_msg_resp.point_array_out[1].point.z, 1.e-10);
    EXPECT_STREQ(p2.getReferenceFrame()->getName().c_str(), p_msg_resp.point_array_out[1].header.frame_id.c_str());
}

TEST_F(KinematicsInterfaceTest, change_3d_vector_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::Change3DVectorFrame>("change_frame_3dvector");
    RobotDynamics::Math::FrameVector v(model->referenceFrameMap["test_link2"], 0.1, 0.2, 0.3);
    rdl_msgs::Change3DVectorFrameRequest v_req;
    rdl_msgs::Change3DVectorFrameResponse v_resp;
    v_req.new_reference_frame = "test_link1";
    v_req.vector_in.header.frame_id = v.getReferenceFrame()->getName();
    v_req.vector_in.vector.x = v.x();
    v_req.vector_in.vector.y = v.y();
    v_req.vector_in.vector.z = v.z();

    v.changeFrame(model->referenceFrameMap["test_link1"]);

    v_client.call(v_req, v_resp);

    EXPECT_TRUE(v_resp.success);
    if (!v_resp.success)
    {
        ROS_ERROR_STREAM("" << v_resp.report);
    }

    EXPECT_STREQ(v_resp.vector_out.header.frame_id.c_str(), "test_link1");
    EXPECT_NEAR(v_resp.vector_out.vector.x, v.x(), 1.e-10);
    EXPECT_NEAR(v_resp.vector_out.vector.y, v.y(), 1.e-10);
    EXPECT_NEAR(v_resp.vector_out.vector.z, v.z(), 1.e-10);
}

TEST_F(KinematicsInterfaceTest, change_3dvector_array_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    RobotDynamics::Math::FrameVector v(model->referenceFrameMap["test_link2"], 1., 2., 3.);
    RobotDynamics::Math::FrameVector v2(model->referenceFrameMap["test_link1"], 1., 2., 3.);

    rdl_msgs::Change3DVectorArrayFrameRequest v_msg_req;
    rdl_msgs::Change3DVectorArrayFrameResponse v_msg_resp;
    v_msg_req.new_reference_frame.push_back("world");
    geometry_msgs::Vector3Stamped vector;
    vector.header.frame_id = v.getReferenceFrame()->getName();
    vector.vector.x = v.x();
    vector.vector.y = v.y();
    vector.vector.z = v.z();
    v_msg_req.vector_array_in.push_back(vector);

    v_msg_req.new_reference_frame.push_back("test_link2");
    vector.header.frame_id = v2.getReferenceFrame()->getName();
    vector.vector.x = v2.x();
    vector.vector.y = v2.y();
    vector.vector.z = v2.z();
    v_msg_req.vector_array_in.push_back(vector);

    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::Change3DVectorArrayFrame>("change_frame_3dvector_array");
    EXPECT_TRUE(v_client.call(v_msg_req, v_msg_resp));

    EXPECT_TRUE(v_msg_resp.success);
    if (!v_msg_resp.success)
    {
        ROS_ERROR_STREAM("" << v_msg_resp.report);
    }

    v.changeFrame(model->worldFrame);
    v2.changeFrame(model->referenceFrameMap["test_link2"]);

    EXPECT_NEAR(v.x(), v_msg_resp.vector_array_out[0].vector.x, 1.e-10);
    EXPECT_NEAR(v.y(), v_msg_resp.vector_array_out[0].vector.y, 1.e-10);
    EXPECT_NEAR(v.z(), v_msg_resp.vector_array_out[0].vector.z, 1.e-10);
    EXPECT_STREQ(v.getReferenceFrame()->getName().c_str(), v_msg_resp.vector_array_out[0].header.frame_id.c_str());

    EXPECT_NEAR(v2.x(), v_msg_resp.vector_array_out[1].vector.x, 1.e-10);
    EXPECT_NEAR(v2.y(), v_msg_resp.vector_array_out[1].vector.y, 1.e-10);
    EXPECT_NEAR(v2.z(), v_msg_resp.vector_array_out[1].vector.z, 1.e-10);
    EXPECT_STREQ(v2.getReferenceFrame()->getName().c_str(), v_msg_resp.vector_array_out[1].header.frame_id.c_str());
}

TEST_F(KinematicsInterfaceTest, change_twist_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::ChangeTwistFrame>("change_frame_twist");
    RobotDynamics::Math::SpatialMotion v(model->referenceFrameMap["world"], model->referenceFrameMap["test_link2"], model->referenceFrameMap["test_link1"], 0.1, 0.2, 0.3,
                                         0.4, 0.5, 0.6);
    rdl_msgs::ChangeTwistFrameRequest v_req;
    rdl_msgs::ChangeTwistFrameResponse v_resp;
    v_req.new_reference_frame = "test_link2";
    v_req.twist_in.header.frame_id = v.getReferenceFrame()->getName();
    v_req.twist_in.twist.linear.x = v.vx();
    v_req.twist_in.twist.linear.y = v.vy();
    v_req.twist_in.twist.linear.z = v.vz();
    v_req.twist_in.twist.angular.x = v.wx();
    v_req.twist_in.twist.angular.y = v.wy();
    v_req.twist_in.twist.angular.z = v.wz();

    v.changeFrame(model->referenceFrameMap["test_link2"]);

    v_client.call(v_req, v_resp);

    EXPECT_TRUE(v_resp.success);
    if (!v_resp.success)
    {
        ROS_ERROR_STREAM("" << v_resp.report);
    }

    EXPECT_STREQ(v_resp.twist_out.header.frame_id.c_str(), "test_link2");
    EXPECT_NEAR(v_resp.twist_out.twist.linear.x, v.vx(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.linear.y, v.vy(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.linear.z, v.vz(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.angular.x, v.wx(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.angular.y, v.wy(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.angular.z, v.wz(), 1.e-10);
}

TEST_F(KinematicsInterfaceTest, change_twist_array_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::ChangeTwistArrayFrame>("change_frame_twist_array");
    RobotDynamics::Math::SpatialMotion v(model->worldFrame, model->referenceFrameMap["test_link2"], model->referenceFrameMap["test_link1"], 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
    RobotDynamics::Math::SpatialMotion v2(model->referenceFrameMap["test_link2"], model->referenceFrameMap["test_link1"], model->referenceFrameMap["test_link1"], 0.1,
                                          0.2, 0.3, 0.4, 0.5, 0.6);
    rdl_msgs::ChangeTwistArrayFrameRequest v_req;
    rdl_msgs::ChangeTwistArrayFrameResponse v_resp;
    geometry_msgs::TwistStamped twist;
    v_req.new_reference_frame.push_back("test_link2");
    twist.header.frame_id = v.getReferenceFrame()->getName();
    twist.twist.linear.x = v.vx();
    twist.twist.linear.y = v.vy();
    twist.twist.linear.z = v.vz();
    twist.twist.angular.x = v.wx();
    twist.twist.angular.y = v.wy();
    twist.twist.angular.z = v.wz();

    v_req.twist_array_in.push_back(twist);

    v_req.new_reference_frame.push_back("world");
    twist.header.frame_id = v2.getReferenceFrame()->getName();
    twist.twist.linear.x = v2.vx();
    twist.twist.linear.y = v2.vy();
    twist.twist.linear.z = v2.vz();
    twist.twist.angular.x = v2.wx();
    twist.twist.angular.y = v2.wy();
    twist.twist.angular.z = v2.wz();

    v_req.twist_array_in.push_back(twist);

    v.changeFrame(model->referenceFrameMap["test_link2"]);
    v2.changeFrame(model->worldFrame);

    v_client.call(v_req, v_resp);

    EXPECT_TRUE(v_resp.success);
    if (!v_resp.success)
    {
        ROS_ERROR_STREAM("" << v_resp.report);
    }

    EXPECT_STREQ(v_resp.twist_array_out[0].header.frame_id.c_str(), "test_link2");
    EXPECT_NEAR(v_resp.twist_array_out[0].twist.linear.x, v.vx(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[0].twist.linear.y, v.vy(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[0].twist.linear.z, v.vz(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[0].twist.angular.x, v.wx(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[0].twist.angular.y, v.wy(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[0].twist.angular.z, v.wz(), 1.e-10);

    EXPECT_STREQ(v_resp.twist_array_out[1].header.frame_id.c_str(), "world");
    EXPECT_NEAR(v_resp.twist_array_out[1].twist.linear.x, v2.vx(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[1].twist.linear.y, v2.vy(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[1].twist.linear.z, v2.vz(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[1].twist.angular.x, v2.wx(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[1].twist.angular.y, v2.wy(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_array_out[1].twist.angular.z, v2.wz(), 1.e-10);
}

TEST_F(KinematicsInterfaceTest, change_wrench_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::ChangeWrenchFrame>("change_frame_wrench");
    RobotDynamics::Math::SpatialForce v(model->referenceFrameMap["test_link1"], 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
    rdl_msgs::ChangeWrenchFrameRequest v_req;
    rdl_msgs::ChangeWrenchFrameResponse v_resp;
    v_req.new_reference_frame = "test_link2";
    v_req.wrench_in.header.frame_id = v.getReferenceFrame()->getName();
    v_req.wrench_in.wrench.force.x = v.fx();
    v_req.wrench_in.wrench.force.y = v.fy();
    v_req.wrench_in.wrench.force.z = v.fz();
    v_req.wrench_in.wrench.torque.x = v.mx();
    v_req.wrench_in.wrench.torque.y = v.my();
    v_req.wrench_in.wrench.torque.z = v.mz();

    v.changeFrame(model->referenceFrameMap["test_link2"]);

    v_client.call(v_req, v_resp);

    EXPECT_TRUE(v_resp.success);
    if (!v_resp.success)
    {
        ROS_ERROR_STREAM("" << v_resp.report);
    }

    EXPECT_STREQ(v_resp.wrench_out.header.frame_id.c_str(), "test_link2");
    EXPECT_NEAR(v_resp.wrench_out.wrench.force.x, v.fx(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_out.wrench.force.y, v.fy(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_out.wrench.force.z, v.fz(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_out.wrench.torque.x, v.mx(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_out.wrench.torque.y, v.my(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_out.wrench.torque.z, v.mz(), 1.e-10);
}

TEST_F(KinematicsInterfaceTest, change_wrench_array_frame)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::ChangeWrenchArrayFrame>("change_frame_wrench_array");
    RobotDynamics::Math::SpatialForce v(model->referenceFrameMap["test_link1"], 0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
    RobotDynamics::Math::SpatialForce v2(model->worldFrame, 0.2, 0.4, -0.1, 0.6, -1., 0.6);
    rdl_msgs::ChangeWrenchArrayFrameRequest v_req;
    rdl_msgs::ChangeWrenchArrayFrameResponse v_resp;
    v_req.new_reference_frame.push_back("test_link2");
    geometry_msgs::WrenchStamped wrench;
    wrench.header.frame_id = v.getReferenceFrame()->getName();
    wrench.wrench.force.x = v.fx();
    wrench.wrench.force.y = v.fy();
    wrench.wrench.force.z = v.fz();
    wrench.wrench.torque.x = v.mx();
    wrench.wrench.torque.y = v.my();
    wrench.wrench.torque.z = v.mz();
    v_req.wrench_array_in.push_back(wrench);

    v_req.new_reference_frame.push_back("test_link1");
    wrench.header.frame_id = v2.getReferenceFrame()->getName();
    wrench.wrench.force.x = v2.fx();
    wrench.wrench.force.y = v2.fy();
    wrench.wrench.force.z = v2.fz();
    wrench.wrench.torque.x = v2.mx();
    wrench.wrench.torque.y = v2.my();
    wrench.wrench.torque.z = v2.mz();
    v_req.wrench_array_in.push_back(wrench);

    v.changeFrame(model->referenceFrameMap["test_link2"]);
    v2.changeFrame(model->referenceFrameMap["test_link1"]);

    v_client.call(v_req, v_resp);

    EXPECT_TRUE(v_resp.success);
    if (!v_resp.success)
    {
        ROS_ERROR_STREAM("" << v_resp.report);
    }

    EXPECT_STREQ(v_resp.wrench_array_out[0].header.frame_id.c_str(), "test_link2");
    EXPECT_NEAR(v_resp.wrench_array_out[0].wrench.force.x, v.fx(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[0].wrench.force.y, v.fy(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[0].wrench.force.z, v.fz(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[0].wrench.torque.x, v.mx(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[0].wrench.torque.y, v.my(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[0].wrench.torque.z, v.mz(), 1.e-10);

    EXPECT_STREQ(v_resp.wrench_array_out[1].header.frame_id.c_str(), "test_link1");
    EXPECT_NEAR(v_resp.wrench_array_out[1].wrench.force.x, v2.fx(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[1].wrench.force.y, v2.fy(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[1].wrench.force.z, v2.fz(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[1].wrench.torque.x, v2.mx(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[1].wrench.torque.y, v2.my(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench_array_out[1].wrench.torque.z, v2.mz(), 1.e-10);
}

TEST_F(KinematicsInterfaceTest, get_twist)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::GetTwist>("get_twist");
    RobotDynamics::Math::SpatialMotion v;
    rdl_msgs::GetTwistRequest v_req;
    rdl_msgs::GetTwistResponse v_resp;
    v_req.base_frame = "test_link2";
    v_req.body_frame = "test_link1_com";
    v_req.expressed_in_frame = "world";

    ASSERT_TRUE(v_client.call(v_req, v_resp));

    v = RobotDynamics::calcSpatialVelocity(*model, q, qdot, model->bodyCenteredFrames[model->GetBodyId("test_link1")], model->referenceFrameMap["test_link2"],
                                           model->worldFrame, false);

    EXPECT_TRUE(v_resp.success);
    if (!v_resp.success)
    {
        ROS_ERROR_STREAM("" << v_resp.report);
    }

    EXPECT_STREQ(v_resp.twist_out.header.frame_id.c_str(), "world");
    EXPECT_NEAR(v_resp.twist_out.twist.linear.x, v.vx(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.linear.y, v.vy(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.linear.z, v.vz(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.angular.x, v.wx(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.angular.y, v.wy(), 1.e-10);
    EXPECT_NEAR(v_resp.twist_out.twist.angular.z, v.wz(), 1.e-10);
}

TEST_F(KinematicsInterfaceTest, get_transform)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::GetTransform>("get_transform");
    rdl_msgs::GetTransformRequest v_req;
    rdl_msgs::GetTransformResponse v_resp;
    v_req.from_reference_frame = "test_link2";
    v_req.to_reference_frame = "test_link1_com";

    ASSERT_TRUE(v_client.call(v_req, v_resp));

    RobotDynamics::ReferenceFramePtr from_frame = model->referenceFrameMap["test_link2"];
    RobotDynamics::ReferenceFramePtr to_frame = model->bodyCenteredFrames[model->GetBodyId("test_link1")];
    RobotDynamics::Math::SpatialTransform X = from_frame->getTransformToDesiredFrame(to_frame);

    EXPECT_TRUE(v_resp.success);
    if (!v_resp.success)
    {
        ROS_ERROR_STREAM("" << v_resp.report);
    }

    v_resp.transform.header.frame_id = "test_link2";
    v_resp.transform.child_frame_id = "test_link1_com";
    EXPECT_NEAR(v_resp.transform.transform.translation.x, X.r.x(), 1.e-10);
    EXPECT_NEAR(v_resp.transform.transform.translation.y, X.r.y(), 1.e-10);
    EXPECT_NEAR(v_resp.transform.transform.translation.z, X.r.z(), 1.e-10);

    RobotDynamics::Math::Quaternion orientation = RobotDynamics::Math::toQuaternion(X.E);

    EXPECT_NEAR(orientation.x(), v_resp.transform.transform.rotation.x, 1.e-10);
    EXPECT_NEAR(orientation.y(), v_resp.transform.transform.rotation.y, 1.e-10);
    EXPECT_NEAR(orientation.z(), v_resp.transform.transform.rotation.z, 1.e-10);
    EXPECT_NEAR(orientation.w(), v_resp.transform.transform.rotation.w, 1.e-10);
}

TEST_F(KinematicsInterfaceTest, get_body_grav_wrench)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);

    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::GetBodyGravityWrench>("get_body_grav_wrench");
    RobotDynamics::Math::SpatialForce v;
    rdl_msgs::GetBodyGravityWrenchRequest v_req;
    rdl_msgs::GetBodyGravityWrenchResponse v_resp;
    v_req.body = "test_link2";

    v_client.call(v_req, v_resp);

    RobotDynamics::calcBodyGravityWrench(*model, model->GetBodyId("test_link2"), v);

    EXPECT_TRUE(v_resp.success);
    if (!v_resp.success)
    {
        ROS_ERROR_STREAM("" << v_resp.report);
    }

    EXPECT_STREQ(v_resp.wrench.header.frame_id.c_str(), "test_link2");
    EXPECT_NEAR(v_resp.wrench.wrench.force.x, v.fx(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench.wrench.force.y, v.fy(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench.wrench.force.z, v.fz(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench.wrench.torque.x, v.mx(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench.wrench.torque.y, v.my(), 1.e-10);
    EXPECT_NEAR(v_resp.wrench.wrench.torque.z, v.mz(), 1.e-10);
}

TEST_F(KinematicsInterfaceTest, get_robot_com)
{
    std::string robot_description;

    ASSERT_TRUE(p_nh.getParam("robot_description", robot_description));

    ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromString(robot_description, model));
    ASSERT_TRUE(RobotDynamics::Urdf::parseJointBodyNameMapFromString(robot_description, joint_to_body_name_map));

    q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
    qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);

    robot_state_publisher = nh.advertise<rdl_msgs::RobotState>("robot_state", 0);
    waitForSubscriber(3.e6);

    randomizeStates();

    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);

    publishRobotState(5);
    ros::ServiceClient v_client = nh.serviceClient<rdl_msgs::GetRobotCenterOfMass>("get_robot_com");
    rdl_msgs::GetRobotCenterOfMassResponse v_resp;
    rdl_msgs::GetRobotCenterOfMassRequest v_req;
    v_req;

    v_client.call(v_req, v_resp);

    RobotDynamics::Math::FramePoint p_com;
    RobotDynamics::Math::FrameVector v_com;
    RobotDynamics::Utils::calcCenterOfMass(*model, q, qdot, p_com, &v_com, false);

    EXPECT_TRUE(v_resp.success);
    if (!v_resp.success)
    {
        ROS_ERROR_STREAM("" << v_resp.report);
    }

    EXPECT_STREQ(v_resp.com.header.frame_id.c_str(), p_com.getReferenceFrame()->getName().c_str());
    EXPECT_NEAR(v_resp.com.point.x, p_com.x(), 1.e-10);
    EXPECT_NEAR(v_resp.com.point.y, p_com.y(), 1.e-10);
    EXPECT_NEAR(v_resp.com.point.z, p_com.z(), 1.e-10);

    EXPECT_NEAR(v_resp.com_vel.vector.x, v_com.x(), 1.e-10);
    EXPECT_NEAR(v_resp.com_vel.vector.y, v_com.y(), 1.e-10);
    EXPECT_NEAR(v_resp.com_vel.vector.z, v_com.z(), 1.e-10);
}

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    ros::init(argc, argv, "kinematics_interface_test");

    ros::AsyncSpinner spinner(1);
    spinner.start();
    int ret = RUN_ALL_TESTS();
    spinner.stop();
    ros::shutdown();
    return ret;
}