UR5 vision base pick and place

Hello, we are implementing vision based pick and place of object using ROS. the camera we are using is realsense depth camera. we have done the camera hand eye calibration and using transformation matrix we are trying to send object coordinate to robot back.

the robot is not moving to particular object position, Please suggest us where we are going wrong.

the code is as bellow

thanking in advance

#!/usr/bin/env python

import os
import sys
import copy
import time
import numpy as numpy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from dh_gripper_msgs.msg import GripperState
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
#from gripper import Gripper
#from task_com_speach import Speach
#from shapedetection_5 import get_coordinates
import cv2
#from get_color_from_speach import get_color_from_speach
import pyrealsense2 as rs
from scipy.spatial.transform import Rotation as R
import numpy as np

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(‘move_group_python_interface_tutorial’,
anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
#group_name = “arm”
group_name = “manipulator”
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher(’/move_group/display_planned_path’,
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)

max_value = 255
max_value_H = 360//2
low_H = 0
low_S = 0
low_V = 0
high_H = max_value_H
high_S = max_value
high_V = max_value
window_capture_name = ‘Video Capture’
window_detection_name = ‘Object Detection’
low_H_name = ‘Low H’
low_S_name = ‘Low S’
low_V_name = ‘Low V’
high_H_name = ‘High H’
high_S_name = ‘High S’
high_V_name = ‘High V’
def on_low_H_thresh_trackbar(val):
global low_H
global high_H
low_H = val
low_H = min(high_H-1, low_H)
cv.setTrackbarPos(low_H_name, window_detection_name, low_H)
def on_high_H_thresh_trackbar(val):
global low_H
global high_H
high_H = val
high_H = max(high_H, low_H+1)
cv.setTrackbarPos(high_H_name, window_detection_name, high_H)
def on_low_S_thresh_trackbar(val):
global low_S
global high_S
low_S = val
low_S = min(high_S-1, low_S)
cv.setTrackbarPos(low_S_name, window_detection_name, low_S)
def on_high_S_thresh_trackbar(val):
global low_S
global high_S
high_S = val
high_S = max(high_S, low_S+1)
cv.setTrackbarPos(high_S_name, window_detection_name, high_S)
def on_low_V_thresh_trackbar(val):
global low_V
global high_V
low_V = val
low_V = min(high_V-1, low_V)
cv.setTrackbarPos(low_V_name, window_detection_name, low_V)
def on_high_V_thresh_trackbar(val):
global low_V
global high_V
high_V = val
high_V = max(high_V, low_V+1)
cv.setTrackbarPos(high_V_name, window_detection_name, high_V)

#def Quat2Mat(self, q: Quaternion) → rotMat:
def Quat2Mat(point1):
qx= 0.497791828975
qy= -0.502527906573
qz= 0.49788582261
qw= 0.501775553177

m00 = 1 - 2 * qy**2 - 2 * qz**2
m01 = 2 * qx * qy - 2 * qz * qw
m02 = 2 * qx * qz + 2 * qy * qw
m10 = 2 * qx * qy + 2 * qz * qw
m11 = 1 - 2 * qx**2 - 2 * qz**2
m12 = 2 * qy * qz - 2 * qx * qw
m20 = 2 * qx * qz - 2 * qy * qw
m21 = 2 * qy * qz + 2 * qx * qw
m22 = 1 - 2 * qx**2 - 2 * qy**2
#result = [[m00,m01,m02, -0.0142438828014],[m10,m11,m12,-0.0916812589659],[m20,m21,m22,-0.0320534281395],[0,0,0,1]]

result = R.from_quat([qx, qy, qz, qw])
result=result.as_dcm()
#print(result)
result = np.concatenate((result,np.reshape([0,0,0],(1,-1))),axis=0)
result = np.concatenate((result,np.reshape([-0.0142438828014,-0.0916812589659,-0.0320534281395,1],(-1,1))),axis=1)
result = np.linalg.pinv(np.array(result))


result_1 = np.matmul(np.array(result),point1)
    print("trans",result_1)
#t= np.array([[-1,0,0,-10.57586207],[0,-1,0,11.16831683],[0,0,1,212.23]])
    #result_2 = np.matmul(t,result_1)
#result_1[0][0]= -(result_1[0][0] +10.22)
#result_1[1][0]= -(result_1[1][0] -11.28)
#result_1[2][0]= +(result_1[2][0] +211.705)
return result_1

cv2.namedWindow(“Binary”)

cv2.createTrackbar(low_H_name, “Binary” , low_H, max_value_H, on_low_H_thresh_trackbar)
cv2.createTrackbar(high_H_name, “Binary” , high_H, max_value_H, on_high_H_thresh_trackbar)
cv2.createTrackbar(low_S_name, “Binary” , low_S, max_value, on_low_S_thresh_trackbar)
cv2.createTrackbar(high_S_name, “Binary” , high_S, max_value, on_high_S_thresh_trackbar)
cv2.createTrackbar(low_V_name, “Binary” , low_V, max_value, on_low_V_thresh_trackbar)
cv2.createTrackbar(high_V_name, “Binary” , high_V, max_value, on_high_V_thresh_trackbar)

def get_coordinates():

B = []
G = []
R = []

count = 0

x = []
y = []
z = []
#color = get_color_from_speach()
    coordinate = 0

pipeline = rs.pipeline()
config = rs.config()
depth_resolution_x =640
depth_resolution_y =480
color_resolution_x =640
color_resolution_y =480
depth_fps =30
color_fps =60
config.enable_stream(rs.stream.depth, depth_resolution_x, depth_resolution_y, rs.format.z16, depth_fps) 
config.enable_stream(rs.stream.color, color_resolution_x, color_resolution_y, rs.format.bgr8, color_fps)
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
align_to = rs.stream.color
    align = rs.align(align_to)


while True:
##wait for a new frame and then get the depth and color frame 
	frames = pipeline.wait_for_frames() 
	frames = align.process(frames)
	depth_frame = frames.get_depth_frame()
	color_frame = frames.get_color_frame()
	if not depth_frame or not color_frame:
		continue

	##create numpy array of depth and color frames
	depth_image = numpy.asanyarray(depth_frame.get_data())
	color_image = numpy.asanyarray(color_frame.get_data())




	img = color_image
            cv2.imwrite("colour.jpg",img)
	depth_image = depth_image
	img1 = img.copy()
	bgr = [33, 74, 100]
	hsv_range = 20


	


	bgr_img = numpy.uint8([[bgr]])
	hsv_img = cv2.cvtColor(bgr_img, cv2.COLOR_BGR2HSV)
	cv_value = hsv_img[0, 0, 0]

	lower = numpy.array([37, 28, 80])
	upper = numpy.array([77, 48, 140])

	# img = cv2.resize(im, (0, 0), fx=0.2, fy=0.2)  # reducing the size of the image
	cv2.imshow('org', img)  # displaying the image
	gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)  # converting the image to gray scale
	in_range_mask = cv2.inRange(img, (0, 0, 90), (59, 124, 255))
         	
	ret, thresh = cv2.threshold(gray, 120, 255, cv2.THRESH_BINARY)  # converting the image to binary
	#gs_image = cv2.cvtColor(in_range_mask, cv2.COLOR_BGR2GRAY)	
	cv2.imshow('gray image', gray)
	cv2.imshow('Binary', in_range_mask)  # displaying the binary image

	_,contours, hierarchy = cv2.findContours(in_range_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_TC89_L1)  # contour output commands
	#print("The length of the contour is: " + str(len(contours)))  # printing the length of the contours
	filtered = []  # creating an empty array for filtering
	for c in contours:
		print("area",cv2.contourArea(c))
		if cv2.contourArea(c) < 500:  # ignore the contour area when it is less than 700px
			continue
		elif cv2.contourArea(c) > 7000:  # ignore the contour area when it is greater than 2500px
			continue
		else:
			color = (0, 255, 255)  # color of the contour boundary
			objects = numpy.zeros([img.shape[0], img.shape[1], 3], 'uint8')  # empty arrat
                            filtered.append(c)  # draw the contour when the condition is satisfied
			x,y,w,h = cv2.boundingRect(c) # offsets - with this you get 'mask'
			cv2.rectangle(img,(x,y),(x+w,y+h),(0,255,0),2)
			cv2.imshow('cutted contour',img[y:y+h,x:x+w])
			#print('Average color (BGR): ',numpy.array(cv2.mean(img[y:y+h,x:x+w])).astype(numpy.uint8))
			b,g,r,u = numpy.array(cv2.mean(img[y:y+h,x:x+w])).astype(numpy.uint8)

			coordinate = []
			cv2.drawContours(img1, [c], -1, color, 2)  # contour characteristics
			M = cv2.moments(c)  # image moment of the contour
			rect = cv2.minAreaRect(c)
			angle = rect[2]
			print(angle)
			if M["m00"] != 0:
			     px = int(M["m10"] / M["m00"])  # x coordinate centroid
			     py = int(M["m01"] / M["m00"])  # y coordinate centroid
			     cv2.drawContours(img, [c], -1, (0, 255, 0), 2)
			     count += 1
			     color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
			     #cx = (px * 0.3) + 130.5
			     #cy = -((py * 0.3) - 86.5)

			     #x = cx
			     #y = cy

			     depth = depth_image[py, px].astype(float)
			     z = depth * depth_scale 
			     point1 = rs.rs2_deproject_pixel_to_point(color_intrin, [px, py], depth)
			     point1= np.reshape(point1,(-1,1))
			     point1= np.concatenate([point1,np.reshape([1],(-1,1))],axis=0)
			     #rect= cv2.minAreaRect(c)
			     #angle = rect[2]
			     #print("angle",angle)
			     print("hello")
			     print("Deprojected point",point1)
			     print("hello")
			     coordinate = Quat2Mat(point1)
			     print("hi")
			else:
			     px, py = 0, 0

			  

			#print(r,b,g,u)
			B.append(b)
			G.append(g)
			R.append(r)
			#print(B,G,R)
			b = 0
			g = 0 
			r = 0

			#print("The total number of contours drawn are: " + str(len(filtered)))  # print the total number of contour drawn


			cv2.imshow('Contours', img1)  # displaying the final image

			
	if cv2.waitKey(30)== ord('q'):
		break
