def __init__(self, address: str, baud: int = 57600):
self.lock = Lock()
self.baud = baud
self.port = None
port = sdk.PortHandler(address)
self.actuators = {}
self.actuatorLock = Lock()
self.syncRegister = None
self.syncDataLen = 0
self.syncLock = Lock()
self.syncEncoder = None
self.syncReadDXLs = None
self.syncWriteCount = 0
try:
port.openPort()
except Exception as x:
raise(DXLException(str(x)))
self.port = port
try:
self.port.setBaudRate(self.baud)
except Exception as x:
raise(DXLException(str(x)))
self.packetHandler = sdk.PacketHandler(1.0)
def __init__(self, tty, deviceId):
self.ttyName = tty
self.tty = sdk.PortHandler(tty)
self.deviceId = deviceId
# Verify that the connection over the tty to the deviceId
# specified works
if self.deviceId != self.BROADCAST:
try:
self.getId()
except:
raise Exception("ERROR: Unable to communicate with the AX-12A")
def ping(device, device_id):
tty = sdk.PortHandler('/dev/tty.usbserial-FT3WFGSI')
tty.openPort()
tty.setBaudRate(1000000)
model, result, error = device.ping(tty, device_id)
if result != sdk.COMM_SUCCESS:
print("%s" % ax12a.getTxRxResult(result))
elif error != 0:
print("%s" % ax12a.getRxPacketError(error))
else:
return model
tty.closePort()
def Open(self, dev='/dev/ttyUSB0', baudrate=None, reopen=False):
set_baudrate = False
try:
#Check if dev is already opened.
info = next(value for value in self.opened if value['dev'] == dev)
info['ref'] += 1 #Increase the reference counter.
port_handler = info['port']
if all((baudrate is None, reopen, info['baudrate'] > 0)):
baudrate = info['baudrate']
#info['baudrate']= 0
set_baudrate = True
except StopIteration: #dev is not opened yet.
info = {
'port': None,
'dev': dev,
'baudrate': 0,
'locker': threading.RLock(),
'ref': 1
}
self.opened.append(info)
with info['locker']:
if info['port'] is None:
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_handler = dynamixel.PortHandler(dev)
#Open port
if port_handler.openPort():
print 'DxlPortHandler: Opened a port:', dev, port_handler
else:
print 'DxlPortHandler: Failed to open a port:', dev, port_handler
port_handler.closePort()
time.sleep(self.time_to_sleep_after_closing_port)
return None
info['port'] = port_handler
if (baudrate is not None
and info['baudrate'] != baudrate) or set_baudrate:
#Set port baudrate
if port_handler.setBaudRate(int(baudrate)):
print 'DxlPortHandler: Changed the baud rate:', dev, port_handler, baudrate
else:
print 'DxlPortHandler: Failed to change the baud rate to:', dev, port_handler, baudrate
return None
info['baudrate'] = baudrate
return port_handler
def __init__(self, device_name, verbose=False):
super().__init__(device_name)
self.__verbose = verbose
self.__port_handler = dynamixel_sdk.PortHandler(self.__device_name)
self.__packet_handler = dynamixel_sdk.PacketHandler(PROTOCOL_VERSION)
if not self.__port_handler.openPort():
print('Connection failed.')
sys.exit(-1)
self.__port_handler.setBaudRate(BAUDRATE)
for id in [ID_AZ, ID_EL]:
self.write_4_byte(id, ADDR_PRO_PROFILE_VELOCITY, VELOCITY)
self.write_2_byte(id, ADDR_PRO_POSITION_P_GAIN, POSITION_P_GAIN)
self.write_2_byte(id, ADDR_PRO_POSITION_I_GAIN, POSITION_I_GAIN)
self.write_2_byte(id, ADDR_PRO_POSITION_D_GAIN, POSITION_D_GAIN)
self.write_1_byte(id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
def Open(self, dev='/dev/ttyUSB2', baudrate=None, reopen=False):
set_baudrate = False
try:
#Check if dev is already opened.
info = next(value for value in self.opened if value['dev'] == dev)
port_handler = info['port']
if all((baudrate is None, reopen, info['baudrate'] > 0)):
baudrate = info['baudrate']
#info['baudrate']= 0
set_baudrate = True
except StopIteration:
info = {'port': None, 'dev': dev, 'baudrate': 0}
self.opened.append(info)
if info['port'] is None:
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_handler = dynamixel.PortHandler(dev)
#Open port
if port_handler.openPort():
print('Opened a port:', dev, port_handler)
else:
print('Failed to open a port:', dev, port_handler)
port_handler.closePort()
return None
info['port'] = port_handler
if (baudrate is not None
and info['baudrate'] != baudrate) or set_baudrate:
#Set port baudrate
if port_handler.setBaudRate(int(baudrate)):
print('Changed the baud rate:', dev, port_handler, baudrate)
else:
print('Failed to change the baud rate to:', dev, port_handler,
baudrate)
return None
info['baudrate'] = baudrate
return port_handler
class GripingRobot(threading.Thread):
g_Trgt_Pos_IRR1_1 = 0
g_Trgt_Pos_IRR1_2 = 0
g_Crrt_Pos_IRR1_1 = 0
g_Crrt_Pos_IRR1_2 = 0
g_bfr_Pos_IRR1_1 = 0
g_bfr_Pos_IRR1_2 = 0
g_flgComm_IRR = False
g_flgGrip_IRR = False
g_flgRls_IRR = False
g_flgForce_IRR = False
g_flgGripCnt = 0
g_Crrt_State_IRR1 = 0
g_flgComm_Move_IRR = False
g_cnt = 0
# Control table address
ADDR_PRO_TORQUE_ENABLE = 562 # Control table address is different in Dynamixel model
ADDR_PRO_GOAL_POSITION = 596
ADDR_PRO_PRESENT_POSITION = 611
g_flgComm_DXL = False
g_flgMove_DXL = False
g_flgServo_DXL = False
g_flgForce_DXL = False
# Protocol version
PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel
# Default setting
DXL_ID_Axis1 = 1 # Dynamixel ID : 1
DXL_ID_Axis2 = 2 # Dynamixel ID : 1
BAUDRATE = 57600 # Dynamixel default baudrate : 57600
DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MINIMUM_POSITION_VALUE = 10 # Dynamixel will rotate between this value
DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
Axis1_TarPos = int(0)
Axis2_TarPos = int(0)
Axis1_CurPos = int(0)
Axis2_CurPos = int(0)
DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold
portHandler = dynamixel_sdk.PortHandler(DEVICENAME)
packetHandler = dynamixel_sdk.PacketHandler(PROTOCOL_VERSION)
#win = Tkinter.Tk()
#frm = Tkinter.Frame(win, width=100, height=1000)
#frm.pack()
#varText1 = Tkinter.StringVar()
#varText2 = Tkinter.StringVar()
#varText1.set('hello')
#varText2.set('hello')
#TWindow1 = Tkinter.Label(win, textvariable=varText1)
#TWindow1.pack()
#TWindow2 = Tkinter.Label(win, textvariable=varText2)
#TWindow2.pack()
Tick = 10
"""
Griper Function
"""
#def PortOpenGriper(self):
# # Port open
# try:
# # !!KSH!![001]
# rst = PythonLibMightyZap.OpenMightyZap("COM7", 57600)
# # Error handling : serial open
# except serial.serialutil.SerialException:
# print("IRR Device RS485 - Connection Fail")
# self.g_flgComm_IRR = False
# # OK
# else:
# print("Connection")
# self.g_flgComm_IRR = True
def moveGriper(self):
if self.g_flgComm_DXL:
self.g_flgComm_Move_IRR = True
# Grip
if self.g_Crrt_Pos_IRR1_1 < self.g_Trgt_Pos_IRR1_1 and self.g_Crrt_Pos_IRR1_2 < self.g_Trgt_Pos_IRR1_2:
self.g_flgForce_IRR = True
self.g_flgGripCnt = 0
# Realese
else:
self.g_flgForce_IRR = False
self.g_flgGripCnt = 0
#def PortCloseGriper(self):
# if self.g_flgComm_IRR:
# PythonLibMightyZap.CloseMightyZap()
"""
Robot Function
"""
# Open Port
def PortOpenDXL(self):
if self.portHandler.openPort():
print("Succeeded to Open the Port")
self.g_flgComm_DXL = True
else:
print("Failed to Open the Port")
self.g_flgComm_DXL = False
# Set port baudrate
if self.portHandler.setBaudRate(self.BAUDRATE) and self.g_flgComm_DXL:
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
self.g_flgComm_DXL = False
# Close Port
def PortCloseDXL(self):
if self.g_flgComm_DXL:
self.portHandler.closePort()
def ErrorPrint(self, comm_result, error, AxisID):
if comm_result != dynamixel_sdk.COMM_SUCCESS:
print("AsisID ", AxisID,
": %s" % self.packetHandler.getTxRxResult(comm_result))
return False
elif error != 0:
print("AsisID ", AxisID,
": %s" % self.packetHandler.getRxPacketError(error))
return False
else:
return True
def DXL_TrqEnable(self):
if self.g_flgComm_DXL:
self.g_flgServo_DXL = True
self.g_flgForce_DXL = True
def DXL_TrqDisable(self):
if self.g_flgComm_DXL:
self.g_flgServo_DXL = True
self.g_flgForce_DXL = False
def run(self):
# init
# print("Start")
# self.stime = time.time()
self.g_flgComm_Move_IRR = False
# initial
if self.g_flgComm_DXL:
self.g_Crrt_Pos_IRR1_1 = PythonLibMightyZap.presentPosition(
self.portHandler, 0)
self.g_Crrt_Pos_IRR1_2 = PythonLibMightyZap.presentPosition(
self.portHandler, 4)
self.g_Trgt_Pos_IRR1_1 = self.g_Crrt_Pos_IRR1_1
self.g_Trgt_Pos_IRR1_2 = self.g_Crrt_Pos_IRR1_2
# print(g_Crrt_Pos_IRR1_1,g_Crrt_Pos_IRR1_2)41
if self.g_flgComm_DXL:
tmp, dxl_comm_result, dxl_error \
= self.packetHandler.read4ByteTxRx(self.portHandler,
self.DXL_ID_Axis1,
self.ADDR_PRO_PRESENT_POSITION)
if tmp & 0x80000000:
tmp = -(((~tmp) & 0xffffffff) + 1)
self.Axis1_CurPos = tmp
tmp, dxl_comm_result, dxl_error \
= self.packetHandler.read4ByteTxRx(self.portHandler,
self.DXL_ID_Axis2,
self.ADDR_PRO_PRESENT_POSITION)
if tmp & 0x80000000:
tmp = -(((~tmp) & 0xffffffff) + 1)
self.Axis2_CurPos = tmp
self.Axis1_TarPos = self.Axis1_CurPos
self.Axis2_TarPos = self.Axis2_CurPos
time.sleep(0.2)
while True:
# Update : position
if self.g_flgComm_DXL:
tmp, dxl_comm_result, dxl_error \
= self.packetHandler.read4ByteTxRx(self.portHandler,
self.DXL_ID_Axis1,
self.ADDR_PRO_PRESENT_POSITION)
if tmp & 0x80000000:
tmp = -(((~tmp) & 0xffffffff) + 1)
self.Axis1_CurPos = tmp
tmp, dxl_comm_result, dxl_error \
= self.packetHandler.read4ByteTxRx(self.portHandler,
self.DXL_ID_Axis2,
self.ADDR_PRO_PRESENT_POSITION)
if tmp & 0x80000000:
tmp = -(((~tmp) & 0xffffffff) + 1)
self.Axis2_CurPos = tmp
# Excution command : Move
if self.g_flgMove_DXL:
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, self.DXL_ID_Axis1,
self.ADDR_PRO_GOAL_POSITION, self.Axis1_TarPos)
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, self.DXL_ID_Axis2,
self.ADDR_PRO_GOAL_POSITION, self.Axis2_TarPos)
self.g_flgMove_DXL = False
# Excution command : Survo On/Off
if self.g_flgServo_DXL:
self.g_flgServo_DXL = 0
if self.g_flgForce_DXL:
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, self.DXL_ID_Axis1,
self.ADDR_PRO_TORQUE_ENABLE, self.TORQUE_ENABLE)
if self.ErrorPrint(dxl_comm_result, dxl_error,
self.DXL_ID_Axis1):
print("Axis1 : Torque [On]")
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, self.DXL_ID_Axis2,
self.ADDR_PRO_TORQUE_ENABLE, self.TORQUE_ENABLE)
if self.ErrorPrint(dxl_comm_result, dxl_error,
self.DXL_ID_Axis2):
print("Axis2 : Torque [On]")
else:
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, self.DXL_ID_Axis1,
self.ADDR_PRO_TORQUE_ENABLE, self.TORQUE_DISABLE)
if self.ErrorPrint(dxl_comm_result, dxl_error,
self.DXL_ID_Axis1):
print("Axis1 : Torque [Off]")
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, self.DXL_ID_Axis2,
self.ADDR_PRO_TORQUE_ENABLE, self.TORQUE_DISABLE)
if self.ErrorPrint(dxl_comm_result, dxl_error,
self.DXL_ID_Axis1):
print("Axis2 : Torque [Off]")
if self.g_flgComm_DXL:
# === Get Position
self.g_Crrt_Pos_IRR1_1 = PythonLibMightyZap.presentPosition(
self.portHandler, 0)
self.g_Crrt_Pos_IRR1_2 = PythonLibMightyZap.presentPosition(
self.portHandler, 4)
# self.stime = time.time()
# === Get State
# None
# === Get Error
# None
# === Excute Command ===
# 1. Move position
if self.g_flgComm_Move_IRR:
PythonLibMightyZap.goalPosition(self.portHandler, 0,
self.g_Trgt_Pos_IRR1_1)
PythonLibMightyZap.goalPosition(self.portHandler, 4,
self.g_Trgt_Pos_IRR1_2)
self.g_flgComm_Move_IRR = False
# 2. Grip_End
elif self.g_flgForce_IRR:
if self.g_flgGripCnt > 2:
if (abs(self.g_Crrt_Pos_IRR1_1 - self.g_bfr_Pos_IRR1_1)
< 40 or abs(self.g_Crrt_Pos_IRR1_2 -
self.g_bfr_Pos_IRR1_2) < 40):
self.g_flgForce_IRR = False
PythonLibMightyZap.forceEnable(
self.portHandler, 0, 0)
PythonLibMightyZap.forceEnable(
self.portHandler, 4, 0)
self.g_Trgt_Pos_IRR1_1 = self.g_Crrt_Pos_IRR1_1
self.g_Trgt_Pos_IRR1_2 = self.g_Crrt_Pos_IRR1_2
PythonLibMightyZap.goalPosition(
self.portHandler, 0, self.g_Trgt_Pos_IRR1_1)
PythonLibMightyZap.goalPosition(
self.portHandler, 4, self.g_Trgt_Pos_IRR1_2)
self.g_flgGripCnt = 0
print(
"!!F-OFF!! Vel1: [%05d]" %
(self.g_Crrt_Pos_IRR1_1 -
self.g_bfr_Pos_IRR1_1),
"Vel2: [%05d]" % (self.g_Crrt_Pos_IRR1_2 -
self.g_bfr_Pos_IRR1_2),
"Pos1 : [%05d" % self.g_Crrt_Pos_IRR1_1,
"/%05d]" % self.g_Trgt_Pos_IRR1_1,
"Pos2 : [%05d/" % self.g_Crrt_Pos_IRR1_2,
"%05d]" % self.g_Trgt_Pos_IRR1_2)
else:
self.g_flgGripCnt += 1
# 3. print
if abs(self.g_Trgt_Pos_IRR1_1 - self.g_Crrt_Pos_IRR1_1
) > 10 and abs(self.g_Trgt_Pos_IRR1_2 -
self.g_Crrt_Pos_IRR1_2) > 10:
print(
"Vel1: [%05d]" %
(self.g_Crrt_Pos_IRR1_1 - self.g_bfr_Pos_IRR1_1),
"Vel2: [%05d]" %
(self.g_Crrt_Pos_IRR1_2 - self.g_bfr_Pos_IRR1_2),
"Pos1 : [%05d" % self.g_Crrt_Pos_IRR1_1,
"/%05d]" % self.g_Trgt_Pos_IRR1_1,
"Pos2 : [%05d/" % self.g_Crrt_Pos_IRR1_2,
"%05d]" % self.g_Trgt_Pos_IRR1_2)
if self.g_flgGrip_IRR:
if self.g_Crrt_Pos_IRR1_1 >= 1400 and self.g_Crrt_Pos_IRR1_2 >= 1400:
PythonLibMightyZap.goalPosition(
self.portHandler, 0, 1400)
PythonLibMightyZap.goalPosition(
self.portHandler, 4, 1400)
self.g_flgGrip_IRR = 0
elif abs(self.g_Trgt_Pos_IRR1_1 - self.g_Crrt_Pos_IRR1_1
) < 50 and abs(self.g_Trgt_Pos_IRR1_2 -
self.g_Crrt_Pos_IRR1_2) < 50:
self.g_Trgt_Pos_IRR1_1 = self.g_Crrt_Pos_IRR1_1 + 200
self.g_Trgt_Pos_IRR1_2 = self.g_Crrt_Pos_IRR1_2 + 200
PythonLibMightyZap.goalPosition(
self.portHandler, 0, self.g_Trgt_Pos_IRR1_1)
PythonLibMightyZap.goalPosition(
self.portHandler, 4, self.g_Trgt_Pos_IRR1_2)
self.g_bfr_Pos_IRR1_1 = self.g_Crrt_Pos_IRR1_1
self.g_bfr_Pos_IRR1_2 = self.g_Crrt_Pos_IRR1_2
# 2. Force Off Function
if g_Cmd == 0:
# if ...
# Update Time : 10 ms
self.g_cnt += 1
time.sleep(0.01)
# UI Update
'''
if(self.Tick == 10):
self.varText1.set("Pos1 : "+ str((self.Axis1_CurPos)) + "/" +str((self.Axis1_TarPos)))
self.varText2.set("Pos2 : "+ str((self.Axis2_CurPos)) + "/" +str((self.Axis2_TarPos)))
self.TWindow1.update()
self.TWindow2.update()
self.Tick = 0
self.Tick += 1
'''
def moveDXL(self):
self.g_flgMove_DXL = True
import dynamixel_sdk as dxl
from sensor_msgs.msg import JointState
from assembly_dxl_gripper import *
from assembly_dxl_gripper.srv import *
from threading import Lock
import math
if __name__ == '__main__':
rospy.init_node('gripper_server')
lock = Lock()
init_pos = {}
pos = {}
vel = {}
portHandler = dxl.PortHandler(DEVICENAME)
packetHandler = dxl.PacketHandler(PROTOCOL_VERSION)
groupSyncWrite = dxl.GroupSyncWrite(portHandler, packetHandler, ADDR_GOAL_POSITION, LEN_GOAL_POSITION)
groupSyncRead = dxl.GroupSyncRead(portHandler, packetHandler, ADDR_PRESENT_VELOCITY, LEN_PRESENT_VELOCITY+LEN_PRESENT_POSITION)
for arm in hand_name_map:
for key in hand_name_map[arm]:
init_pos[key] = rospy.get_param('/assembly_dxl_gripper/' + key + '_init_pos')
groupSyncRead.addParam(dxl_id_map[key])
# Open port
try: portHandler.clearPort()
except: pass
try: portHandler.closePort()
except: pass
if portHandler.openPort(): print("Succeeded to open the port")
def open_tty(name, baudrate):
tty = sdk.PortHandler(name)
tty.openPort()
tty.setBaudRate(baudrate)
return tty
class GripingRobot(threading.Thread):
g_Trgt_Pos_IRR1_1 = 0
g_Trgt_Pos_IRR1_2 = 0
g_Crrt_Pos_IRR1_1 = 0
g_Crrt_Pos_IRR1_2 = 0
g_bfr_Pos_IRR1_1 = 0
g_bfr_Pos_IRR1_2 = 0
g_flgComm_IRR = False
g_flgGrip_IRR = False
g_flgRls_IRR = False
g_flgForce_IRR = False
g_flgGripCnt = 0
g_Crrt_State_IRR1 = 0
g_flgComm_Move_IRR = False
g_cnt = 0
# Control table address
ADDR_PRO_TORQUE_ENABLE = 562 # Control table address is different in Dynamixel model
ADDR_PRO_GOAL_POSITION = 596
ADDR_PRO_PRESENT_POSITION = 611
g_flgComm_DXL = False
g_flgMove_DXL = False
g_flgServo_DXL = False
g_flgForce_DXL = False
# Protocol version
PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel
# Default setting
DXL_ID_Axis1 = 1 # Dynamixel ID : 1
DXL_ID_Axis2 = 2 # Dynamixel ID : 1
BAUDRATE = 57600 # Dynamixel default baudrate : 57600
DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MINIMUM_POSITION_VALUE = 10 # Dynamixel will rotate between this value
DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
Axis1_TarPos = int(0)
Axis2_TarPos = int(0)
Axis1_CurPos = int(0)
Axis2_CurPos = int(0)
DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold
portHandler = dynamixel_sdk.PortHandler(DEVICENAME)
packetHandler = dynamixel_sdk.PacketHandler(PROTOCOL_VERSION)
Tick = 10
def moveGriper(self):
if self.g_flgComm_DXL:
self.g_flgComm_Move_IRR = True
# Grip
if self.g_Crrt_Pos_IRR1_1 < self.g_Trgt_Pos_IRR1_1 and self.g_Crrt_Pos_IRR1_2 < self.g_Trgt_Pos_IRR1_2:
self.g_flgForce_IRR = True
self.g_flgGripCnt = 0
# Realese
else:
self.g_flgForce_IRR = False
self.g_flgGripCnt = 0
def PortOpenDXL(self):
if self.portHandler.openPort():
print("Succeeded to Open the Port")
self.g_flgComm_DXL = True
else:
print("Failed to Open the Port")
self.g_flgComm_DXL = False
# Set port baudrate
if self.portHandler.setBaudRate(self.BAUDRATE) and self.g_flgComm_DXL:
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
self.g_flgComm_DXL = False
# Close Port
def PortCloseDXL(self):
if self.g_flgComm_DXL:
self.portHandler.closePort()
def ErrorPrint(self, comm_result, error, AxisID):
if comm_result != dynamixel_sdk.COMM_SUCCESS:
print("AsisID ", AxisID,
": %s" % self.packetHandler.getTxRxResult(comm_result))
return False
elif error != 0:
print("AsisID ", AxisID,
": %s" % self.packetHandler.getRxPacketError(error))
return False
else:
return True
def DXL_TrqEnable(self):
if self.g_flgComm_DXL:
self.g_flgServo_DXL = True
self.g_flgForce_DXL = True
def DXL_TrqDisable(self):
if self.g_flgComm_DXL:
self.g_flgServo_DXL = True
self.g_flgForce_DXL = False
def run(self):
# init
# print("Start")
# self.stime = time.time()
self.g_flgComm_Move_IRR = False
# initial
if self.g_flgComm_DXL:
self.g_Crrt_Pos_IRR1_1 = PythonLibMightyZap.presentPosition(
self.portHandler, 0)
self.g_Crrt_Pos_IRR1_2 = PythonLibMightyZap.presentPosition(
self.portHandler, 4)
self.g_Trgt_Pos_IRR1_1 = self.g_Crrt_Pos_IRR1_1
self.g_Trgt_Pos_IRR1_2 = self.g_Crrt_Pos_IRR1_2
# print(g_Crrt_Pos_IRR1_1,g_Crrt_Pos_IRR1_2)41
if self.g_flgComm_DXL:
tmp, dxl_comm_result, dxl_error \
= self.packetHandler.read4ByteTxRx(self.portHandler, self.DXL_ID_Axis1, self.ADDR_PRO_PRESENT_POSITION)
if tmp & 0x80000000:
tmp = -(((~tmp) & 0xffffffff) + 1)
self.Axis1_CurPos = tmp
tmp, dxl_comm_result, dxl_error \
= self.packetHandler.read4ByteTxRx(self.portHandler, self.DXL_ID_Axis2, self.ADDR_PRO_PRESENT_POSITION)
if tmp & 0x80000000:
tmp = -(((~tmp) & 0xffffffff) + 1)
self.Axis2_CurPos = tmp
self.Axis1_TarPos = self.Axis1_CurPos
self.Axis2_TarPos = self.Axis2_CurPos
time.sleep(0.2)
while True:
# Update : position
if self.g_flgComm_DXL:
tmp, dxl_comm_result, dxl_error \
= self.packetHandler.read4ByteTxRx(self.portHandler, self.DXL_ID_Axis1,self.ADDR_PRO_PRESENT_POSITION)
if tmp & 0x80000000:
tmp = -(((~tmp) & 0xffffffff) + 1)
self.Axis1_CurPos = tmp
tmp, dxl_comm_result, dxl_error \
= self.packetHandler.read4ByteTxRx(self.portHandler, self.DXL_ID_Axis2,self.ADDR_PRO_PRESENT_POSITION)
if tmp & 0x80000000:
tmp = -(((~tmp) & 0xffffffff) + 1)
self.Axis2_CurPos = tmp
# Excution command : Move
if self.g_flgMove_DXL:
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, self.DXL_ID_Axis1,
self.ADDR_PRO_GOAL_POSITION, self.Axis1_TarPos)
dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
self.portHandler, self.DXL_ID_Axis2,
self.ADDR_PRO_GOAL_POSITION, self.Axis2_TarPos)
self.g_flgMove_DXL = False
# Excution command : Survo On/Off
if self.g_flgServo_DXL:
self.g_flgServo_DXL = 0
if self.g_flgForce_DXL:
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, self.DXL_ID_Axis1,
self.ADDR_PRO_TORQUE_ENABLE, self.TORQUE_ENABLE)
if self.ErrorPrint(dxl_comm_result, dxl_error,
self.DXL_ID_Axis1):
print("Axis1 : Torque [On]")
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, self.DXL_ID_Axis2,
self.ADDR_PRO_TORQUE_ENABLE, self.TORQUE_ENABLE)
if self.ErrorPrint(dxl_comm_result, dxl_error,
self.DXL_ID_Axis2):
print("Axis2 : Torque [On]")
else:
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, self.DXL_ID_Axis1,
self.ADDR_PRO_TORQUE_ENABLE, self.TORQUE_DISABLE)
if self.ErrorPrint(dxl_comm_result, dxl_error,
self.DXL_ID_Axis1):
print("Axis1 : Torque [Off]")
dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
self.portHandler, self.DXL_ID_Axis2,
self.ADDR_PRO_TORQUE_ENABLE, self.TORQUE_DISABLE)
if self.ErrorPrint(dxl_comm_result, dxl_error,
self.DXL_ID_Axis1):
print("Axis2 : Torque [Off]")
if self.g_flgComm_DXL:
# === Get Position
self.g_Crrt_Pos_IRR1_1 = PythonLibMightyZap.presentPosition(
self.portHandler, 0)
self.g_Crrt_Pos_IRR1_2 = PythonLibMightyZap.presentPosition(
self.portHandler, 4)
# self.stime = time.time()
# === Get State
# None
# === Get Error
# None
# === Excute Command ===
# 1. Move position
if self.g_flgComm_Move_IRR:
PythonLibMightyZap.goalPosition(self.portHandler, 0,
self.g_Trgt_Pos_IRR1_1)
PythonLibMightyZap.goalPosition(self.portHandler, 4,
self.g_Trgt_Pos_IRR1_2)
self.g_flgComm_Move_IRR = False
# 2. Grip_End
elif self.g_flgForce_IRR:
if self.g_flgGripCnt > 2:
if (abs(self.g_Crrt_Pos_IRR1_1 - self.g_bfr_Pos_IRR1_1)
< 40 or abs(self.g_Crrt_Pos_IRR1_2 -
self.g_bfr_Pos_IRR1_2) < 40):
self.g_flgForce_IRR = False
PythonLibMightyZap.forceEnable(
self.portHandler, 0, 0)
PythonLibMightyZap.forceEnable(
self.portHandler, 4, 0)
self.g_Trgt_Pos_IRR1_1 = self.g_Crrt_Pos_IRR1_1
self.g_Trgt_Pos_IRR1_2 = self.g_Crrt_Pos_IRR1_2
PythonLibMightyZap.goalPosition(
self.portHandler, 0, self.g_Trgt_Pos_IRR1_1)
PythonLibMightyZap.goalPosition(
self.portHandler, 4, self.g_Trgt_Pos_IRR1_2)
self.g_flgGripCnt = 0
# print("!!F-OFF!! Vel1: [%05d]"%(self.g_Crrt_Pos_IRR1_1 - self.g_bfr_Pos_IRR1_1), "Vel2: [%05d]"%(self.g_Crrt_Pos_IRR1_2 - self.g_bfr_Pos_IRR1_2),"Pos1 : [%05d"%self.g_Crrt_Pos_IRR1_1 ,"/%05d]"%self.g_Trgt_Pos_IRR1_1, "Pos2 : [%05d/"%self.g_Crrt_Pos_IRR1_2, "%05d]"%self.g_Trgt_Pos_IRR1_2)
else:
self.g_flgGripCnt += 1
# 3. print
if abs(self.g_Trgt_Pos_IRR1_1 - self.g_Crrt_Pos_IRR1_1
) > 10 and abs(self.g_Trgt_Pos_IRR1_2 -
self.g_Crrt_Pos_IRR1_2) > 10:
pass
# print("Vel1: [%05d]"%(self.g_Crrt_Pos_IRR1_1 - self.g_bfr_Pos_IRR1_1), "Vel2: [%05d]"%(self.g_Crrt_Pos_IRR1_2 - self.g_bfr_Pos_IRR1_2),"Pos1 : [%05d"%self.g_Crrt_Pos_IRR1_1 ,"/%05d]"%self.g_Trgt_Pos_IRR1_1, "Pos2 : [%05d/"%self.g_Crrt_Pos_IRR1_2, "%05d]"%self.g_Trgt_Pos_IRR1_2)
if self.g_flgGrip_IRR:
if self.g_Crrt_Pos_IRR1_1 >= 1400 and self.g_Crrt_Pos_IRR1_2 >= 1400:
PythonLibMightyZap.goalPosition(
self.portHandler, 0, 1400)
PythonLibMightyZap.goalPosition(
self.portHandler, 4, 1400)
self.g_flgGrip_IRR = 0
elif abs(self.g_Trgt_Pos_IRR1_1 - self.g_Crrt_Pos_IRR1_1
) < 50 and abs(self.g_Trgt_Pos_IRR1_2 -
self.g_Crrt_Pos_IRR1_2) < 50:
self.g_Trgt_Pos_IRR1_1 = self.g_Crrt_Pos_IRR1_1 + 200
self.g_Trgt_Pos_IRR1_2 = self.g_Crrt_Pos_IRR1_2 + 200
PythonLibMightyZap.goalPosition(
self.portHandler, 0, self.g_Trgt_Pos_IRR1_1)
PythonLibMightyZap.goalPosition(
self.portHandler, 4, self.g_Trgt_Pos_IRR1_2)
self.g_bfr_Pos_IRR1_1 = self.g_Crrt_Pos_IRR1_1
self.g_bfr_Pos_IRR1_2 = self.g_Crrt_Pos_IRR1_2
# 2. Force Off Function
def moveDXL(self):
self.g_flgMove_DXL = True
def __init__(self,
baudrate=1000000,
port=None,
protocol=1,
top=256,
verbose=True):
# load the JSON file and convert keys from strings to ints
with open("dynamixel.json", 'r') as f:
self.modeldict = {}
for key, value in json.load(f).items():
self.modeldict[int(key)] = value
# setup the serial port
# Initialize PortHandler instance
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
self.port = sdk.PortHandler(port)
self.verbose = verbose
# Open port
if self.port.openPort():
print("Succeeded to open the port")
else:
raise RuntimeError("Failed to open the port")
# Set port baudrate
if self.port.setBaudRate(baudrate):
print("Succeeded to change the baudrate")
else:
raise RuntimeError("Failed to change the baudrate")
# Initialize PacketHandler instance
# Set the protocol version
# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
if protocol == 1:
self.packetHandler = sdk.PacketHandler("1.0")
else:
self.packetHandler = sdk.PacketHandler("2.0")
# X-series memory map
self.address = {}
self.address['model'] = (2, 4)
self.address['firmware'] = (6, 1)
self.address['return_delay'] = (9, 1)
self.address['drivemode'] = (10, 1, self._drivemode_handler)
self.address['opmode'] = (11, 1, self._opmode_handler)
self.address['shadow_id'] = (12, 1)
self.address['min_position'] = (52, 4)
self.address['max_position'] = (48, 4)
self.address['velocity_limit'] = (44, 4)
self.address['torque_enable'] = (64, 1)
self.address['hardware_error_status'] = (70, 1)
self.address['goal_position'] = (116, 4)
self.address['goal_velocity'] = (104, 4)
self.address['goal_pwm'] = (100, 2)
self.address['present_position'] = (132, 4)
self.address['present_position2'] = (132, 2)
self.address['present_current'] = (126, 2)
self.address['present_velocity'] = (128, 2)
self.address['profile_velocity'] = (112, 4)
self.address['profile_acceleration'] = (108, 4)
self.address['temp'] = (146, 1)
self.address['led'] = (65, 1)
self.idlist = []
self.models = {}
# poll the ports
for id in range(0, top):
model = self.ping(id)
if model > 0:
self.idlist.append(id)
self.models[id] = model
self.set(id, 'torque_enable', False)
self.set(id, 'led', False)
print('servos found', self.idlist)
error = self.get('all', 'hardware_error_status')
if any(error):
print('hardware error', error)
def __init__(self, port: str, baudrate: int = 1_000_000):
self.port = port
self.port_handler = sdk.PortHandler(port)
self.baudrate = baudrate
self.opened = False
def __init__(self, preset_file, verbosity='minimal'):
"""Inits
Args:
preset_file: The path of the \'<preset>.json\' file
verbosity: 'quiet' or 'minimal' or 'detailed'
Raises:
RuntimeError: If some motor has no ID
"""
# Verbose
assert_verbosity(verbosity)
self.verbosity = _verbose_level[verbosity]
# Load preset
with open(preset_file, 'r') as f:
preset = json.load(f, object_hook=byteify)
############################################
# Port Handlers
############################################
# The 'PortHandler' requires 'device name' for initialization.
# And the device name is different for each OS.
# windows: "COM1"
# linux: "/dev/ttyUSB0"
# mac: "/dev/tty.usbserial-*"
self.port_handlers = {}
# e.g.
# {
# "/dev/ttyUSB0": {
# "handler": PortHandler,
# "baud rate": None
# }
# Open port
for name in preset['ports']:
self.__check_type_n_dupe('port', str, name, self.port_handlers)
port_handler = dxlsdk.PortHandler(name)
try:
port_handler.openPort()
except Exception as inst:
print("Helper: [ERROR] " + inst.__str__())
raise inst
else:
self.port_handlers[name] = {'handler': port_handler,
'baud rate': None}
if self.verbosity >= _verbose_level['detailed']:
print("Helper: Succeeded to open the port: \""+name+"\"")
############################################
# Packet Handlers
############################################
# The 'PacketHandler' requires 'protocol version' for initialization.
# KEY: 'protocol version', VALUE: 'packet_handler'
# "auto" keywords
if isinstance(preset["protocol versions"], list):
# Duplicate cleaning
protocol_set = set(preset["protocol versions"])
elif preset["protocol versions"] == "auto":
protocol_set = [1.0, 2.0]
else:
print("Helper: [ERROR] There is a typo in the protocol of the preset file.")
raise NotImplementedError
self.packet_handlers = {
version: dxlsdk.PacketHandler(version) for version in protocol_set}
############################################
# Motor
############################################
# Motor List
id_set = set()
alias_set = set()
motor_list = []
for motor in preset['motors']:
# ID Validation
try:
self.__check_type_n_dupe('motor.id', int, motor['id'], id_set)
except KeyError:
print("Helper: [ERROR] One motor has no ID.")
print(" Please check again: "+preset_file)
raise RuntimeError
else:
id_set.add(motor['id'])
# Alias Validation
try:
if motor['alias']:
self.__check_type_n_dupe('motor.alias', str, motor['alias'], alias_set)
alias_set.add(motor['alias'])
else:
motor['alias'] = None
except KeyError:
motor.update({'alias': None})
# Clean list
motor_list.append(motor)
# {
# "id": 1,
# "alias": "joint_L1",
# "model": "XM430-W210"
# }
# KEY: 'robotID' or 'alias', VALUE: 'DxlMotor'
self.__motors = {}
logging = {'O': [], 'X': []}
for motor in motor_list:
motor_log = [motor['id'], motor['alias'], motor['model']]
try:
motorInstance = DxlMotor(
motor['id'], motor['alias'], motor['model'],
self.port_handlers, preset['baud rates'],
self.packet_handlers, verbosity=verbosity)
except Exception as inst: # to catch all errors
logging['X'].append(motor_log)
if self.verbosity >= _verbose_level['detailed']:
print("Helper: One motor setup was skipped.")
print(" The reason is: \""+inst.__str__()+"\"")
raise inst
else:
logging['O'].append(motor_log)
self.__motors[motor['id']] = motorInstance
if motor['alias']:
self.__motors[motor['alias']] = motorInstance
if self.verbosity >= _verbose_level['detailed']:
print("Helper: One motor setup was completed.")
############################################
# Log
############################################
if self.verbosity >= _verbose_level['minimal']:
print("Helper: All motor setups have been completed.")
print(" Success: {} motor(s) / Fail: {} motor(s)"
.format(len(logging['O']), len(logging['X'])))
for ox, logs in logging.items():
logs.sort()
for log in logs:
print(" ({}) id:{}, alias:{}, model:{}"
.format(ox, log[0], log[1], log[2]))
