Computing forward kinematics in Cartesian Coordinates

edit

Hi All,

I am trying to compute the forward kinematics for a Phantom Omni haptic feedback arm.

I am following the MoveIt! kinematics API instructions, but the forward kinematics instructions seem to have been added as an afterthought. In particular, I do not wish to compute forward kinematics for random joint positions. The API documentation isn't very detailed and some of the functions don't seem documented at all.

My question is: given a URDF and an SRDF of the robot, and a set of joint angles, how do I use the forward kinematics API of MoveIt! to take a set of joint angles and compute an end effector position in Cartesian coordinates? What I really want is the transform from the waist joint to the wrist1 joint in cylindrical coordinates. At present it only seems to give direction sines and cosines. How do I fix this? I thought doing it this way would be cleaner than using /tf but am I wrong?

I can confirm the correct joint states are getting into the robot model.

/*
 * phantom_forward_kinematics.cpp
 *
 *  Created on: 30/09/2014
 *      Author: mech801
 */

// Forward kinematics for the Phantom Omni arm.
// BJEM 30th September 2014.

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/robot_state.h>
#include "phantom_omni/OmniFeedback.h"
#include <eigen3/Eigen/Geometry>



// Forward kinematics for the Phantom Omni.
class PhantomFK
{
public:
    PhantomFK(): nh_(ros::NodeHandle()) {}

    ~PhantomFK() {}

    bool init()
    {
        using namespace robot_model_loader;

        // Subscribers and publishers.
        joint_states_sub_ = nh_.subscribe("joint_states", 10, &PhantomFK::jointStatesCallBack, this);

        phantom_pub_ = nh_.advertise<phantom_omni::OmniFeedback>("omni_feedback", 10);
        robot_model_loader = RobotModelLoader("robot_description");
        kinematic_model = robot_model_loader.getModel();

        ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

        kinematic_state.reset(new robot_state::RobotState(kinematic_model));
        kinematic_state->setToDefaultValues();

        return true;
    }

    void jointStatesCallBack(const sensor_msgs::JointStateConstPtr &jsc)
    {
        js_ = *jsc;
        issuePhantomCommand();
    }

    void issuePhantomCommand()
    {
        std::map<std::string, double> joint_map;
        joint_map["waist"] = js_.position[0];
        joint_map["shoulder"] = js_.position[1];
        joint_map["elbow"] = js_.position[2];

        /* Compute FK for a set of random joint values*/
        kinematic_state->setVariablePositions(joint_map);
        kinematic_state->update();

        const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("upper_arm");

        phantom_omni::OmniFeedback pof;

        //      ROS_INFO_STREAM("   waist: " << kinematic_state->getVariablePosition("waist"));
        //      ROS_INFO_STREAM("shoulder: " << kinematic_state->getVariablePosition("shoulder"));
        //      ROS_INFO_STREAM("   elbow: " << kinematic_state->getVariablePosition("elbow"));
        //      ROS_INFO_STREAM("");

        //
//      ROS_INFO_STREAM("   waist: " << kinematic_state->getVariablePosition("waist"));
//      ROS_INFO_STREAM("shoulder: " << kinematic_state->getVariablePosition("shoulder"));
//      ROS_INFO_STREAM("   elbow: " << kinematic_state->getVariablePosition("elbow"));
//      ROS_INFO_STREAM("");

        pof.position.x = end_effector_state.data()[0];
        pof.position.y = end_effector_state.data()[1];
        pof.position.z = end_effector_state.data()[2];

        phantom_pub_.publish(pof);
    }


private:
    ros::NodeHandle nh_;
    ros::Subscriber joint_states_sub_;
    ros::Publisher phantom_pub_;

    sensor_msgs::JointState js_;

    moveit::core::RobotStatePtr kinematic_state;
    robot_model_loader::RobotModelLoader robot_model_loader;
    moveit::core::RobotModelPtr kinematic_model;
    const moveit::core::JointModelGroup *joint_model_group;
};

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "phantom_forward_kinematics");

    PhantomFK pfk;

    pfk.init();

    ros::spin();

    return 0;
}

Here is the URDF of the Phantom:

<?xml version="1.0" encoding="utf-8"?>
<robot
    xmlns="http://www.ros.org/wiki/urdf"
    xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
    xsi:schemaLocation=
    "https://raw.github.com/laas/urdf_validator/master/urdf.xsd"
    name="omni">
  <link name="base" >
    <visual>
      <origin xyz="0 0 0.045" />
      <geometry>
    <cylinder radius="0.06" length="0.09 ...