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,
conn: Connection,
initial: List[Motor] = [],
protocol_v: float = 1.0):
self.homogenous = True
self.motors = initial
self._update_homogenous()
self.conn = conn
self.packet_handler = sdk.PacketHandler(protocol_v)
def Setup(self):
DxlPortHandler.Open(dev=self.DevName, baudrate=self.Baudrate)
if self.port_handler() is None:
return False
# Initialize PacketHandler
self.packet_handler= dynamixel.PacketHandler(self.PROTOCOL_VERSION)
self.WriteFuncs= (None, self.packet_handler.write1ByteTxRx, self.packet_handler.write2ByteTxRx, None, self.packet_handler.write4ByteTxRx)
self.ReadFuncs= (None, self.packet_handler.read1ByteTxRx, self.packet_handler.read2ByteTxRx, None, self.packet_handler.read4ByteTxRx)
self.SetOpMode(self.OP_MODE[self.OpMode])
#self.EnableTorque()
return True
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)
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
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")
else: print("Failed to open the port")
class AX12A:
BROADCAST = 254
ttyName = None
tty = None
deviceId = None
protocol = sdk.PacketHandler(1.0)
#==========================================================================
# EEPROM Control Area (durable)
#==========================================================================
# Constructor - it attempts to communicate with the AX-12A over
# the tty at the specified deviceId by invoking getId(), if that
# fails, an exception is thrown.
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")
# This address stores model number of DYNAMIXEL.
#
# Control Table 0
# Read Only
def getModelNumber(self):
return self.__read2Bytes(FIELD_MODEL_NUMBER)
# This address stores firmware version of DYNAMIXEL.
#
# Control Table 2
# Read Only
def getFirmwareVersion(self):
return self.__read1Byte(FIELD_FIRMWARE_VERSION)
# The ID is a unique value in the network to identify each
# DYNAMIXEL with an Instruction Packet.
#
# 0~252 (0xFC) values can be used as an ID, and
#
# 254 (0xFE) is occupied as a broadcast ID. The Broadcast ID
# (254, 0xFE) can send an Instruction Packet to all connected
# DYNAMIXEL simultaneously.
#
# Control Table 3
def getId(self):
return self.__read1Byte(FIELD_ID)
def setId(self, value):
self.__write1Byte(FIELD_ID, value)
self.deviceId = value
# Baud Rate determines serial communication speed between a
# controller and DYNAMIXEL’s.
#
# Control Table 4
def getBaudRate(self):
return self.__read1Byte(FIELD_BAUD_RATE)
def setBaudRate(self, value):
self.__write1Byte(FIELD_BAUD_RATE, value)
# If the DYNAMIXEL receives an Instruction Packet, it will return
# the Status Packet after the time of the set Return Delay
# Time(5).
#
# Note that the range of values is 0 to 254 (0XFE) and its unit is
# 2 [μsec]. For instance, if the Return Delay Time(5) is set to
# ‘10’, the Status Packet will be returned after 20[μsec] when the
# Instruction Packet is received.
#
# Control Table 5
def getReturnDelayTime(self):
return self.__read1Byte(FIELD_RETURN_DELAY_TIME)
def setReturnDelayTime(self, value):
self.__write1Byte(FIELD_RETURN_DELAY_TIME, value)
# The angle limit allows the motion to be restrained. The range
# and the unit of the value is the same as Goal Position(30).
#
# CW Angle Limit: the minimum value of Goal Position(30)
#
# CCW Angle Limit: the maximum value of Goal Position(30)
#
# The following two modes can be set pursuant to the value of CW
# and CCW.
#
# Wheel Mode: both are 0
# Joint Mode: neither are 0
#
# The wheel mode can be used to wheel-type operation robots since
# motors of the robots spin infinitely. The joint mode can be used
# to multi-joints robot since the robots can be controlled with
# specific angles.
#
# Control Table 6
def getCwAngleLimit(self):
return self.__read2Bytes(FIELD_CW_ANGLE_LIMIT)
def setCwAngleLimit(self, value):
self.__write2Bytes(FIELD_CW_ANGLE_LIMIT, value)
# The angle limit allows the motion to be restrained. The range
# and the unit of the value is the same as Goal Position(30).
#
# CW Angle Limit: the minimum value of Goal Position(30)
#
# CCW Angle Limit: the maximum value of Goal Position(30)
#
# The following two modes can be set pursuant to the value of CW
# and CCW.
#
# Wheel Mode: both are 0
# Joint Mode: neither are 0
#
# The wheel mode can be used to wheel-type operation robots since
# motors of the robots spin infinitely. The joint mode can be used
# to multi-joints robot since the robots can be controlled with
# specific angles.
#
# Control Table 8
def getCcwAngleLimit(self):
return self.__read2Bytes(FIELD_CCW_ANGLE_LIMIT)
def setCcwAngleLimit(self, value):
self.__write2Bytes(FIELD_CCW_ANGLE_LIMIT, value)
def setWheelMode(self):
self.setCwAngleLimit(0)
self.setCcwAngleLimit(0)
def setJointMode(self):
self.setCwAngleLimit(0)
self.setCcwAngleLimit(1023)
# About 1°C 0 ~ 99
#
# Control Table 11
def getTemperatureLimit(self):
return self.__read1Byte(FIELD_TEMPERATURE_LIMIT)
def setTemperatureLimit(self, value):
self.__write1Byte(FIELD_TEMPERATURE_LIMIT, value)
# About 0.1V 50 ~ 160 5.0 ~ 16.0V
#
# For example, if the value is 80, the voltage is 8V. If Present
# Voltage(42) is out of the range, Voltage Range Error Bit (Bit0)
# of Status Packet is returned as ‘1’ and Alarm is triggered as
# set in the addresses 17 and 18.
#
# Control Table 12
def getMinVoltageLimit(self):
return self.__read1Byte(FIELD_MIN_VOLTAGE_LIMIT)
def setMinVoltageLimit(self, value):
self.__write1Byte(FIELD_MIN_VOLTAGE_LIMIT, value)
# About 0.1V 50 ~ 160 5.0 ~ 16.0V
#
# For example, if the value is 80, the voltage is 8V. If Present
# Voltage(42) is out of the range, Voltage Range Error Bit (Bit0)
# of Status Packet is returned as ‘1’ and Alarm is triggered as
# set in the addresses 17 and 18.
#
# Control Table 13
def getMaxVoltageLimit(self):
return self.__read1Byte(FIELD_MAX_VOLTAGE_LIMIT)
def setMaxVoltageLimit(self, value):
self.__write1Byte(FIELD_MAX_VOLTAGE_LIMIT, value)
# It is the torque value of maximum output.
#
# 0 to 1,023 (0x3FF) can be used, and the unit is about 0.1%.
#
# For example, Data 1,023 (0x3FF) means that DYNAMIXEL will use
# 100% of the maximum torque it can produce while Data 512 (0x200)
# means that DYNAMIXEL will use 50% of the maximum torque. When
# the power is turned on, Torque Limit(34) uses the value as the
# initial value.
#
# Control Table 14
def getMaxTorque(self):
return self.__read2Bytes(FIELD_MAX_TORQUE)
def setMaxTorque(self, value):
self.__write2Bytes(FIELD_MAX_TORQUE, value)
# The Status Return Level(68) decides how to return Status Packet
# when DYNAMIXEL receives an Instruction Packet.
#
# 0 PING Instruction Returns the Status Packet for PING Instruction only
# 1 PING/Read Instruction Returns the Status Packet for PING and READ Instruction
# 2 All Instructions Returns the Status Packet for all Instructions
#
# Control Table 16
def getStatusReturnLevel(self):
return self.__read1Byte(FIELD_STATUS_RETURN_LEVEL)
def setStatusReturnLevel(self, value):
self.__write1Byte(FIELD_STATUS_RETURN_LEVEL, value)
# The DYNAMIXEL can protect itself by detecting dangerous
# situations that could occur during the operation. Each Bit is
# inclusively processed with the ‘OR’ logic, therefore, multiple
# options can be generated. For instance, when ‘0x05’ (binary :
# 00000101) is defined in Shutdown(18), DYNAMIXEL can detect both
# Input Voltage Error (binary : 00000001) and Overheating Error
# (binary : 00000100). If those errors are detected, Torque
# Enable(24) is cleared to ‘0’ and the motor output becomes 0 [%].
# REBOOT is the only method to reset Torque Enable(24) to ‘1’
# (Torque ON) after the shutdown. The followings are detectable
# situations.
#
# Bit 7 0 -
# Bit 6 Instruction Error Detects that undefined Instruction is transmitted or the Action command is delivered without the reg_write command
# Bit 5 Overload Error Detects that persistent load exceeds maximum output
# Bit 4 CheckSum Error Detects that the Checksum of the transmitted Instruction Packet is invalid
# Bit 3 Range Error Detects that the command is given beyond the range of usage
# Bit 2 Overheating Error Detects that the internal temperature exceeds the set temperature
# Bit 1 Angle Limit Error Detects that Goal Position is written with the value that is not between CW Angle Limit and CCW Angle Limit
# Bit 0 Input Voltage Error Detects that input voltage exceeds the configured operating voltage
#
# Control Table 17
def getAlarmLed(self):
return self.__read1Byte(FIELD_ALARM_LED)
def setAlarmLed(self, value):
self.__write1Byte(FIELD_ALARM_LED, value)
# The DYNAMIXEL can protect itself by detecting dangerous
# situations that could occur during the operation. Each Bit is
# inclusively processed with the ‘OR’ logic, therefore, multiple
# options can be generated. For instance, when ‘0x05’ (binary :
# 00000101) is defined in Shutdown(18), DYNAMIXEL can detect both
# Input Voltage Error (binary : 00000001) and Overheating Error
# (binary : 00000100). If those errors are detected, Torque
# Enable(24) is cleared to ‘0’ and the motor output becomes 0 [%].
# REBOOT is the only method to reset Torque Enable(24) to ‘1’
# (Torque ON) after the shutdown. The followings are detectable
# situations.
#
# Bit 7 0 -
# Bit 6 Instruction Error Detects that undefined Instruction is transmitted or the Action command is delivered without the reg_write command
# Bit 5 Overload Error Detects that persistent load exceeds maximum output
# Bit 4 CheckSum Error Detects that the Checksum of the transmitted Instruction Packet is invalid
# Bit 3 Range Error Detects that the command is given beyond the range of usage
# Bit 2 Overheating Error Detects that the internal temperature exceeds the set temperature
# Bit 1 Angle Limit Error Detects that Goal Position is written with the value that is not between CW Angle Limit and CCW Angle Limit
# Bit 0 Input Voltage Error Detects that input voltage exceeds the configured operating voltage
#
# Control Table 18
def getShutdown(self):
return self.__read1Byte(FIELD_SHUTDOWN)
def setShutdown(self, value):
self.__write1Byte(FIELD_SHUTDOWN, value)
#==========================================================================
# RAM Control Area (transient)
#==========================================================================
# 0 (Default) Turn off the torque
# 1 Turn on the torque and lock EEPROM area
#
# Control Table 24
def getTorqueEnable(self):
return self.__read1Byte(FIELD_TORQUE_ENABLE)
def setTorqueEnable(self, value):
self.__write1Byte(FIELD_TORQUE_ENABLE, value)
# 0 (Default) Turn OFF the LED
# 1 Turn ON the LED
#
# Control Table 25
def getLed(self):
return self.__read1Byte(FIELD_LED)
def setLed(self, value):
self.__write1Byte(FIELD_LED, value)
# It exists in each direction of CW/CCW and means the error
# between goal position and present position. The range of the
# value is 0~255, and the unit is the same as Goal
# Position.(Address 30,31) The greater the value, the more
# difference occurs.
#
# Control Table 26
def getCwComplianceMargin(self):
return self.__read1Byte(FIELD_CW_COMPLIANCE_MARGIN)
def setCwComplianceMargin(self, value):
self.__write1Byte(FIELD_CW_COMPLIANCE_MARGIN, value)
# It exists in each direction of CW/CCW and means the error
# between goal position and present position. The range of the
# value is 0~255, and the unit is the same as Goal
# Position.(Address 30,31) The greater the value, the more
# difference occurs.
#
# Control Table 27
def getCcwComplianceMargin(self):
return self.__read1Byte(FIELD_CCW_COMPLIANCE_MARGIN)
def setCcwComplianceMargin(self, value):
self.__write1Byte(FIELD_CCW_COMPLIANCE_MARGIN, value)
# It exists in each direction of CW/CCW and sets the level of
# Torque near the goal position. Compliance Slope is set in 7
# steps, the higher the value, the more flexibility is
# obtained. Data representative value is actually used value. That
# is, even if the value is set to 25, 16 is used internally as the
# representative value.
#
# 1 0(0x00) ~ 3(0x03) 2(0x02)
# 2 4(0x04) ~ 7(0x07) 4(0x04)
# 3 8(0x08)~15(0x0F) 8(0x08)
# 4 16(0x10)~31(0x1F) 16(0x10)
# 5 32(0x20)~63(0x3F) 32(0x20)
# 6 64(0x40)~127(0x7F) 64(0x40)
# 7 128(0x80)~254(0xFE) 128(0x80)
#
# Control Area 28
def getCwComplianceSlope(self):
return self.__read1Byte(FIELD_CW_COMPLIANCE_SLOPE)
def setCwComplianceSlope(self, value):
self.__write1Byte(FIELD_CW_COMPLIANCE_SLOPE, value)
# It exists in each direction of CW/CCW and sets the level of
# Torque near the goal position. Compliance Slope is set in 7
# steps, the higher the value, the more flexibility is
# obtained. Data representative value is actually used value. That
# is, even if the value is set to 25, 16 is used internally as the
# representative value.
#
# 1 0(0x00) ~ 3(0x03) 2(0x02)
# 2 4(0x04) ~ 7(0x07) 4(0x04)
# 3 8(0x08)~15(0x0F) 8(0x08)
# 4 16(0x10)~31(0x1F) 16(0x10)
# 5 32(0x20)~63(0x3F) 32(0x20)
# 6 64(0x40)~127(0x7F) 64(0x40)
# 7 128(0x80)~254(0xFE) 128(0x80)
#
# Control Area 29
def getCcwComplianceSlope(self):
return self.__read1Byte(FIELD_CCW_COMPLIANCE_SLOPE)
def setCcwComplianceSlope(self, value):
self.__write1Byte(FIELD_CCW_COMPLIANCE_SLOPE, value)
# It is a position value of destination.
#
# 0 ~ 1,023 (0x3FF) is available.
#
# The unit is 0.29°.
#
# If Goal Position is out of the range, Angle Limit Error Bit (Bit
# 1) of Status Packet is returned as ‘1’ and Alarm is triggered as
# set in Alarm LED/Shutdown.
#
# Control Area 30
def getGoalPosition(self):
return self.__read2Bytes(FIELD_GOAL_POSITION)
def setGoalPosition(self, value):
self.__write2Bytes(FIELD_GOAL_POSITION, value)
while True:
currentPosition = self.getPresentPosition()
print("[ID:%03d] GoalPos:%03d PresPos:%03d" %
(self.deviceId, value, currentPosition))
if abs(value - currentPosition) < 10:
break
# It is a moving speed to Goal Position. The range and the unit
# of the value may vary depending on the operation mode.
#
# Joint Mode
#
# 0 ~ 1,023 (0x3FF) can be used, and the unit is about 0.111rpm.
#
# If it is set to 0, it means the maximum rpm of the motor is used
# without controlling the speed.
#
# If it is 1023, it is about 114rpm. For example, if it is set to
# 300, it is about 33.3 rpm.
#
# Wheel Mode
#
# 0 ~ 2,047 (0x7FF) can be used, the unit is about 0.1%.
#
# If a value in the range of 0 ~ 1,023 is used, it is stopped by
# setting to 0 while rotating to CCW direction.
#
# If a value in the range of 1,024 ~ 2,047 is used, it is stopped
# by setting to 1,024 while rotating to CW direction.
#
# That is, the 10th bit becomes the direction bit to control the
# direction. In Wheel Mode, only the output control is possible,
# not speed. For example, if it is set to 512, it means the
# output is controlled by 50% of the maximum output.
#
# Control Area 32
def getMovingSpeed(self):
return self.__read2Bytes(FIELD_MOVING_SPEED)
def setMovingSpeed(self, value):
self.__write2Bytes(FIELD_MOVING_SPEED, value)
# It is the value of the maximum torque limit.
#
# 0 ~ 1,023 (0x3FF) is available, and the unit is about 0.1%.
#
# For example, if the value is 512, it is about 50%; that means
# only 50% of the maximum torque will be used. If the power is
# turned on, the value of Max Torque(14) is used as the initial
# value.
#
# Control Area 34
def getTorqueLimit(self):
return self.__read2Bytes(FIELD_TORQUE_LIMIT)
def setTorqueLimit(self, value):
self.__write2Bytes(FIELD_TORQUE_LIMIT, value)
# It is the present position value of DYNAMIXEL.
#
# The range of the value is 0~1023 (0x3FF), and the unit is 0.29
# [°].
#
# Control Area 36
# Read Only
def getPresentPosition(self):
return self.__read2Bytes(FIELD_PRESENT_POSITION)
# It is the present moving speed.
#
# 0~2,047 (0x7FF) can be used.
#
# If a value is in the rage of 0~1,023, it means that the motor
# rotates to the CCW direction.
#
# If a value is in the rage of 1,024~2,047, it means that the
# motor rotates to the CW direction.
#
# That is, the 10th bit becomes the direction bit to control the
# direction, and 0 and 1,024 are equal. The unit of this value
# varies depending on operation mode.
#
# Joint Mode
#
# The unit is about 0.111rpm. For example, if it is set to 300, it
# means that the motor is moving to the CCW direction at a rate of
# about 33.3rpm.
#
# Wheel Mode
#
# The unit is about 0.1%. For example, if it is set to 512, it
# means that the torque is controlled by 50% of the maximum torque
# to the CCW direction.
#
# Control Area 38
# Read Only
def getPresentSpeed(self):
return self.__read2Bytes(FIELD_PRESENT_SPEED)
# It means currently applied load.
#
# The range of the value is 0~2047, and the unit is about 0.1%.
#
# If the value is 0~1,023, it means the load works to the CCW
# direction.
#
# If the value is 1,024~2,047, it means the load works to the CW
# direction. That is, the 10th bit becomes the direction bit to
# control the direction, and 1,024 is equal to 0. For example, the
# value is 512, it means the load is detected in the direction of
# CCW about 50% of the maximum torque.
#
# Control Area 40
# Read Only
def getPresentLoad(self):
return self.__read2Bytes(FIELD_PRESENT_LOAD)
# It is the size of the present voltage supplied. This value is 10
# times larger than the actual voltage. For example, when 10V is
# supplied, the data value is 100 (0x64)
#
# If Present Voltage(42) value is out of range, Voltage Range
# Error Bit (Bit0) of Status Packet is returned as ‘1’ and Alarm
# is triggered and set the address 17 and set 1 to the Bit 0 of
# the address 18.
#
# Control Area 42
# Read Only
def getPresentVoltage(self):
return self.__read1Byte(FIELD_PRESENT_VOLTAGE)
# It is the internal temperature of DYNAMIXEL in Celsius. Data
# value is identical to the actual temperature in Celsius. For
# example, if the data value is 85 (0x55), the current internal
# temperature is 85°C.
#
# Control Area 43
# Read Only
def getPresentTemperature(self):
return self.__read1Byte(FIELD_PRESENT_TEMPERATURE)
# 0 No instruction registered by REG_WRITE.
# 1 Instruction registered by REG_WRITE exsists.
#
# Control Area 44
# Read Only
def getRegisteredInstruction(self):
return self.__read1Byte(FIELD_REGISTERED_INSTRUCTION)
# 0 Goal position command execution is completed
# 1 Goal position command execution is in progress
#
# Control Area 46
# Read Only
def getMoving(self):
return self.__read1Byte(FIELD_MOVING)
# 0 EEPROM area can be modified
# 1 EEPROM area cannot be modified
#
# Control Area 47
def getLock(self):
return self.__read1Byte(FIELD_LOCK)
def setLock(self, value):
self.__write1Byte(FIELD_LOCK, value)
# Minimum current to drive motor. This value ranges from 0x20 to
# 0x3FF.
#
# Control Area 48
def getPunch(self):
return self.__read2Bytes(FIELD_PUNCH)
def setPunch(self, value):
self.__write2Bytes(FIELD_PUNCH, value)
#==========================================================================
# Private functions
#==========================================================================
def __read1Byte(self, field):
self.tty.openPort()
self.tty.setBaudRate(1000000)
response, result, error = self.protocol.read1ByteTxRx(
self.tty, self.deviceId, field)
self.__verifyResponse(
result, error, "ERROR: read1ByteTxRx('%s', %s, %s)" %
(self.ttyName, self.deviceId, self.__fieldToString(field)))
self.tty.closePort()
return response
def __read2Bytes(self, field):
self.tty.openPort()
self.tty.setBaudRate(1000000)
response, result, error = self.protocol.read2ByteTxRx(
self.tty, self.deviceId, field)
self.__verifyResponse(
result, error, "ERROR: read2ByteTxRx('%s', %s, %s)" %
(self.ttyName, self.deviceId, self.__fieldToString(field)))
self.tty.closePort()
return response
def __write1Byte(self, field, value):
self.tty.openPort()
self.tty.setBaudRate(1000000)
result, error = self.protocol.write1ByteTxRx(self.tty, self.deviceId,
field, value)
self.__verifyResponse(
result, error, "ERROR: write1ByteTxRx('%s', %s, %s, %s)" %
(self.ttyName, self.deviceId, self.__fieldToString(field), value))
self.tty.closePort()
def __write2Bytes(self, field, value):
self.tty.openPort()
self.tty.setBaudRate(1000000)
result, error = self.protocol.write2ByteTxRx(self.tty, self.deviceId,
field, value)
self.__verifyResponse(
result, error, "ERROR: write2ByteTxRx('%s', %s, %s, %s)" %
(self.ttyName, self.deviceId, self.__fieldToString(field), value))
self.tty.closePort()
def __fieldToString(self, field):
if field == FIELD_MODEL_NUMBER:
return "MODEL_NUMBER"
if field == FIELD_FIRMWARE_VERSION:
return "FIRMWARE_VERSION"
if field == FIELD_ID:
return "ID"
if field == FIELD_BAUD_RATE:
return "BAUD_RATE"
if field == FIELD_RETURN_DELAY_TIME:
return "RETURN_DELAY_TIME"
if field == FIELD_CW_ANGLE_LIMIT:
return "CW_ANGLE_LIMIT"
if field == FIELD_CCW_ANGLE_LIMIT:
return "CCW_ANGLE_LIMIT"
if field == FIELD_TEMPERATURE_LIMIT:
return "TEMPERATURE_LIMIT"
if field == FIELD_MIN_VOLTAGE_LIMIT:
return "MIN_VOLTAGE_LIMIT"
if field == FIELD_MAX_VOLTAGE_LIMIT:
return "MAX_VOLTAGE_LIMIT"
if field == FIELD_MAX_TORQUE:
return "MAX_TORQUE"
if field == FIELD_STATUS_RETURN_LEVEL:
return "STATUS_RETURN_LEVEL"
if field == FIELD_ALARM_LED:
return "ALARM_LED"
if field == FIELD_SHUTDOWN:
return "SHUTDOWN"
if field == FIELD_TORQUE_ENABLE:
return "TORQUE_ENABLE"
if field == FIELD_LED:
return "LED"
if field == FIELD_CW_COMPLIANCE_MARGIN:
return "CW_COMPLIANCE_MARGIN"
if field == FIELD_CCW_COMPLIANCE_MARGIN:
return "CCW_COMPLIANCE_MARGIN"
if field == FIELD_CW_COMPLIANCE_SLOPE:
return "CW_COMPLIANCE_SLOPE"
if field == FIELD_CCW_COMPLIANCE_SLOPE:
return "CCW_COMPLIANCE_SLOPE"
if field == FIELD_GOAL_POSITION:
return "GOAL_POSITION"
if field == FIELD_MOVING_SPEED:
return "MOVING_SPEED"
if field == FIELD_TORQUE_LIMIT:
return "TORQUE_LIMIT"
if field == FIELD_PRESENT_POSITION:
return "PRESENT_POSITION"
if field == FIELD_PRESENT_SPEED:
return "PRESENT_SPEED"
if field == FIELD_PRESENT_LOAD:
return "PRESENT_LOAD"
if field == FIELD_PRESENT_VOLTAGE:
return "PRESENT_VOLTAGE"
if field == FIELD_PRESENT_TEMPERATURE:
return "PRESENT_TEMPERATURE"
if field == FIELD_REGISTERED_INSTRUCTION:
return "REGISTERED_INSTRUCTION"
if field == FIELD_MOVING:
return "MOVING"
if field == FIELD_LOCK:
return "LOCK"
if field == FIELD_PUNCH:
return "PUNCH"
def __verifyResponse(self, result, error, s):
if result != sdk.COMM_SUCCESS:
raise Exception("%s %s" % (s, self.protocol.getTxRxResult(result)))
elif error != 0:
raise Exception("%s %s" %
(s, self.protocol.getRxPacketError(error)))
tty.setBaudRate(baudrate)
return tty
def verify_response(result, error):
if result != sdk.COMM_SUCCESS:
print("%s" % protocol.getTxRxResult(result))
elif error != 0:
print("%s" % protocol.getRxPacketError(error))
def set_torque(tty, protocol, device, value):
result, error = protocol.write1ByteTxRx(tty, device, TORQUE_ENABLE, value)
verify_response(result, error)
def goto(tty, protocol, device, goal):
result, error = protocol.write2ByteTxRx(tty, device, GOAL_POSITION, goal)
verify_response(result, error)
while True:
position, result, error = protocol.read2ByteTxRx(tty, device, PRESENT_POSITION)
verify_response(result, error)
print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (device, goal, position))
if not abs(goal - position) > 20:
break
# Open TTY
tty = open_tty('/dev/tty.usbserial-FT3WFGSI', 1000000)
protocol = sdk.PacketHandler(1.0)
set_torque(tty, protocol, 1, 1)
goto(tty, protocol, 1, 1023)
set_torque(tty, protocol, 1, 0)
tty.closePort()
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
import dynamixel_sdk as sdk
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()
ax12a = sdk.PacketHandler(1.0)
model = ping(ax12a, 1)
print("Model: %d" % model)
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, 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]))
