Baxter Robot get_current_position() of MoveGroupCommander is wrong

asked 2014-05-15 07:09:01 -0500

lwi gravatar image

Hi,

I am currently working with a Baxter robot and the Python MoveGroupCommander. It seems to me that the get_current_pose() method gives me false results.

I runned the following example program and the pose I got from get_current_pose() is significant different from the one I get over the ROS topic /robot/limb/right/endpoint_state.

Am I missing something? Or is this a bug?

Program:

def main():
rospy.init_node("move_test")
rightHand = MoveGroupCommander("right_arm")

rightHand.set_goal_position_tolerance(0.02)
rightHand.set_goal_orientation_tolerance(0.03)
rightHand.allow_replanning(True)

quaternion = tf.transformations.quaternion_from_euler(0, math.pi, 0.0)

poseStamped = PoseStamped()
poseStamped.header.frame_id = "base"
poseStamped.pose.position.x =  0.45
poseStamped.pose.position.y =  -0.46
poseStamped.pose.position.z =  0.168
poseStamped.pose.orientation.x = quaternion[0]
poseStamped.pose.orientation.y = quaternion[1]
poseStamped.pose.orientation.z = quaternion[2]
poseStamped.pose.orientation.w = quaternion[3]

rightHand.set_pose_target(poseStamped)
rightHand.go()

print (rightHand.get_current_pose())
print (rightHand.get_current_pose(rightHand.get_end_effector_link()))

Output Program:

header: 
  seq: 0
  stamp: 
    secs: 1400173189
    nsecs: 552232980
  frame_id: /base
pose: 
  position: 
    x: 0.813394209535
    y: -1.01086754781
    z: 0.314464719758
  orientation: 
    x: 0.271248387605
    y: 0.651768157495
    z: -0.270353659959
    w: 0.654623158499
header: 
  seq: 0
  stamp: 
    secs: 1400173190
    nsecs: 554366111
  frame_id: /base
pose: 
  position: 
    x: 0.813394209535
    y: -1.01086754781
    z: 0.314464719758
  orientation: 
    x: 0.271248387605
    y: 0.651768157495
    z: -0.270353659959
    w: 0.654623158499

Output rostopic echo /robot/limb/right/endpoint_state:

 position: 
    x: 0.451398551432
    y: -0.462045965561
    z: 0.159395117373
  orientation: 
    x: 0.0156779318377
    y: 0.999591289499
    z: 0.0230686023968
    w: 0.00626865171373
edit retag flag offensive close merge delete

Comments

Are these both in the same coordinate frame?

ahendrix gravatar image ahendrix  ( 2014-05-15 07:15:28 -0500 )edit

They should be. The coordinates supplied for the movement are relative to /base. After the movement the endpoint_state shows pretty much these coordinates so the x=0.45 coordinates are fine. The get_current_pose() coordinates have also the frame_id /base. Therefore they seem to be in the same frame

lwi gravatar image lwi  ( 2014-05-15 07:47:14 -0500 )edit

I'm not very familiar with MoveIt. Is it possible that go() is asynchronous, and get_current_pose() is executing before the robot moves much, returning a position near the start pose?

ahendrix gravatar image ahendrix  ( 2014-05-16 06:07:00 -0500 )edit

Yea I've faced this problem too. Moreover in my case rightHand.get_current_pose() would give the same value irrespective of the actual pose. So in y implementations I use the rethink robotics sdk to get the end point pose. Works decently well

Lerrel gravatar image Lerrel  ( 2014-11-07 17:39:58 -0500 )edit