# Best way to get FK for many different joint-states

Hello!

I am trying to calculate the end-effector pose for a huge amount of different JointStates.

The two ingredients to do that are my URDF and the numpy array that contains the joint angles. I am currently using ros indigo + moveit on a regular (but fairly fast) notebook.

In my python script I use a joint_state_publisher to publish the JointState and the move_group API to get_current_pose(). Unfortunately this is very slow... If I don't wait long enough (around 0.1 s) between publishing and getting the pose, it will return the previous pose instead of using the new JointState. I am not sure if the problem is the JointState Message being delayed or if the call to the move_group and subsequently tf takes so long. My goal would be something between 1 kHz and a MHz.

My two questions are:

1. Is there an easy or elegant method to get the Pose only if the new one is there?
2. Is there a more direct way to calculate the cartesian position given the joint states? The package arm_kinematics looks promising but it's still from Fuerte and does not seem to be developed anymore.

Here is a simplified version of my code:

#!/usr/bin/env python

import sys
import rospy
import numpy
import copy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import sensor_msgs.msg
import matplotlib.pyplot as plt
from sensor_msgs.msg import JointState
from std_msgs.msg import String

moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("whole_arm")

pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher')

pose_list = []

##
#... joint_state_list is created and filled with JointState Objects
##

for next_joint_state in joint_state_list:

pub.publish(next_joint_state)
rospy.sleep(0.1)
current_pose = group.get_current_pose()
pose_list.append(copy.deepcopy(current_pose))


Thank you very much!

Edit:

I have now started with using the package urdfdom_py to obtain my robot in the python Code. As a next step I would like to convert that to a KDL:Tree, so I can use python_orocos_kdl for my forward kinematics request. Unfortunately I can't find any info on how to do that or how the python_orocos_kdl API is supposed to work. Any help would be greatly appreciated

Originally posted by Alex_Nitsch on ROS Answers with karma: 41 on 2016-12-15

Post score: 4

If you are just wanting the end effector position for a desired set of joint angles, you have a few different options.

1. FK service created by running move_group node
2. FK function call from KDL Kinematics using pykdl_kinematics and URDF (link)
3. IKFast solver if you have created one

Originally posted by JoshMarino with karma: 592 on 2016-12-15

This answer was ACCEPTED on the original site

Post score: 4