class uArmSwift: def __init__(self): self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, cmd_pend_size=2, callback_thread_pool_size=1) if not self.swift.connected: print('lose connect') self.swift.waiting_ready() device_info = self.swift.get_device_info() print(device_info) firmware_version = device_info['firmware_version'] if firmware_version and not firmware_version.startswith( ('0.', '1.', '2.', '3.')): self.swift.set_speed_factor(0.00005) self.swift.set_mode(0) self.speed = 500000 self.swift.set_wrist(angle=90) self.wristAngle = self.swift.get_servo_angle(0, timeout=10) def set_position(self, x=100, y=0, z=100, wait=False): self.swift.set_position(x, y, z, speed=self.speed, wait=wait) def set_polar(self, stretch, rotation, height, wait=False): self.swift.set_polar(stretch, rotation, height, speed=self.speed, wait=wait) def set_servo_angle(self, num, angle, wait=False): if num < 0 and num > 3: print("num is wrong") self.swift.set_servo_angle(num, angle, wait, speed=self.speed, wait=wait) def set_wrist(self, angle=90, wait=False): # 第四电机 self.swift.set_wrist(angle, wait) def set_pump(self, on=False): self.swift.set_pump(on) def set_buzzer(self, freq=1000, duration=1, wait=False): self.swift.set_buzzer(freq, duration, wait) def get_position(self): return self.swift.get_position() def get_servo_angle(self, id=0): return self.swift.get_servo_angle(id, timeout=10) def is_moving(self): return self.swift.get_is_moving() def disconnect(self): self.swift.disconnect()
sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) from uarm.wrapper import SwiftAPI from uarm.utils.log import logger logger.setLevel(logger.DEBUG) swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) # swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_handle_thread=False) # swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_write_thread=True) # swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_report_thread=True) # swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}, enable_handle_thread=True, enable_write_thread=True, enable_report_thread=True) swift.reset(speed=50000, wait=True) swift.set_position(x=250, y=0, z=10, speed=50000, wait=True) swift.flush_cmd() swift.set_pump(True) swift.set_position(x=250, y=0, z=170, speed=50000, wait=True) swift.flush_cmd() swift.set_position(x=220, y=70, z=170, speed=50000, wait=True) swift.flush_cmd() swift.set_position(x=200, y=140, z=170, speed=50000, wait=True) swift.flush_cmd() swift.set_position(x=180, y=210, z=170, speed=50000, wait=True) swift.flush_cmd() swift.set_position(x=250, y=210, z=10, speed=50000, wait=True) swift.flush_cmd() swift.set_pump(False) swift.flush_cmd() swift.set_position(x=250, y=210, z=120, speed=50000, wait=True) swift.flush_cmd() swift.reset(speed=50000, wait=True)
class UArm_SDK(object): def __init__(self): ''' connect to UArm ''' self.swift = SwiftAPI() self.swift.connect() self.swift.get_power_status() print(self.swift.get_device_info()) self.swift.reset(wait=True) # back to home position print('init complete') self.gripper_temp = 0 # keep track of gripper state def __del__(self): ''' disconnect UArm ''' self.swift.disconnect() print('uarm disconnected') def set_servo_angle(self, joint_angles, dt): ''' set servo angle via SDK input: joint_angles, 5-vector: [theta1, theta2, theta3, theta4, pump state] in degrees dt, time step ''' wait = True self.swift.set_servo_angle(servo_id=0, angle=joint_angles[0] + 90, speed=5000, wait=wait) time.sleep(dt / 4) self.swift.set_servo_angle(servo_id=1, angle=joint_angles[1], speed=5000, wait=wait) time.sleep(dt / 4) self.swift.set_servo_angle(servo_id=2, angle=joint_angles[2] - joint_angles[1], speed=5000, wait=wait) time.sleep(dt / 4) self.swift.set_servo_angle(servo_id=3, angle=180 - joint_angles[3], speed=5000, wait=wait) time.sleep(dt / 4) if joint_angles[4] > 0: self.swift.set_pump(on=True) elif joint_angles[4] == 0: self.swift.set_pump(on=False) else: print("ERROR") def control_uarm_via_traj(self, position, wrist_angle, pump_state, dt): ''' set end effector position, wrist angle and pump state via SDK input: position, 3-vector: [px, py, pz] wrist_angle: wrist angle in rad pump_state: bool, 0 - off, 1 - on ''' px, py, pz = position[0], position[1], position[2] # conver m to mm px *= 1000 py *= 1000 pz *= 1000 # change end effector position e = self.swift.set_position(x=px, y=py, z=pz, speed=100000, wait=True) print(e) # change wrist angle self.swift.set_wrist(90 - wrist_angle * 180 / PI) if self.gripper_temp == 0 and pump_state == 1: # enable suction cup self.swift.set_pump(on=True, wait=True) print('pump on') self.gripper_temp = 1 if self.gripper_temp == 1 and pump_state == 0: # disable suction cup self.swift.set_pump(on=False, wait=True) print('pump off') self.gripper_temp = 0 time.sleep(dt)
class uarmRobotClass(object): def __init__(self): self.x_pos = 0 self.y_pos = 0 self.tcp_rot = 0 self.z_pos = 0 self.pick = 0 self.connect = 0 self.swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) self.swift.waiting_ready(timeout=3) self.swift.set_speed_factor(0.5) self.swift.waiting_ready(timeout=3) self.swift.disconnect() def openPort(self): self.swift.connect() #swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) self.swift.waiting_ready(timeout=3) self.swift.set_speed_factor(0.5) self.swift.waiting_ready(timeout=3) self.swift.set_position(150, 0, 50, wait=True) def closePort(self): self.swift.disconnect() self.swift.waiting_ready(timeout=3) def move(self): Index = 0 pick = 0 connect = 0 while True: if (dummy == False) and (connect == 0) and (self.connect == 1): self.openPort() ret = self.swift.get_position(wait=True) print("ret values %d, %d, %d" % (ret[0], ret[1], ret[2])) x_pos = ret[0] y_pos = ret[1] tcp_rot = ret[2] print("connected") connect = 1 if (dummy == False) and (connect == 1) and (self.connect == 0): self.closePort() connect = 0 print("disconnnected") if (self.x_pos != 0) or (self.y_pos != 0) or (self.tcp_rot != 0) or (self.z_pos != 0) or (self.pick != 0) or (self.connect != 0): if logLevel > 1: print("index = %d: x_pos = %d; y_pos = %d; z_pos = %d; tcp_rot = %d; pick = %d; connect = %d" % (Index, self.x_pos, self.y_pos, self.z_pos, self.tcp_rot, self.pick, self.connect)) Index = Index + 1 if (dummy == False) and (connect == 1) and ((self.x_pos != 0) or (self.y_pos != 0) or (self.tcp_rot != 0) or (self.z_pos != 0)): self.swift.set_position(x=self.x_pos, y=self.y_pos, z=self.z_pos, wait=True, relative=True); ret = self.swift.get_position(wait=True) if logLevel > 0: print("ret values %d, %d, %d" % (ret[0], ret[1], ret[2])) if (dummy == False) and (pick == 0) and (self.pick != 0): self.swift.set_pump(on=True) pick = 1 if (dummy == False) and (pick == 1) and (self.pick == 0): self.swift.set_pump(on=False) pick = 0 time.sleep(0.001);
from uarm.wrapper import SwiftAPI swift = SwiftAPI() swift.waiting_ready(timeout=10) swift.set_position(100, -120, 50, wait=True) test = swift.get_position() print(test) swift.set_position(300, -120, 50, wait=True) test = swift.get_position() print(test) swift.set_position(300, 120, 100, wait=True) test = swift.get_position() print(test) swift.set_position(100, 120, 100, wait=True) test = swift.get_position() print(test) swift.set_position(120, 0, 50, wait=True) test = input("the pump will be set on, press enter to continue") swift.set_pump(on=True) swift.set_position(200, 0, 50, wait=True) test = input("the pump will be set off, press enter to continue") swift.set_pump(on=False) swift.set_position(120, 0, 50, wait=True)
from uarm.wrapper import SwiftAPI print("アームと接続を開始...") swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) swift.waiting_ready() print(swift.get_device_info()) print("アームと接続完了") swift.set_pump(on=True)
# # Author: Vinman <*****@*****.**> <*****@*****.**> import os import sys import time sys.path.append(os.path.join(os.path.dirname(__file__), '../../..')) from uarm.wrapper import SwiftAPI """ api test: set """ swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'}) swift.waiting_ready() swift.set_buzzer(frequency=1000, duration=2, wait=True) print(swift.set_pump(on=True)) time.sleep(2) print(swift.set_pump(on=False)) print(swift.set_gripper(catch=True)) time.sleep(2) print(swift.set_gripper(catch=False)) time.sleep(4) swift.flush_cmd() swift.disconnect()
class Arm: def __init__(self): self.arm = SwiftAPI() self.arm.reset() sleep(2) def rest(self): self.arm.set_position(x=0, y=180, z=146, wait=True, timeout=10, speed=10000) print("moving to rest pos!") def correction(self): print("=================check start!=================\n") self.arm.set_position(x=227, y=0, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=227, y=0, z=47, wait=True, timeout=10, speed=10000) print("check center\n") sleep(1) input() self.arm.set_position(x=292, y=75, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=292, y=75, z=47, wait=True, timeout=10, speed=10000) print("check left-up\n") sleep(1) input() self.arm.set_position(x=293, y=-75, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=293, y=-75, z=47, wait=True, timeout=10, speed=10000) print("check right-up\n") sleep(1) input() self.arm.set_position(x=160, y=75, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=160, y=75, z=47, wait=True, timeout=10, speed=10000) print("check left-down\n") sleep(1) input() self.arm.set_position(x=160, y=-75, z=60, wait=True, speed=10000) sleep(1) self.arm.set_position(x=160, y=-75, z=47, wait=True, timeout=10, speed=10000) print("check right-down\n") sleep(1) input() self.arm.set_position(x=0, y=180, z=146, wait=True, timeout=10, speed=10000) print("=================check finish!=================\n") def move(self, color, coords): global count count += 1 if coords == "pass" or coords == "resign": return None y, x = coords y = int(ord(y) - ord('A')) x = int(x) - 1 if count == 1: x_take = 309 elif count == 2: x_take = 280 elif count == 3: x_take = 254 elif count == 4: x_take = 223 elif count == 5: x_take = 200 elif count == 6: x_take = 170 elif count == 7: x_take = 144 count = 0 if color == "B": xp = x_take yp = -145 zp = 100 elif color == "W": xp = x_take yp = 157 zp = 100 else: print("Wrong color input!") self.arm.set_position(x=xp, y=yp, z=zp, wait=True, speed=10000) self.arm.flush_cmd(wait_stop=True) self.arm.set_position(z=54, wait=True, speed=10000) sleep(1.5) self.arm.set_pump(True) sleep(2) self.arm.set_position(z=100, wait=True, speed=10000) self.arm.set_position(x=294 - x * 22, y=-66 + y * 24, z=100, wait=True, speed=10000) self.arm.flush_cmd(wait_stop=True) print("moving to:", coords) self.arm.set_position(z=54, wait=True, speed=10000) # original z is -26 sleep(1) self.arm.set_pump(False) sleep(0.5) self.arm.set_position(z=100, wait=True, speed=10000) sleep(0.5) self.arm.flush_cmd() def remove(self, coords, remove_list): self.arm.set_position(x=1, y=2, z=45, wait=True, speed=10000) x, y = coords remove_list def remove_chess(self, color, coords): y, x = coords y = int(y) - 1 x = int(x) - 1 self.arm.set_position(x=294 - x * 22, y=-66 + y * 24, z=100, wait=True, speed=10000) self.arm.flush_cmd(wait_stop=True) self.arm.set_position(z=54, wait=True, speed=10000) sleep(1.5) self.arm.set_pump(True) sleep(2) print("removing :", coords) self.arm.set_position(z=100, wait=True, speed=10000) self.arm.set_position(x=342, y=0, z=90, wait=True, speed=10000) self.arm.flush_cmd(wait_stop=True) self.arm.set_pump(False) sleep(0.5) def take_photo(self): self.arm.set_position(x=166, y=4, z=220, wait=True, timeout=10, speed=10000) print("Take Picture place!") def game_over(self): self.arm.set_position(x=0, y=170, z=100, wait=True, timeout=10, speed=10000) print("Game over!")