kinematics_interface.cpp

Go to the documentation of this file.

/*
 * RDL - Robot Dynamics Library
 * Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#include <geometry_msgs/PointStamped.h>
#include <ros/ros.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_eigenmath.h"
#include "rdl_dynamics/rdl_utils.h"
#include "rdl_msgs/ChangeOrientationFrame.h"
#include "rdl_msgs/ChangePointFrame.h"
#include "rdl_msgs/ChangePointArrayFrame.h"
#include "rdl_msgs/ChangePoseFrame.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"

RobotDynamics::Math::VectorNd q, qdot;
std::map<std::string, std::string> joint_to_body_name_map;
std::map<std::string, unsigned int> joint_name_q_index;
RobotDynamics::ModelPtr model;

void updateKinematics()
{
    RobotDynamics::updateKinematicsCustom(*model, &q, &qdot, nullptr);
}

void robotStateCallback(const rdl_msgs::RobotState& msg)
{
    if (msg.position.size() != model->q_size)
    {
        std::cout << "kinematics_interface: position vector has incorrect size. Should be " << model->q_size << std::endl;
        return;
    }

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

    if (msg.velocity.size() != model->qdot_size)
    {
        qdot.setZero();
    }
    else
    {
        for (unsigned int i = 0; i < model->qdot_size; i++)
        {
            qdot[i] = msg.velocity[i];
        }
    }

    updateKinematics();
}

bool hasEnding(std::string const& fullString, std::string const& ending)
{
    if (fullString.length() >= ending.length())
    {
        return (0 == fullString.compare(fullString.length() - ending.length(), ending.length(), ending));
    }
    else
    {
        return false;
    }
}

RobotDynamics::ReferenceFramePtr getFrame(const std::string& name)
{
    try
    {
        if (!std::strcmp(name.c_str(), "world"))
        {
            return model->worldFrame;
        }
        else
        {
            return model->referenceFrameMap.at(name);
        }
    }
    catch (const std::out_of_range& e)
    {
        // Check and see if it's a link COM frame. All com frame
        // names end with '_com'
        if (hasEnding(name, "_com"))
        {
            // Name does end in '_com', now get the body name the com frame is on
            std::string parent_body_name = name.substr(0, name.size() - 4);

            // now get the body if of the body the com frame lives on
            unsigned int parent_body_id = model->GetBodyId(parent_body_name.c_str());

            // didn't find the body
            if (parent_body_id == std::numeric_limits<unsigned int>::max())
            {
                return RobotDynamics::ReferenceFramePtr();
            }

            return model->bodyCenteredFrames[parent_body_id];
        }

        return RobotDynamics::ReferenceFramePtr();
    }
}

bool changePoseFrame(rdl_msgs::ChangePoseFrameRequest& req, rdl_msgs::ChangePoseFrameResponse& resp)
{
    RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.pose_in.header.frame_id);
    if (old_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.pose_in.header.frame_id;
        resp.success = false;
        return true;
    }

    RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame);
    if (new_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.new_reference_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::Math::Quaternion orientation(req.pose_in.pose.orientation.x, req.pose_in.pose.orientation.y, req.pose_in.pose.orientation.z,
                                                req.pose_in.pose.orientation.w);
    RobotDynamics::Math::FrameOrientation quat(old_frame, orientation);
    RobotDynamics::Math::FramePoint p(old_frame, req.pose_in.pose.position.x, req.pose_in.pose.position.y, req.pose_in.pose.position.z);

    quat.changeFrame(new_frame);
    p.changeFrame(new_frame);

    resp.pose_out.header.frame_id = new_frame->getName();
    resp.pose_out.pose.orientation.x = quat.x();
    resp.pose_out.pose.orientation.y = quat.y();
    resp.pose_out.pose.orientation.z = quat.z();
    resp.pose_out.pose.orientation.w = quat.w();
    resp.pose_out.pose.position.x = p.x();
    resp.pose_out.pose.position.y = p.y();
    resp.pose_out.pose.position.z = p.z();
    resp.success = true;

    return true;
}

bool changeOrientationFrame(rdl_msgs::ChangeOrientationFrameRequest& req, rdl_msgs::ChangeOrientationFrameResponse& resp)
{
    RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.orientation_in.header.frame_id);
    if (old_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.orientation_in.header.frame_id;
        resp.success = false;
        return true;
    }

    RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame);
    if (new_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.new_reference_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::Math::FrameOrientation quat(old_frame, req.orientation_in.quaternion.x, req.orientation_in.quaternion.y, req.orientation_in.quaternion.z,
                                               req.orientation_in.quaternion.w);

    quat.changeFrame(new_frame);

    resp.orientation_out.header.frame_id = new_frame->getName();
    resp.orientation_out.quaternion.x = quat.x();
    resp.orientation_out.quaternion.y = quat.y();
    resp.orientation_out.quaternion.z = quat.z();
    resp.orientation_out.quaternion.w = quat.w();
    resp.success = true;

    return true;
}

bool changePointFrame(rdl_msgs::ChangePointFrameRequest& req, rdl_msgs::ChangePointFrameResponse& resp)
{
    RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.point_in.header.frame_id);
    if (old_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.point_in.header.frame_id;
        resp.success = false;
        return true;
    }

    RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame);
    if (new_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.new_reference_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::Math::FramePoint p(old_frame, req.point_in.point.x, req.point_in.point.y, req.point_in.point.z);

    p.changeFrame(new_frame);

    resp.point_out.header.frame_id = new_frame->getName();
    resp.point_out.point.x = p.x();
    resp.point_out.point.y = p.y();
    resp.point_out.point.z = p.z();
    resp.success = true;

    return true;
}

bool changePointArrayFrame(rdl_msgs::ChangePointArrayFrameRequest& req, rdl_msgs::ChangePointArrayFrameResponse& resp)
{
    if (req.point_array_in.size() != req.new_reference_frame.size())
    {
        resp.success = false;
        resp.report = "Size of point array doesn't match size of new reference frame array";
        return true;
    }

    geometry_msgs::PointStamped point;
    for (unsigned int i = 0; i < req.point_array_in.size(); i++)
    {
        RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.point_array_in[i].header.frame_id);
        if (old_frame == nullptr)
        {
            resp.report = "No reference frame found with name " + req.point_array_in[i].header.frame_id;
            resp.success = false;
            return true;
        }

        RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame[i]);
        if (new_frame == nullptr)
        {
            resp.report = "No reference frame found with name " + req.new_reference_frame[i];
            resp.success = false;
            return true;
        }

        RobotDynamics::Math::FramePoint p(old_frame, req.point_array_in[i].point.x, req.point_array_in[i].point.y, req.point_array_in[i].point.z);

        p.changeFrame(new_frame);

        point.header.frame_id = new_frame->getName();
        point.point.x = p.x();
        point.point.y = p.y();
        point.point.z = p.z();

        resp.point_array_out.push_back(point);
    }

    resp.success = true;

    return true;
}

bool change3DVectorFrame(rdl_msgs::Change3DVectorFrameRequest& req, rdl_msgs::Change3DVectorFrameResponse& resp)
{
    RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.vector_in.header.frame_id);
    if (old_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.vector_in.header.frame_id;
        resp.success = false;
        return true;
    }

    RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame);
    if (new_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.new_reference_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::Math::FrameVector v(old_frame, req.vector_in.vector.x, req.vector_in.vector.y, req.vector_in.vector.z);

    v.changeFrame(new_frame);

    resp.vector_out.header.frame_id = new_frame->getName();
    resp.vector_out.vector.x = v.x();
    resp.vector_out.vector.y = v.y();
    resp.vector_out.vector.z = v.z();
    resp.success = true;

    return true;
}

bool change3DVectorArrayFrame(rdl_msgs::Change3DVectorArrayFrameRequest& req, rdl_msgs::Change3DVectorArrayFrameResponse& resp)
{
    geometry_msgs::Vector3Stamped vector;
    for (unsigned int i = 0; i < req.vector_array_in.size(); i++)
    {
        RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.vector_array_in[i].header.frame_id);
        if (old_frame == nullptr)
        {
            resp.report = "No reference frame found with name " + req.vector_array_in[i].header.frame_id;
            resp.success = false;
            return true;
        }

        RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame[i]);
        if (new_frame == nullptr)
        {
            resp.report = "No reference frame found with name " + req.new_reference_frame[i];
            resp.success = false;
            return true;
        }

        RobotDynamics::Math::FrameVector v(old_frame, req.vector_array_in[i].vector.x, req.vector_array_in[i].vector.y, req.vector_array_in[i].vector.z);

        v.changeFrame(new_frame);

        vector.header.frame_id = new_frame->getName();
        vector.vector.x = v.x();
        vector.vector.y = v.y();
        vector.vector.z = v.z();

        resp.vector_array_out.push_back(vector);
    }

    resp.success = true;

    return true;
}

bool changeWrenchFrame(rdl_msgs::ChangeWrenchFrameRequest& req, rdl_msgs::ChangeWrenchFrameResponse& resp)
{
    RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.wrench_in.header.frame_id);

    if (old_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.wrench_in.header.frame_id;
        resp.success = false;
        return true;
    }

    RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame);
    if (new_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.new_reference_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::Math::SpatialForce force(old_frame, req.wrench_in.wrench.torque.x, req.wrench_in.wrench.torque.y, req.wrench_in.wrench.torque.z,
                                            req.wrench_in.wrench.force.x, req.wrench_in.wrench.force.y, req.wrench_in.wrench.force.z);

    force.changeFrame(new_frame);

    resp.wrench_out.header.frame_id = new_frame->getName();
    resp.wrench_out.wrench.torque.x = force.mx();
    resp.wrench_out.wrench.torque.y = force.my();
    resp.wrench_out.wrench.torque.z = force.mz();
    resp.wrench_out.wrench.force.x = force.fx();
    resp.wrench_out.wrench.force.y = force.fy();
    resp.wrench_out.wrench.force.z = force.fz();
    resp.success = true;

    return true;
}

bool changeWrenchArrayFrame(rdl_msgs::ChangeWrenchArrayFrameRequest& req, rdl_msgs::ChangeWrenchArrayFrameResponse& resp)
{
    if (req.wrench_array_in.size() != req.new_reference_frame.size())
    {
        resp.report = "Size of wrench array does not match size of new reference frame array";
        resp.success = false;
        return true;
    }

    geometry_msgs::WrenchStamped wrench;
    for (unsigned int i = 0; i < req.wrench_array_in.size(); i++)
    {
        RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.wrench_array_in[i].header.frame_id);

        if (old_frame == nullptr)
        {
            resp.report = "No reference frame found with name " + req.wrench_array_in[i].header.frame_id;
            resp.success = false;
            return true;
        }

        RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame[i]);
        if (new_frame == nullptr)
        {
            resp.report = "No reference frame found with name " + req.new_reference_frame[i];
            resp.success = false;
            return true;
        }

        RobotDynamics::Math::SpatialForce force(old_frame, req.wrench_array_in[i].wrench.torque.x, req.wrench_array_in[i].wrench.torque.y,
                                                req.wrench_array_in[i].wrench.torque.z, req.wrench_array_in[i].wrench.force.x, req.wrench_array_in[i].wrench.force.y,
                                                req.wrench_array_in[i].wrench.force.z);

        force.changeFrame(new_frame);

        wrench.header.frame_id = new_frame->getName();
        wrench.wrench.torque.x = force.mx();
        wrench.wrench.torque.y = force.my();
        wrench.wrench.torque.z = force.mz();
        wrench.wrench.force.x = force.fx();
        wrench.wrench.force.y = force.fy();
        wrench.wrench.force.z = force.fz();
        resp.wrench_array_out.push_back(wrench);
    }

    resp.success = true;
    return true;
}

bool changeTwistFrame(rdl_msgs::ChangeTwistFrameRequest& req, rdl_msgs::ChangeTwistFrameResponse& resp)
{
    RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.twist_in.header.frame_id);
    if (old_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.twist_in.header.frame_id;
        resp.success = false;
        return true;
    }

    RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame);
    if (new_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.new_reference_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::Math::MotionVector twist(req.twist_in.twist.angular.x, req.twist_in.twist.angular.y, req.twist_in.twist.angular.z, req.twist_in.twist.linear.x,
                                            req.twist_in.twist.linear.y, req.twist_in.twist.linear.z);

    RobotDynamics::Math::SpatialTransform X = old_frame->getTransformToDesiredFrame(new_frame);
    twist.transform(X);

    resp.twist_out.header.frame_id = new_frame->getName();
    resp.twist_out.twist.angular.x = twist.wx();
    resp.twist_out.twist.angular.y = twist.wy();
    resp.twist_out.twist.angular.z = twist.wz();
    resp.twist_out.twist.linear.x = twist.vx();
    resp.twist_out.twist.linear.y = twist.vy();
    resp.twist_out.twist.linear.z = twist.vz();
    resp.success = true;

    return true;
}

bool changeTwistArrayFrame(rdl_msgs::ChangeTwistArrayFrameRequest& req, rdl_msgs::ChangeTwistArrayFrameResponse& resp)
{
    if (req.twist_array_in.size() != req.new_reference_frame.size())
    {
        resp.report = "Size of twist array does not match size of new reference frame array";
        resp.success = false;
        return true;
    }

    geometry_msgs::TwistStamped twist_msg;
    for (unsigned int i = 0; i < req.twist_array_in.size(); i++)
    {
        RobotDynamics::ReferenceFramePtr old_frame = getFrame(req.twist_array_in[i].header.frame_id);
        if (old_frame == nullptr)
        {
            resp.report = "No reference frame found with name " + req.twist_array_in[i].header.frame_id;
            resp.success = false;
            return true;
        }

        RobotDynamics::ReferenceFramePtr new_frame = getFrame(req.new_reference_frame[i]);
        if (new_frame == nullptr)
        {
            resp.report = "No reference frame found with name " + req.new_reference_frame[i];
            resp.success = false;
            return true;
        }

        RobotDynamics::Math::MotionVector twist(req.twist_array_in[i].twist.angular.x, req.twist_array_in[i].twist.angular.y, req.twist_array_in[i].twist.angular.z,
                                                req.twist_array_in[i].twist.linear.x, req.twist_array_in[i].twist.linear.y, req.twist_array_in[i].twist.linear.z);

        RobotDynamics::Math::SpatialTransform X = old_frame->getTransformToDesiredFrame(new_frame);
        twist.transform(X);

        twist_msg.header.frame_id = new_frame->getName();
        twist_msg.twist.angular.x = twist.wx();
        twist_msg.twist.angular.y = twist.wy();
        twist_msg.twist.angular.z = twist.wz();
        twist_msg.twist.linear.x = twist.vx();
        twist_msg.twist.linear.y = twist.vy();
        twist_msg.twist.linear.z = twist.vz();
        resp.twist_array_out.push_back(twist_msg);
    }

    resp.success = true;

    return true;
}

bool getTwist(rdl_msgs::GetTwistRequest& req, rdl_msgs::GetTwistResponse& resp)
{
    RobotDynamics::ReferenceFramePtr body_frame = getFrame(req.body_frame);
    if (body_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.body_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::ReferenceFramePtr base_frame = getFrame(req.base_frame);
    if (base_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.base_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::ReferenceFramePtr expressed_in_frame = getFrame(req.expressed_in_frame);
    if (expressed_in_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.expressed_in_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::Math::SpatialMotion m = RobotDynamics::calcSpatialVelocity(*model, q, qdot, body_frame, base_frame, expressed_in_frame, false);
    resp.success = true;
    resp.twist_out.header.frame_id = expressed_in_frame->getName();
    resp.twist_out.twist.angular.x = m.wx();
    resp.twist_out.twist.angular.y = m.wy();
    resp.twist_out.twist.angular.z = m.wz();
    resp.twist_out.twist.linear.x = m.vx();
    resp.twist_out.twist.linear.y = m.vy();
    resp.twist_out.twist.linear.z = m.vz();

    return true;
}

bool getTransform(rdl_msgs::GetTransformRequest& req, rdl_msgs::GetTransformResponse& resp)
{
    RobotDynamics::ReferenceFramePtr body_frame = getFrame(req.from_reference_frame);

    if (body_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.from_reference_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::ReferenceFramePtr base_frame = getFrame(req.to_reference_frame);
    if (base_frame == nullptr)
    {
        resp.report = "No reference frame found with name " + req.to_reference_frame;
        resp.success = false;
        return true;
    }

    RobotDynamics::Math::SpatialTransform X = body_frame->getTransformToDesiredFrame(base_frame);
    RobotDynamics::Math::Quaternion orientation(RobotDynamics::Math::toQuaternion(X.E));

    resp.success = true;
    resp.transform.header.frame_id = body_frame->getName();
    resp.transform.child_frame_id = base_frame->getName();
    resp.transform.transform.rotation.x = orientation.x();
    resp.transform.transform.rotation.y = orientation.y();
    resp.transform.transform.rotation.z = orientation.z();
    resp.transform.transform.rotation.w = orientation.w();

    resp.transform.transform.translation.x = X.r.x();
    resp.transform.transform.translation.y = X.r.y();
    resp.transform.transform.translation.z = X.r.z();

    return true;
}

bool getRobotCenterOfMass(rdl_msgs::GetRobotCenterOfMassRequest& req, rdl_msgs::GetRobotCenterOfMassResponse& resp)
{
    RobotDynamics::Math::FramePoint p_com;
    RobotDynamics::Math::FrameVector v_com;
    RobotDynamics::Utils::calcCenterOfMass(*model, q, qdot, p_com, &v_com, false);

    resp.success = true;
    resp.com.header.frame_id = p_com.getReferenceFrame()->getName();
    resp.com.point.x = p_com.x();
    resp.com.point.y = p_com.y();
    resp.com.point.z = p_com.z();

    resp.com_vel.header.frame_id = v_com.getReferenceFrame()->getName();
    resp.com_vel.vector.x = v_com.x();
    resp.com_vel.vector.y = v_com.y();
    resp.com_vel.vector.z = v_com.z();

    return true;
}

bool getBodyGravityWrench(rdl_msgs::GetBodyGravityWrenchRequest& req, rdl_msgs::GetBodyGravityWrenchResponse& resp)
{
    unsigned int body_id = model->GetBodyId(req.body.c_str());

    if (body_id == std::numeric_limits<unsigned int>::max())
    {
        resp.report = "No body found with name " + req.body;
        resp.success = false;
        return true;
    }

    RobotDynamics::Math::SpatialForce wrench;
    RobotDynamics::calcBodyGravityWrench(*model, body_id, wrench);

    resp.success = true;
    resp.wrench.header.frame_id = wrench.getReferenceFrame()->getName();
    resp.wrench.wrench.torque.x = wrench.mx();
    resp.wrench.wrench.torque.y = wrench.my();
    resp.wrench.wrench.torque.z = wrench.mz();

    resp.wrench.wrench.force.x = wrench.fx();
    resp.wrench.wrench.force.y = wrench.fy();
    resp.wrench.wrench.force.z = wrench.fz();

    return true;
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "rdl_kinematics_interface");
    ros::NodeHandle nh("~");
    std::string joint_states_topic;
    nh.param<std::string>("joint_states_topic", joint_states_topic, "joint_states");

    std::string urdf_string;
    if (!nh.getParam("robot_description", urdf_string))
    {
        ROS_ERROR_STREAM("kinematics_interface: Unable to retrieve urdf from param server at " << nh.getNamespace() << "/robot_description");
        return 1;
    }

    model.reset(new RobotDynamics::Model());
    if (!RobotDynamics::Urdf::urdfReadFromString(urdf_string, model))
    {
        return 1;
    }

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

    if (!RobotDynamics::Urdf::parseJointBodyNameMapFromString(urdf_string, joint_to_body_name_map))
    {
        return 1;
    }

    for (std::pair<std::string, std::string> pair : joint_to_body_name_map)
    {
        try
        {
            joint_name_q_index[pair.first] = model->mJoints[model->GetBodyId(pair.second.c_str())].q_index;
        }
        catch (const std::out_of_range& e)
        {
            ROS_ERROR_STREAM("Error determining q_index for " << pair.first << ": " << e.what());
            return 1;
        }
    }

    ros::Subscriber joint_states_subscriber;
    ros::Subscriber robot_state_subscriber;

    robot_state_subscriber = nh.subscribe("robot_state", 0, robotStateCallback);

    ros::ServiceServer changePointFrameSrv = nh.advertiseService("change_frame_point", changePointFrame);
    ros::ServiceServer changePoseFrameSrv = nh.advertiseService("change_frame_pose", changePoseFrame);
    ros::ServiceServer changeOrientationFrameSrv = nh.advertiseService("change_frame_orientation", changeOrientationFrame);
    ros::ServiceServer changePointArrayFrameSrv = nh.advertiseService("change_frame_point_array", changePointArrayFrame);
    ros::ServiceServer change3DVectorFrameSrv = nh.advertiseService("change_frame_3dvector", change3DVectorFrame);
    ros::ServiceServer change3DVectorArrayFrameSrv = nh.advertiseService("change_frame_3dvector_array", change3DVectorArrayFrame);
    ros::ServiceServer changeWrenchFrameSrv = nh.advertiseService("change_frame_wrench", changeWrenchFrame);
    ros::ServiceServer changeWrenchArrayFrameSrv = nh.advertiseService("change_frame_wrench_array", changeWrenchArrayFrame);
    ros::ServiceServer changeTwistFrameSrv = nh.advertiseService("change_frame_twist", changeTwistFrame);
    ros::ServiceServer changeTwistArrayFrameSrv = nh.advertiseService("change_frame_twist_array", changeTwistArrayFrame);
    ros::ServiceServer getTwistSrv = nh.advertiseService("get_twist", getTwist);
    ros::ServiceServer getTransformSrv = nh.advertiseService("get_transform", getTransform);
    ros::ServiceServer getRobotCenterOfMassSrv = nh.advertiseService("get_robot_com", getRobotCenterOfMass);
    ros::ServiceServer getBodyGravityWrenchSrv = nh.advertiseService("get_body_grav_wrench", getBodyGravityWrench);

    while (ros::ok())
    {
        ros::spin();
    }

    return 0;
}