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