ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# 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>
<cylinder radius="0.06" length="0.09 ...
edit retag close merge delete

Sort by ยป oldest newest most voted

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.

more

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.

( 2014-10-03 20:04:56 -0500 )edit

The robot_state_publisher uses the kdl library. You don't have to use the node, you can use their library directly

( 2014-10-04 08:33:09 -0500 )edit

I'm assuming you don't like robot_state_publisher because it continuously publishes, with the library you can publish only when you want.

( 2014-10-04 08:34:34 -0500 )edit

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.

( 2014-10-04 19:48:40 -0500 )edit

I ended up using the kdl kinematics libraries, although in all honesty there was barely any benefit in time or programming ease over using the transform lookup tools as Airuno2L suggested.

more