Rosanswers logo

I am trying to find a method for calculating cartesian positions of specific joint angles. My goal is to be able to publish joint states and get cartesian positions back for each set of joint states, without running a robot simulation. I found this question: http://answers.ros.org/question/193916/computing-forward-kinematics-in-cartesian-coordinates/ After looking into the joint_state_publisher and robot_state_publisher nodes and the /tf topic, I have come up with a somewhat convoluted method of achieving a forward kinematics solver.

1. My node publishes joint states for the robot, read from a text file
2. robot_state_publisher receives joint states and publishes transforms
3. My node receives transforms, calculates base to tip transform (lookupTransform), and outputs a cartesian position

Then I found this: http://docs.ros.org/hydro/api/pr2_moveit_tutorials/html/kinematics/src/doc/kinematics_tutorial.html#forward-kinematics This page briefly mentions the use of a FK/IK solution which seems promising, but it looks like it's using a robot simulation as the joint states input. Is it possible to use this to retrieve xyz positions from joint angles that are read from a file instead? Note that I do not expect to find a solution that will read my file and use the information, I have a file parser node that will handle this and format the data in whatever message type is necessary for the interacting nodes.

Originally posted by Robot_Cpak on ROS Answers with karma: 71 on 2016-10-28

Post score: 2

Original comments

Comment by lucasw on 2016-10-28:
When you say 'calculates base to tip transform', are you doing that manually or using tf lookupTransform(base, tip,...) to do it in one call?

Comment by Robot_Cpak on 2016-10-28:
I'm using lookupTransform. Added that to the above. Thanks!

Comment by JoshMarino on 2016-10-29:
Do you have access to URDF? If so, you can create a robot model from that and then use FK to compute it using something like this.

Comment by gvdhoorn on 2016-10-29:
Pedantic maybe, but MoveIt! does not run "a robot simulation" at all. A MoveIt configuration pkg just provides a bunch of configuration files for move_group, which enables it to plan trajectories for a kinematic structure. There is no simulation anywhere.

Comment by gvdhoorn on 2016-10-29:
If you don't want to use services, you could use the underlying API directly. See Kinematic Model Tutorial for a tutorial. No join states or robot states needed.

Comment by Robot_Cpak on 2016-11-01:
Thank you all for your help! With your suggestions, I was able to come up with a solution for my problem, posted below. Thanks again!


2 Answers 2


Rosanswers logo

Based on that configuration, move_group then exposes the GetPositionFK and GetPositionIK services, which would seem to do what you are after.

Originally posted by gvdhoorn with karma: 86574 on 2016-10-29

This answer was NOT ACCEPTED on the original site

Post score: 1


Rosanswers logo

Starting with this tutorial, I wrote a standalone c++ app that calculates forward kinematics for my robot.

The following is a snippet that includes the nearby relevant code from the tutorial with my changes in the middle.

// straight from the tutorial
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

// this part is different - input any joint values and receive cartesian position as output
double current_pos[] = {0,0,0,0,0,0};
ROS_INFO("Joint pos = [ %f, %f, %f, %f, %f, %f ]", current_pos[0], current_pos[1], current_pos[2], current_pos[3], current_pos[4], current_pos[5] );

// straight from the tutorial
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("link_6");
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());

Thank you all for your help!

Originally posted by Robot_Cpak with karma: 71 on 2016-11-01

This answer was ACCEPTED on the original site

Post score: 3


Your Answer

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