# UR5 vision base pick and place

**URL:** <https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017>\
**Category:** ROS\
**Created:** [June 21, 2021, 6:08am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017 "2021-06-21T06:08:04Z")\
**Posts on this page:** 9\
**Page:** 1

<div class="post-metadata">

**Author:** ![anon81261137](https://avatars.discourse-cdn.com/v4/letter/a/f19dbf/32.png) [@anon81261137](https://forum.universal-robots.com/u/anon81261137)\
**Post date:** [June 21, 2021, 6:08am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/1 "2021-06-21T06:08:04Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![mauch](https://avatars.discourse-cdn.com/v4/letter/m/71e660/32.png) [@mauch](https://forum.universal-robots.com/u/mauch)\
**Post date:** [June 21, 2021, 7:07am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/2 "2021-06-21T07:07:35Z")

</div>

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

```python
def my_function():
    return 0

```

---

<div class="post-metadata">

**Author:** ![anon81261137](https://avatars.discourse-cdn.com/v4/letter/a/f19dbf/32.png) [@anon81261137](https://forum.universal-robots.com/u/anon81261137)\
**Post date:** [June 21, 2021, 7:31am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/3 "2021-06-21T07:31:29Z")

</div>

Hi Mauch,

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

```python
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)

```

---

<div class="post-metadata">

**Author:** ![mauch](https://avatars.discourse-cdn.com/v4/letter/m/71e660/32.png) [@mauch](https://forum.universal-robots.com/u/mauch)\
**Post date:** [June 21, 2021, 8:12am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/4 "2021-06-21T08:12:25Z")

</div>

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.

---

<div class="post-metadata">

**Author:** ![anon81261137](https://avatars.discourse-cdn.com/v4/letter/a/f19dbf/32.png) [@anon81261137](https://forum.universal-robots.com/u/anon81261137)\
**Post date:** [June 21, 2021, 10:27am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/5 "2021-06-21T10:27:39Z")

</div>

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.

```python
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

---

<div class="post-metadata">

**Author:** ![anon81261137](https://avatars.discourse-cdn.com/v4/letter/a/f19dbf/32.png) [@anon81261137](https://forum.universal-robots.com/u/anon81261137)\
**Post date:** [June 21, 2021, 10:45am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/6 "2021-06-21T10:45:46Z")

</div>

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

thank you

---

<div class="post-metadata">

**Author:** ![mauch](https://avatars.discourse-cdn.com/v4/letter/m/71e660/32.png) [@mauch](https://forum.universal-robots.com/u/mauch)\
**Post date:** [June 23, 2021, 10:45am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/7 "2021-06-23T10:45:38Z")

</div>

Of the script [above](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/2) 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`?

---

<div class="post-metadata">

**Author:** ![anon81261137](https://avatars.discourse-cdn.com/v4/letter/a/f19dbf/32.png) [@anon81261137](https://forum.universal-robots.com/u/anon81261137)\
**Post date:** [June 29, 2021, 4:33am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/8 "2021-06-29T04:33:34Z")

</div>

`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

```python

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.

---

<div class="post-metadata">

**Author:** ![anon81261137](https://avatars.discourse-cdn.com/v4/letter/a/f19dbf/32.png) [@anon81261137](https://forum.universal-robots.com/u/anon81261137)\
**Post date:** [June 29, 2021, 4:43am UTC](https://forum.universal-robots.com/t/ur5-vision-base-pick-and-place/15017/9 "2021-06-29T04:43:47Z")

</div>

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.

```python
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