Ask Your Question
1

[SOLVED] Forward Kinematics: Get cartesian position from joint angles [closed]

asked 2016-10-28 13:02:47 -0600

Robot_Cpak gravatar image

updated 2016-11-01 15:35:56 -0600

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/19391... 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_mov... 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.

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by Robot_Cpak
close date 2016-11-03 09:51:22.740689

Comments

1

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?

lucasw gravatar imagelucasw ( 2016-10-28 15:55:53 -0600 )edit

I'm using lookupTransform. Added that to the above. Thanks!

Robot_Cpak gravatar imageRobot_Cpak ( 2016-10-28 16:12:47 -0600 )edit

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.

JoshMarino gravatar imageJoshMarino ( 2016-10-29 00:30:00 -0600 )edit

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.

gvdhoorn gravatar imagegvdhoorn ( 2016-10-29 03:54:16 -0600 )edit

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

gvdhoorn gravatar imagegvdhoorn ( 2016-10-29 03:55:18 -0600 )edit

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.

gvdhoorn gravatar imagegvdhoorn ( 2016-10-29 03:57:21 -0600 )edit

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!

Robot_Cpak gravatar imageRobot_Cpak ( 2016-11-01 15:35:36 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2016-11-01 15:33:38 -0600

Robot_Cpak gravatar image

updated 2016-11-01 15:33:57 -0600

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] );
kinematic_state->setVariablePositions(current_pos);
kinematic_state->update();

// 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!

edit flag offensive delete link more

Question Tools

3 followers

Stats

Asked: 2016-10-28 13:02:47 -0600

Seen: 1,950 times

Last updated: Nov 01 '16