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_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 ...