MoveGroupCommander get_current_pose() returns incorrect result when using real robot

asked 2018-08-20 00:23:24 -0500

fvd gravatar image

updated 2018-09-20 02:41:15 -0500

I want to perform a movement relative to the current pose of a UR robot, which I have prototyped in Python. When I am running the simulation and the physical robot, the same .urdf and .srdf files are loaded, so the scene known to MoveIt should be the same. However, when the real robot is connected, the output of the MoveGroupCommander function get_current_pose() is different from the simulation:

The code:

# ==== MISBEHAVING VERSION:
start_pos_mgc = group.get_current_pose() 
# ==== WORKING VERSION:
gripper_pos = geometry_msgs.msg.PoseStamped()
gripper_pos.header.frame_id = "a_bot_gripper_tip_link"
gripper_pos.pose.orientation.w = 1.0
start_pos_manual = self.listener.transformPose("world", gripper_pos)
# ====

rospy.loginfo("The EE link is: " + group.get_end_effector_link())
rospy.loginfo("The Planning frame is: " + group.get_planning_frame())
rospy.loginfo("Pose via MoveGroupCommander is: ")
rospy.loginfo(start_pos_mgc)
rospy.loginfo("Pose via manual TF is: ")
rospy.loginfo(start_pos_manual)

When running the demo.launch of the moveit_config package, the two poses are the same, but when using the real robot, the output is this:

[INFO] [1534740471.283544]: The EE link is: a_bot_gripper_tip_link
[INFO] [1534740471.283821]: The Planning frame is: /world
[INFO] [1534740471.284032]: Pose via MoveGroupCommander is: 
[INFO] [1534740471.284445]: header: 
  seq: 0
  stamp: 
    secs: 1534740471
    nsecs: 279306888
  frame_id: "/world"
pose: 
  position: 
    x: 0.814750000001
    y: -0.22355
    z: 0.742509000003
  orientation: 
    x: 0.499999999995
    y: 0.5
    z: 0.500000000002
    w: 0.500000000002
[INFO] [1534740471.285085]: Pose via manual TF is: 
[INFO] [1534740471.285504]: header: 
  seq: 0
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "world"
pose: 
  position: 
    x: -0.10676008707785575
    y: -0.47442804315315057
    z: 1.1395218025815932
  orientation: 
    x: -0.7067673098798285
    y: 0.007619800057650674
    z: 0.7073708035668745
    w: -0.006960933376064573

The pose returned by get_current_pose() does not change, even though the robot's position changes correctly and is displayed in rviz. The planning using rviz and in the rest of my code works fine, but I have not used get_current_pose() anywhere else before. I published markers for three calls to the code above (the three frames with green blobs near the robot are start_pos_manual, the one near the bottom of the screen is start_pos_mgc):

image description

I cannot find what the issue is. I am especially confused that all of the other planning works. I tried digging in the code, but I'm not sure where _moveit_move_group_interface is imported from (if it's a wrapped C++ file then I might need a hand). What could be the issue?

I am using this version of the ur_modern_driver on Kinetic, Ubuntu 16.04 and the binary version of MoveIt. Thanks in advance.

edit: I just noticed that start_pos_mgc seems to be where the end effector link would be with all of the robot joints at 0. There must be something going wrong with the joint states not being updated, or not being accessed correctly by get_current_pose(), but what could it be? I don't know why the planning works, but only get_current_pose() does not. For the record, the getCurrentPose() function in the C++ MoveGroupInterface class works fine.

edit2: I had another problem where the move group reported an incorrect joint position, and ... (more)

edit retag flag offensive close merge delete