cv2.destroyAllWindows()  # after pressing any key close all the window
    return coordinate

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
print(“Pose Goal:”)
print(pose_goal)
group.set_pose_target(pose_goal)
plan= group.plan()
rospy.sleep(5)
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
group.execute(plan)
rospy.sleep(5)
#group.go(wait=True)
#group.stop()
group.clear_pose_targets()

def gripper():
pub = rospy.Publisher(‘gripper_pose_1’, GripperState )
r = rospy.Rate(1000) #10hz
msg = GripperState()
count = 0
while ~ rospy.is_shutdown():
msg.target_position = 0
msg.target_force = 20

	rospy.loginfo(msg)
	pub.publish(msg)

	time.sleep(2)
	
	print("Done once")
	#time.sleep(2)
	count+= 1
	if count ==2:
		break

def follow_cartesian_path():
“”" This function shows an example of how to have the UR5 follow a cartesian path with MoveIt"""
waypoints =

pose_goal = group.get_current_pose().pose
print("pose",pose_goal)
pose_goal.position.x = 0.15970
pose_goal.position.y = 0.3580
pose_goal.position.z = 0.340  # 0.21
waypoints.append(copy.deepcopy(pose_goal))


(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0)

print(waypoints)

display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
time.sleep(2)
group.execute(plan)

