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