We are doing camera and ur5 integration (As discussed in this topic) .
we are using following transformation method to pass camera coordinate to robot base coordinate.
Following codes are used to transform camera coordinate to base coordinate, here point1 is the object coordinates from camera
def callback(point1):
print("camera_coordinate",point1)
result_1 = []
trans = 0
rot = 0
listener = tf.TransformListener()
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('/tool0', '/camera_link', rospy.Time(0))
break
except :
continue
matrix2=np.dot(translation_matrix(trans), quaternion_matrix(rot))
point1[0][0] = -point1[0][0]
point1[1][0] = -point1[1][0]
point1[2][0] = point1[2][0]
result_1 = np.matmul(np.array(matrix2),point1)
result_1[0][0]=-result_1[0][0]
result_1[1][0]=-result_1[1][0]
print("object coordinates",result_1)
move_to_pos_1(result_1)
return result_1
And the robot move function to feed coordinate to robot
def move_to_pos(Goal_x,Goal_y,Goal_z):
""" This function shows an example of how to move the UR5 to a position with MoveIt"""
pose_goal = group.get_current_pose().pose
print("Current location:")
print(pose_goal)
pose_goal.position.x = Goal_x
pose_goal.position.y = Goal_y
pose_goal.position.z = Goal_z
pose_goal.orientation.x = 0.923274429142
pose_goal.orientation.y = -0.383710458579
pose_goal.orientation.z = -0.00432136844868
pose_goal.orientation.w = 0.0176617735317
print("Pose Goal:")
print(pose_goal)
group.set_pose_target(pose_goal)
plan= group.plan()
rospy.sleep(5)
group.go(wait=True)
group.clear_pose_targets()
The values from the camera for object detection is proper. The robot is not moving to exact object coordinate. for every object position the position error is varying.
Please let me know whether we are doing with right way