Ask Your Question
3

Best way to get FK for many different joint-states

asked 2016-12-15 10:32:16 -0600

Alex_Nitsch gravatar image

updated 2016-12-19 02:55:55 -0600

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

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
3

answered 2016-12-15 20:24:41 -0600

JoshMarino gravatar image

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
edit flag offensive delete link more

Comments

pykdl_kinematics looks very promising! Thanks :) IKFast would be faster, but I don't really know how to communicate with it at a high rate without running into the issue with ros topics mentioned above. Or what would I have to do to actually get from my joint states and URDF to the pose with IKFast?

Alex_Nitsch gravatar imageAlex_Nitsch ( 2016-12-16 03:35:37 -0600 )edit

You first have to create an IKFast solution from your URDF, then you can load it into a C++ file and have direct access to it, or create a ROS service.

JoshMarino gravatar imageJoshMarino ( 2016-12-17 08:37:06 -0600 )edit

Is there a good way to access it via python? Using a service would probably mean a lot of overhead again...

Alex_Nitsch gravatar imageAlex_Nitsch ( 2016-12-19 02:57:34 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2016-12-15 10:30:06 -0600

Seen: 601 times

Last updated: Dec 19 '16