def follow_cartesian_path_1():
“”" This function shows an example of how to have the UR5 follow a cartesian path with MoveIt"""
waypoints =

pose_goal = group.get_current_pose().pose
pose_goal.position.x = 0.18970
pose_goal.position.y = 0.3180
pose_goal.position.z = 0.340 # 0.21

waypoints.append(copy.deepcopy(pose_goal))


(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0)

print(waypoints)

display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
time.sleep(2)
group.execute(plan)

def Pick_position():
“”" This function shows an example of how to have the UR5 follow a cartesian path with MoveIt"""
waypoints =
points = get_coordinates()
print(“point”,points)
waypoints =

pose_goal = group.get_current_pose().pose
pose_goal.position.x = points[0][0]/1000
pose_goal.position.y = points[1][0]/1000
pose_goal.position.z = 0.340 # 0.21

waypoints.append(copy.deepcopy(pose_goal))


(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0)

print(waypoints)

display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
time.sleep(2)
group.execute(plan)

#move_to_pos(0.182,0.233,0.369)
#move_to_pos(0.18970,0.3180,0.340)
follow_cartesian_path()
#follow_cartesian_path_1()
gripper()

Pick_position()

If you could try stripping down your code example to the relevant motion parts, it would be much easier for others to help diagnosing your problems.

