class set_arm_control():
def __init__(self,RELAY_PIN=12):
self.arm = SwiftAPI()
self.arm.get_power_status()
self.RELAY_PIN = RELAY_PIN
self.arm.reset()
#機械手臂進行初始化動作
def set_arm_origin(self):
self.arm.waiting_ready()
self.arm.set_position(200, 0, 170, speed=100000)
# 機械手臂設定(x, y, z)座標移動
def set_arm_move(self, x, y, z):
self.arm.set_position(x, y, z, speed=100000)
# 機械手臂切斷連線
def arm_disconnect(self):
self.arm.disconnect()
def get_position(self):
print(self.arm.get_position())
#初始設定繼電器
def origin_pump(self):
GPIO.setmode(GPIO.BOARD) # 指定模式BOARD 啟用RPi板子相對應之腳位編號
GPIO.setwarnings(False) # 避免出現警告
GPIO.setup(self.RELAY_PIN, GPIO.OUT) # 設定pin腳為輸出
#打開幫浦=開啟繼電器(使電流流通)
def start_pump(self):
GPIO.output(self.RELAY_PIN, 0)
#關閉幫浦=關閉繼電器(使電流不能流通)
def close_pump(self):
GPIO.output(self.RELAY_PIN, 1)
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)
# -*- coding: utf-8 -*- """ Created on Thu Apr 11 11:21:16 2019 @author: xumw1 """ import time import numpy as np from uarm.wrapper import SwiftAPI swift = SwiftAPI() swift.connect() swift.get_power_status() print(swift.get_device_info()) T = 2 dt = 0.01 # best we can get: 0.01 N = int(T / dt) #swift.set_servo_angle(servo_id=0, angle=90) ##time.sleep(3) #swift.set_servo_angle(servo_id=1, angle=90) ##time.sleep(3) #swift.set_servo_angle(servo_id=2, angle=0) ##time.sleep(3) #swift.set_servo_angle(servo_id=3, angle=90) swift.reset() time.sleep(3)
import os
import sys
import time
import functools
sys.path.append(os.path.join(os.path.dirname(__file__), '../../..'))
from uarm.wrapper import SwiftAPI
"""
api test: get
"""
swift = SwiftAPI(filters={'hwid': 'USB VID:PID=2341:0042'})
swift.waiting_ready()
ret = swift.get_power_status()
print('power status: {}'.format(ret))
ret = swift.get_device_info()
print('device info: {}'.format(ret))
ret = swift.get_limit_switch()
print('limit switch: {}'.format(ret))
ret = swift.get_gripper_catch()
print('gripper catch: {}'.format(ret))
ret = swift.get_mode()
print('mode: {}'.format(ret))
print('set mode:', swift.set_mode(1))
ret = swift.get_mode()
print('mode: {}'.format(ret))
ret = swift.get_servo_attach(servo_id=2)
print('servo attach: {}'.format(ret))