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!")
