0
$\begingroup$

Rosanswers logo

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


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

Post score: 4

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Original comments

Comment by Alex_Nitsch on 2016-12-16:
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?

Comment by JoshMarino on 2016-12-17:
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.

Comment by Alex_Nitsch on 2016-12-19:
Is there a good way to access it via python? Using a service would probably mean a lot of overhead again...

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.