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

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

Robot_Cpak
71 ●2 ●5 ●13

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

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 close merge delete

Comments

2

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 ( 2016-10-28 15:55:53 -0500 )edit

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

Robot_Cpak ( 2016-10-28 16:12:47 -0500 )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 ( 2016-10-29 00:30:00 -0500 )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 ( 2016-10-29 03:54:16 -0500 )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 ( 2016-10-29 03:57:21 -0500 )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 ( 2016-11-01 15:35:36 -0500 )edit

add a comment

3

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

Robot_Cpak
71 ●2 ●5 ●13

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

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!