0
$\begingroup$

Rosanswers logo

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" />
      </geometry>
    </visual>
  </link>
  <link name="torso" >
    <visual>
      <origin xyz="0 0 0.04" />
      <geometry>
    <sphere radius=".06" />
      </geometry>
    </visual>
  </link>
  <link name="upper_arm" >
    <visual>
      <origin xyz="0.0655 0 0" />
      <geometry>
    <box size="0.131 0.02 0.035" />
      </geometry>
    </visual>
  </link>
  <link name="lower_arm" >
    <visual>
      <origin xyz="0.0685 0 0" />
      <geometry>
    <box size="0.137 0.02 0.025" />
      </geometry>
    </visual>
  </link>
  <link name="wrist1" />
  <link name="wrist2" />
  <link name="stylus" >
    <visual>
      <origin xyz="0.025 0 0" rpy="0 1.57 0" />
      <geometry>
    <cylinder radius="0.01" length="0.135" />
      </geometry>
    </visual>
  </link>

  <joint name="waist" type="continuous" >
    <parent link="base" />
    <child link="torso" />
    <origin xyz="0 0 0.09" rpy="0 0 0" />
    <axis xyz="0 0 1" />
  </joint>

  <joint name="shoulder" type="continuous" >
    <parent link="torso" />
    <child link="upper_arm" />
    <origin xyz="0 0 0.04" rpy="0 0 0" />
    <axis xyz="0 -1 0" />
  </joint>

  <joint name="elbow" type="continuous" >
    <parent link="upper_arm" />
    <child link="lower_arm" />
    <origin xyz="0.131 0 0" rpy="0 1.5 0" />
    <axis xyz="0 -1 0" />
  </joint>
  
  <joint name="wrist1" type="continuous" >
    <parent link="lower_arm" />
    <child link="wrist1" />
    <origin xyz="0.137 0 0" />
    <axis xyz="1 0 0" />
  </joint>
  
  <joint name="wrist2" type="continuous" >
    <parent link="wrist1" />
    <child link="wrist2" />
    <origin xyz="0.05 0 0" rpy="0 -0.5 0" />
    <axis xyz="0 -1 0" />
  </joint>
  
  <joint name="wrist3" type="continuous" >
    <parent link="wrist2" />
    <child link="stylus" />
    <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

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Original comments

Comment by bjem85 on 2014-10-03:
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.

Comment by Airuno2L on 2014-10-04:
The robot_state_publisher uses the kdl library. You don't have to use the node, you can use their library directly

Comment by Airuno2L on 2014-10-04:
I'm assuming you don't like robot_state_publisher because it continuously publishes, with the library you can publish only when you want.

Comment by bjem85 on 2014-10-04:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.