Baxter Robot get_current_position() of MoveGroupCommander is wrong
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
Are these both in the same coordinate frame?
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
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?
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