forward kinematics in moveit commander

[Sudarshan Srinivasan]

Hello,

I was wondering if there is a function that I could call via moveit_commander.py which would perform forward kinematics taking in the joint angles for the planning group and return the end effector position. I am unable to find it in the documentation.

I am using ROS Groovy on Ubuntu 12.04.

Thanks,

Sudarshan.

[Chris Reardon]

Sudharshan and I have discussed this offline, but since no one appeared to know the answer to his question, I wanted to post an answer for others' benefit.

For fk for an arbitrary position, with moveit running, I noticed that a "compute_fk" service is created.

$ rosservice info /compute_fk
Node: /move_group
URI: rosrpc://meka:58491
Type: moveit_msgs/GetPositionFK
Args: header fk_link_names robot_state

I won't post the rossrv show here, but it looks like a modification of the arm_kinematics/GetPositionFK state, so I figured I could do something like this: http://wiki.ros.org/pr2_kinematics/Tutorials/Tutorial%203#Populating_the_GetPositionFK_service_call_request

Here is a test in python:

right_arm = MoveGroupCommander('right_arm')
rospy.wait_for_service('compute_fk')
try:
  moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
except rospy.ServiceException, e:
  rospy.logerror("Service call failed: %s"%e)
fkln = ['palm']
joint_names = []
joint_positions = []
for i in range(7):
  joint_names.append('right_arm_j'+str(i)) # your names may vary
  joint_positions.append(0.8) # try some arbitrary joint angle
header = Header(0,rospy.Time.now(),"/world")
rs = RobotState()
rs.joint_state.name = joint_names
rs.joint_state.position = joint_positions
rospy.loginfo(["FK LOOKUP:", moveit_fk(header, fkln, rs)]) # Lookup the pose
#### To test, execute joint_positions here ####
rospy.loginfo(["POST MOVE:", right_arm.get_current_pose()]) # Verify that the new pose matches your computed pose