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):
I cannot find what the issue is. I am especially confused that all of the other planning works, and . I tried digging in [the code](https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_commander/src/moveit_commander/move_group.py), 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](https://github.com/Zagitta/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.
↧