# Computing forward kinematics in Cartesian Coordinates

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_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()
{

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

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();

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;
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">
<visual>
<origin xyz="0 0 0.045" />
<geometry>
</geometry>
</visual>
<visual>
<origin xyz="0 0 0.04" />
<geometry>
</geometry>
</visual>
<visual>
<origin xyz="0.0655 0 0" />
<geometry>
<box size="0.131 0.02 0.035" />
</geometry>
</visual>
<visual>
<origin xyz="0.0685 0 0" />
<geometry>
<box size="0.137 0.02 0.025" />
</geometry>
</visual>
<visual>
<origin xyz="0.025 0 0" rpy="0 1.57 0" />
<geometry>
</geometry>
</visual>

<joint name="waist" type="continuous" >
<origin xyz="0 0 0.09" rpy="0 0 0" />
<axis xyz="0 0 1" />
</joint>

<joint name="shoulder" type="continuous" >
<origin xyz="0 0 0.04" rpy="0 0 0" />
<axis xyz="0 -1 0" />
</joint>

<joint name="elbow" type="continuous" >
<origin xyz="0.131 0 0" rpy="0 1.5 0" />
<axis xyz="0 -1 0" />
</joint>

<joint name="wrist1" type="continuous" >
<origin xyz="0.137 0 0" />
<axis xyz="1 0 0" />
</joint>

<joint name="wrist2" type="continuous" >
<origin xyz="0.05 0 0" rpy="0 -0.5 0" />
<axis xyz="0 -1 0" />
</joint>

<joint name="wrist3" type="continuous" >
<origin xyz="0.01 0 0" />
<axis xyz="1 0 0" />
</joint>
</robot>


Any help appreciated.

Thanks, Bart

Originally posted by bjem85 on ROS Answers with karma: 163 on 2014-10-01

Post score: 2

Hello, I haven't used MoveIt! much, but I think the information you are trying to gain from your robot is usually accessed using the joint_state_publisher and the robot_state_publisher.

joint_state_publisher looks at the URDF to find all the non-fixed joints on the robot and publishes a sensor_msgs/JointState message. It can get the joint info from different sources such as a GUI or Gazebo, etc.

The robot_state_publisher takes the information from joint_state_publisher and the URDF, then publishes a TF for each joint. Once you have a TF for each joint, it is easy peezy to get the transform from the waist joint to the wrist1 joint in Cartesian coordinate, then you just have to convert from Cartesian to cylindrical.

Originally posted by Airuno2L with karma: 3460 on 2014-10-03

This answer was ACCEPTED on the original site

Post score: 1

I've done that before, though I found it kludge-y putting in a robot_state_publisher then polling the /tf topic and was looking for an improvement. I will look at the kdl kinematics libraries.
The robot_state_publisher uses the kdl library. You don't have to use the node, you can use their library directly
It's not that I don't like robot_state_publisher, it is that my robotic system is very small and simple (3 joints) and I just want a shim node to convert from one coordinate system to another.