Best way to get FK for many different joint-states

edit

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 Header
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:

    next_joint_state.header.stamp = rospy.Time.now()
    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