针对从相机英特尔d435 Realsense——可打印版本

代码:

进口cv2 #库最先进的计算机视觉算法
进口numpy np #科学计算的基本方案
进口imageio
进口matplotlib。pyplot plt # 2 d绘图图书馆出版生产质量数据
进口pyrealsense2 rs
进口win32gui
进口win32api
导入的时间
进口pyautogui
进口tkinter
进口tkinter.messagebox
从robolink进口* # API与RoboDK沟通
从robodk进口* #基本矩阵运算

#任何与RoboDK交互都必须通过
RL = Robolink ()

顶级= tkinter.Tk ()

点= rs.points ()
管= rs.pipeline ()
配置= rs.config ()
形象= pipe.start(配置)

彩色化= rs.colorizer ()
align = rs.align (rs.stream.color)
state_left = win32api.GetKeyState (0 x01) #左按钮= 0或1。扣= -127年或-128年
设备= profile.get_device() #类型:rs.device
depth_sensor = device.first_depth_sensor() #类型:rs.depth_sensor


如果depth_sensor.supports (rs.option.depth_units):
depth_sensor.set_option (rs.option.depth_units, 0.0001)

机器人= RL.Item (“UR5e”)
帧= RL。项目(第二帧)
目标= RL。AddTarget(目标2,框架)

试一试:
而真正的:
帧= pipe.wait_for_frames ()
帧= align.process(帧)
depth_frame = frames.get_depth_frame ()
color_frame = frames.get_color_frame ()
如果不是depth_frame:继续
#打印(宽度、高度)
深度= np.asanyarray (colorizer.colorize (depth_frame) .get_data ())
颜色= np.asanyarray (color_frame.get_data ())
图像= np。hstack((颜色、深度))
# cv2.namedWindow (RGB_View, cv2.WINDOW_NORMAL)
# cv2。cv2 setWindowProperty (“RGB_View”。WND_PROP_FULLSCREEN cv2.WINDOW_FULLSCREEN)
cv2。imshow (“RGB_View”、深度)
.intrinsics depth_intrin = depth_frame.profile.as_video_stream_profile ()
#旗帜,hcursor (x, y) = win32gui.GetCursorInfo ()
x, y = win32api.GetCursorPos ()
#打印(x, y)
pixel_coord = (x, y)
keypressed = win32api.GetKeyState (0 x01)
关键= cv2.waitKey (1)
#按esc或“q”关闭窗口形象
如果关键& 0 xff = =奥德(q)或关键= = 27:
cv2.destroyAllWindows ()
打破
#计算距离
如果keypressed ! = state_left:
state_left = keypressed
打印(keypressed)
如果keypressed < 0:
如果(0 < x < 632和32 < < 512):
dist_to_center = depth_frame.get_distance (x, y)
dist_to_center =圆(dist_to_center, 4)
depth_point = rs.rs2_deproject_pixel_to_point (depth_intrin pixel_coord dist_to_center)
depth_point =(我为我在depth_point * 100)
depth_point =[圆(我,5)因为我在depth_point)
# depth_point = (“,”。加入(ein depth_point repr (e)))
#张开(“坐标。txt”、“w”)作为f:
# f。写(“% s \ n”% depth_point)
# f.close ()
打印(depth_point)
print(相机正面临一个对象:,dist_to_center,“米”)
Target.setPose(抵消(眼睛(),depth_point [0], depth_point [1], depth_point [2], 0, 0, 0))
robot.MoveJ(目标)
time . sleep (0.7)

最后:
pipe.stop ()

代码:

#以当前机器人位置为一个完整的姿势(你可以使用这个方向)
target_pose = robot.Pose ()

#计算绝对目标坐标(世界/普通参考系)
# target_pose_abs = target.Parent () .PoseAbs () * target_pose
target_pose_abs = target.PoseAbs() #前一行做了同样的事情

这个姿势#力XYZ绝对坐标(关于世界坐标系)
target_pose_abs.setPos (xyz_abs)

#设置目标给定的姿势绝对位置(相对目标相应的计算)
target.setPoseAbs (target_pose_abs)

#移动到目标
robot.MoveJ(目标)