Also, please post code inside a code block:

```python
def my_function():
    return 0
```

which results in

def my_function():
    return 0

Hi Mauch,

Thanks for your suggestion, i have posted where i am going wrong

def Pick_position():
“”" This function shows an example of how to have the UR5 follow a cartesian path with MoveIt"""
waypoints =
points = get_coordinates()
print(“point”,points)
waypoints =

pose_goal = group.get_current_pose().pose
pose_goal.position.x = points[0][0]/1000
pose_goal.position.y = points[1][0]/1000
pose_goal.position.z = 0.340 # 0.21

waypoints.append(copy.deepcopy(pose_goal))


(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0)

print(waypoints)

display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
time.sleep(2)
group.execute(plan)

So… what would be an example output (Of your script, ob moveit, of the robot driver, etc…)? What is going wrong? what would you expect?

Any bit of information provided can help others identifying what is going wrong.

here we are using Intel realsense depth camera for vision. We have done Hand eye calibration for getting transformation matrix for camera.

Using the matrix we have done inverse and passed object detected coordinate. And output to robot. robot is not moving to object position, the error is more than 500mm in both x and y.

The following code is used for transformation matrix. x,y,z,qx,qy,qz, and qw got it from hand eye calibration.

def Quat2Mat(point1):
qx= 0.497791828975
qy= -0.502527906573
qz= 0.49788582261
qw= 0.501775553177

m00 = 1 - 2 * qy**2 - 2 * qz**2
m01 = 2 * qx * qy - 2 * qz * qw
m02 = 2 * qx * qz + 2 * qy * qw
m10 = 2 * qx * qy + 2 * qz * qw
m11 = 1 - 2 * qx**2 - 2 * qz**2
m12 = 2 * qy * qz - 2 * qx * qw
m20 = 2 * qx * qz - 2 * qy * qw
m21 = 2 * qy * qz + 2 * qx * qw
m22 = 1 - 2 * qx**2 - 2 * qy**2
#result = [[m00,m01,m02, -0.0142438828014],[m10,m11,m12,-0.0916812589659],[m20,m21,m22,-0.0320534281395],[0,0,0,1]]

result = R.from_quat([qx, qy, qz, qw])
result=result.as_dcm()
#print(result)
result = np.concatenate((result,np.reshape([0,0,0],(1,-1))),axis=0)
result = np.concatenate((result,np.reshape([-0.0142438828014,-0.0916812589659,-0.0320534281395,1],(-1,1))),axis=1)
result = np.linalg.pinv(np.array(result))


result_1 = np.matmul(np.array(result),point1)
    print("trans",result_1)
#t= np.array([[-1,0,0,-10.57586207],[0,-1,0,11.16831683],[0,0,1,212.23]])
    #result_2 = np.matmul(t,result_1)
#result_1[0][0]= -(result_1[0][0] +10.22)
#result_1[1][0]= -(result_1[1][0] -11.28)
#result_1[2][0]= +(result_1[2][0] +211.705)
return result_1

suggest if any other method is there

Thank you

Please suggest how to transfer camera coordinates to robot coordinates of object detected in realsense depth camera

thank you

Of the script above you have a print(“point”,points) statement. What does that print in an example configuration. In which frame are those points given?

With the line (plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0) you ask MoveIt for a plan. It would be good to know what that plan looks like or if MoveIt prints an error while requesting this.

group.execute(plan) should execute the plan using the driver. Does that yield any output from MoveIt or from the driver?

What is the actual action goal being published on /scaled_pos_joint_traj_controller/follow_joint_trajectory/goal?

print(“point”,points) point value is given with base frame reference.

group.execute(plan) Not getting any error output

(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0) with this i am not getting error from the moveit. only the issue is the coordinates passing are not matching with robot.

due to above error i have used following code


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()

Through the above code its matching both input and robot value.

I am stuck at passing the camera coordinate to robot coordinate. Here i used eye on hand calibration and created a tf node. whatever the object coordinate i got passed to this transformation.

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

The robot not moving to object position exactly. there are different position error for each object.

Please help me, where i am going wrong