From 9740bc74e77dcf347637cac0e9f010bbf9fb9685 Mon Sep 17 00:00:00 2001 From: 2929ss <1814754095@qq.com> Date: Mon, 11 Jul 2022 14:18:12 +0800 Subject: [PATCH] update --- .../mechArm_270/launch/vision_m5.launch | 12 ++-- .../mechArm_270/launch/vision_pi.launch | 2 +- .../scripts/combine_detect_obj_color.py | 69 +++++++++++-------- .../combine_detect_obj_img_folder_opt.py | 66 ++++++++++-------- mycobot_ai/mechArm_270/scripts/test.py | 44 +++++++----- 5 files changed, 108 insertions(+), 85 deletions(-) diff --git a/mycobot_ai/mechArm_270/launch/vision_m5.launch b/mycobot_ai/mechArm_270/launch/vision_m5.launch index 4805952..e4f9bd9 100644 --- a/mycobot_ai/mechArm_270/launch/vision_m5.launch +++ b/mycobot_ai/mechArm_270/launch/vision_m5.launch @@ -1,9 +1,9 @@  - + - - + + @@ -13,14 +13,14 @@ ["joint_states"] - - + + - + diff --git a/mycobot_ai/mechArm_270/launch/vision_pi.launch b/mycobot_ai/mechArm_270/launch/vision_pi.launch index df576e0..28ea918 100644 --- a/mycobot_ai/mechArm_270/launch/vision_pi.launch +++ b/mycobot_ai/mechArm_270/launch/vision_pi.launch @@ -2,7 +2,7 @@ - + diff --git a/mycobot_ai/mechArm_270/scripts/combine_detect_obj_color.py b/mycobot_ai/mechArm_270/scripts/combine_detect_obj_color.py index cf95c53..338b625 100644 --- a/mycobot_ai/mechArm_270/scripts/combine_detect_obj_color.py +++ b/mycobot_ai/mechArm_270/scripts/combine_detect_obj_color.py @@ -1,5 +1,5 @@ -# -*- coding:utf-8 -*- #!/usr/bin/env python2 +# -*- coding:utf-8 -*- from operator import imod from tokenize import Pointfloat import cv2 @@ -9,7 +9,7 @@ import json import os,sys import rospy from visualization_msgs.msg import Marker -from pymycobot.mypalletizer import MyPalletizer +from pymycobot.mycobot import MyCobot from moving_utils import Movement @@ -20,7 +20,7 @@ __version__ = "1.0" class Object_detect(Movement): - def __init__(self, camera_x = 160, camera_y = 10): + def __init__(self, camera_x = 145, camera_y = -5): # inherit the parent class super(Object_detect, self).__init__() # get path of file @@ -31,17 +31,16 @@ class Object_detect(Movement): # 移动角度 self.move_angles = [ - [0, 0, 0, 0], # init the point - [-29.0, 5.88, -4.92, -76.28], # point to grab - [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab + [0, 0, 0, 0, 90, 0], # point to grab + [-33.31, 2.02, -10.72, -0.08, 95, -54.84], # init the point ] # 移动坐标 self.move_coords = [ - [141.2, -142.0, 210, -26.8], # above the red bucket - [234.3, -120, 210, -48.77], # above the green bucket - [100.9, 159.3, 248.6, -124.27], # above the blue bucket - [-17.6, 161.6, 238.4, -152.31], # above the gray bucket + [92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket + [165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket + [88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket + [-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket ] # which robot: USB* is m5; ACM* is wio; AMA* is raspi @@ -154,17 +153,18 @@ class Object_detect(Movement): # Grasping motion def move(self, x, y, color): - # send Angle to move mypal260 + # send Angle to move 270 print(color) - self.mc.send_angles(self.move_angles[0], 20) + self.mc.send_angles(self.move_angles[0], 30) + time.sleep(4) + + # send coordinates to move 270 + self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0) time.sleep(3) - - # send coordinates to move mypal260 - self.mc.send_coords([x, y, 160, 0], 20, 0) - time.sleep(1.5) - self.mc.send_coords([x, y, 90, 0], 20, 0) - time.sleep(1.5) - + + self.mc.send_coords([x, y, 92, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 + time.sleep(3) + # open pump if "dev" in self.robot_m5: self.pump_on() @@ -172,14 +172,23 @@ class Object_detect(Movement): self.gpio_status(True) time.sleep(1.5) - self.mc.send_angle(2, 0, 20) - time.sleep(0.3) - self.mc.send_angle(3, -15, 20) - time.sleep(2) + tmp = [] + while True: + if not tmp: + tmp = self.mc.get_angles() + else: + break + time.sleep(0.5) + + # print(tmp) + self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30) + time.sleep(3) - self.mc.send_coords(self.move_coords[color], 20, 1) - self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color] - [1]/1000.0, self.move_coords[color][2]/1000.0) + + self.mc.send_coords(self.move_coords[color], 30, 1) + self.pub_marker(self.move_coords[color][0]/1000.0, + self.move_coords[color][1]/1000.0, + self.move_coords[color][2]/1000.0) time.sleep(3) # close pump @@ -196,7 +205,7 @@ class Object_detect(Movement): self.pub_marker( self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0) - self.mc.send_angles(self.move_angles[1], 20) + self.mc.send_angles(self.move_angles[1], 30) time.sleep(1.5) # decide whether grab cube @@ -214,12 +223,12 @@ class Object_detect(Movement): # init mypal260 def run(self): if "dev" in self.robot_m5: - self.mc = MyPalletizer(self.robot_m5, 115200) + self.mc = MyCobot(self.robot_m5, 115200) elif "dev" in self.robot_raspi: - self.mc = MyPalletizer(self.robot_raspi, 1000000) + self.mc = MyCobot(self.robot_raspi, 1000000) if not self.raspi: self.pub_pump(False, self.Pin) - self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) + self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30) time.sleep(3) # draw aruco diff --git a/mycobot_ai/mechArm_270/scripts/combine_detect_obj_img_folder_opt.py b/mycobot_ai/mechArm_270/scripts/combine_detect_obj_img_folder_opt.py index c40ffbe..143f8c3 100755 --- a/mycobot_ai/mechArm_270/scripts/combine_detect_obj_img_folder_opt.py +++ b/mycobot_ai/mechArm_270/scripts/combine_detect_obj_img_folder_opt.py @@ -20,7 +20,7 @@ from threading import Thread import tkFileDialog as filedialog import Tkinter as tk from moving_utils import Movement -from pymycobot.mypalletizer import MyPalletizer +from pymycobot.mycobot import MyCobot IS_CV_4 = cv2.__version__[0] == '4' __version__ = "1.0" # Adaptive seeed @@ -28,7 +28,7 @@ __version__ = "1.0" # Adaptive seeed class Object_detect(Movement): - def __init__(self, camera_x = 160, camera_y = 10): + def __init__(self, camera_x = 145, camera_y = -5): # inherit the parent class super(Object_detect, self).__init__() # get path of file @@ -38,17 +38,16 @@ class Object_detect(Movement): self.mc = None # 移动角度 self.move_angles = [ - [0, 0, 0, 0], # init the point - [-29.0, 5.88, -4.92, -76.28], # point to grab - [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab + [0, 0, 0, 0, 90, 0], # point to grab + [-33.31, 2.02, -10.72, -0.08, 95, -54.84], # init the point ] # 移动坐标 self.move_coords = [ - [141.2, -142.0, 210, -26.8], # above the red bucket - [234.3, -120, 210, -48.77], # above the green bucket - [100.9, 159.3, 248.6, -124.27], # above the blue bucket - [-17.6, 161.6, 238.4, -152.31], # above the gray bucket + [92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket + [165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket + [88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket + [-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket ] # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed @@ -176,15 +175,16 @@ class Object_detect(Movement): # Grasping motion def move(self, x, y, color): - # send Angle to move mypal260 - self.mc.send_angles(self.move_angles[0], 20) - time.sleep(3) + # send Angle to move 270 + self.mc.send_angles(self.move_angles[0], 30) + time.sleep(4) - # send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度 - self.mc.send_coords([x, y, 160, 0], 20, 0) - time.sleep(1.5) - self.mc.send_coords([x, y, 90, 0], 20, 0) - time.sleep(1.5) + # send coordinates to move 270 根据不同底板机械臂，调整吸泵高度 + self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0) + time.sleep(4) + + self.mc.send_coords([x, y, 92, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 + time.sleep(4) # open pump if "dev" in self.robot_m5: @@ -193,12 +193,20 @@ class Object_detect(Movement): self.gpio_status(True) time.sleep(1.5) - self.mc.send_angle(2, 0, 20) - time.sleep(0.3) - self.mc.send_angle(3, -15, 20) - time.sleep(2) + tmp = [] + while True: + if not tmp: + tmp = self.mc.get_angles() + else: + break + time.sleep(0.5) + + # print(tmp) + self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30) + time.sleep(3) + - self.mc.send_coords(self.move_coords[color], 20, 1) + self.mc.send_coords(self.move_coords[color], 30, 1) self.pub_marker(self.move_coords[color][0] / 1000.0, self.move_coords[color][1] / 1000.0, self.move_coords[color][2] / 1000.0) @@ -211,7 +219,7 @@ class Object_detect(Movement): self.gpio_status(False) time.sleep(6) - self.mc.send_angles(self.move_angles[1], 20) + self.mc.send_angles(self.move_angles[1], 30) time.sleep(1.5) # decide whether grab cube @@ -226,15 +234,15 @@ class Object_detect(Movement): # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 self.move(x, y, color) - # init mypal260 + # init 270 def run(self): if "dev" in self.robot_m5: - self.mc = MyPalletizer(self.robot_m5, 115200) + self.mc = MyCobot(self.robot_m5, 115200) elif "dev" in self.robot_raspi: - self.mc = MyPalletizer(self.robot_raspi, 1000000) + self.mc = MyCobot(self.robot_raspi, 1000000) if not self.raspi: self.pub_pump(False, self.Pin) - self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) + self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30) time.sleep(3) # draw aruco @@ -420,8 +428,8 @@ class Object_detect(Movement): # The path to save the image folder def parse_folder(folder): restore = [] - path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/' + folder - path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/' + folder + path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/mechArm_270/' + folder + path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/mechArm_270/' + folder if os.path.exists(path1): path = path1 diff --git a/mycobot_ai/mechArm_270/scripts/test.py b/mycobot_ai/mechArm_270/scripts/test.py index d73de19..167e662 100755 --- a/mycobot_ai/mechArm_270/scripts/test.py +++ b/mycobot_ai/mechArm_270/scripts/test.py @@ -1,38 +1,44 @@ -# -*- coding: utf-8 -*- +#!/usr/bin/env python2 +# -*- coding:utf-8 -*- +from pymycobot.mycobot import MyCobot from pymycobot.mypalletizer import MyPalletizer from pymycobot.genre import Angle from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化 import time,os -mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200) - +# mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200) +mc = MyCobot("/dev/ttyAMA0", 1000000) # mc = MyPalletizer("/dev/ttyAMA0", 1000000) -# mc.send_angles([-29.0, 5.88, -4.92, -76.28],25) # init the point coords:[155.3, -86.1, 218.4, -47.28] -# time.sleep(1.5) -# mc.send_angles([-47.1, 10.19, -10.1, -76.37],25) # above the red bucket; coords:[127.3, -137.1, 219.2, -29.26] -# time.sleep(1.5) -mc.send_angles([0,0,-15,0],25) -time.sleep(2) +mc.send_angles([0,0,0,0,90,0],30) # coords:[95.6, 0.5, 166.4, 179.12, -0.18, 179.46] +# mc.send_angles([-3.25, 17.22, -32.51, 2.37, 92.54, -36.21],30) -# mc.send_coords([141.2, -142.0, 206.2, -26.8],25,1) # above the red bucket -# time.sleep(2) -# mc.send_coords([234.3, -120, 210, -48.77],25,1) # above the green bucket -# time.sleep(2) -# mc.send_coords([100.9, 159.3, 248.6, -124.27],20,1) # above the blue bucket -# time.sleep(3) -# mc.send_coords([-17.6, 161.6, 238.4, -152.31],20,1) # above the gray bucket +time.sleep(4) + +# mc.send_coords([92.3, -104.9, 211.4, -179.6, 28.91, 131.29], 30, 0) # above the red bucket +# time.sleep(4) + +# mc.send_coords([165.0, -93.6, 201.4, -173.43, 46.23, 160.65],30,0) # above the green bucket +# time.sleep(4) + +# mc.send_coords([88.1, 126.3, 193.4, 162.15, 2.23, 156.02],30,0) # above the blue bucket +# time.sleep(4) + +# mc.send_coords([-5.4, 120.6, 204.6, 162.66, -6.96, 159.93],30,0) # above the gray bucket # time.sleep(3) +# mc.send_coords([80, 0, 130, 0 ,0, 0], 30, 0) + # mc.send_angle(3,0,25) # print(mc.get_angles()) # print(mc.get_coords()) +# mc.release_all_servos() # while True: -# print("angles:%s"%mc.get_angles()) -# print("coords:%s"%mc.get_coords()) -# print("\n") +# print("angles:%s"% mc.get_angles()) +# print("coords:%s"% mc.get_coords()) +# print("\n") # mc.release_all_servos() # mc.set_servo_calibration(1)