Ask Your Question
2

Computing forward kinematics in Cartesian Coordinates

asked 2014-10-01 02:08:26 -0600

bjem85 gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2014-10-03 07:16:25 -0600

Airuno2L gravatar image

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.

edit flag offensive delete link more

Comments

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.

bjem85 gravatar image bjem85  ( 2014-10-03 20:04:56 -0600 )edit

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

Airuno2L gravatar image Airuno2L  ( 2014-10-04 08:33:09 -0600 )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.

Airuno2L gravatar image Airuno2L  ( 2014-10-04 08:34:34 -0600 )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.

bjem85 gravatar image bjem85  ( 2014-10-04 19:48:40 -0600 )edit
0

answered 2014-10-06 16:36:32 -0600

bjem85 gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-10-01 02:08:26 -0600

Seen: 3,060 times

Last updated: Oct 06 '14