def init_motors(ids_list, offsets):
global PORT_NUM, GROUP_NUM, IDS
IDS = ids_list
PORT_NUM = dynamixel.portHandler(DEVICENAME)
print(PORT_NUM)
# initialize PacketHandler structs
dynamixel.packetHandler()
GROUP_NUM = dynamixel.groupSyncWrite(PORT_NUM, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION)
# open port
if not dynamixel.openPort(PORT_NUM):
print("Failed to open port!")
return -1
# set port baudrate
if not dynamixel.setBaudRate(PORT_NUM, BAUDRATE):
print("Failed to change the baudrate!")
return
# enable dynamixel torque
for DXL_ID in IDS:
dynamixel.write1ByteTxRx(PORT_NUM, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(PORT_NUM, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
return -1
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
return -1
def __init__(self, port):
self.port = port
self.protocol = 2
self.resolution = MX_RESOLUTION # MX-28 Resolution
self.groupwrite = dxl.groupSyncWrite(self.port, self.protocol,
ADDR_GOAL_POS, LEN_GOAL_POSITION)
self.groupread = dxl.groupSyncRead(self.port, self.protocol,
ADDR_PRES_POS, LEN_PRESENT_POSITION)
print("Protocol 2 ready..")
def __init__(self, port):
self.port = port
self.protocol = 1
self.mx_res = MX_RESOLUTION # MX-28 Resolution
self.fsr_res = FSR_RESOLUTION #FSR Resolution
self.groupwrite = dxl.groupSyncWrite(self.port, self.protocol,
ADDR_GOAL_POS, LEN_GOAL_POSITION)
# self.groupspeed = dxl.groupSyncWrite(self.port, self.protocol, ADDR_MOV_SPEED, LEN_MOV_SPEED)
# self.groupread = dxl.groupSyncRead(self.port, self.protocol, ADDR_PRES_POS, LEN_PRESENT_POSITION)
print("Protocol 1 ready")
def __init__(self, baudrate=1000000, port='/dev/ttyUSB0', protocol=2):
self.baudrate = baudrate
self.port = dxl.portHandler(port.encode('utf-8'))
self.protocol = protocol
self.resolution = MX_RESOLUTION # MX-28 Resolution
self.groupwrite = dxl.groupSyncWrite(self.port, self.protocol,
ADDR_GOAL_POS, LEN_GOAL_POSITION)
self.groupread = dxl.groupSyncRead(self.port, self.protocol,
ADDR_PRES_POS, LEN_PRESENT_POSITION)
dxl.packetHandler()
self.connect()
def group_sync_write_goal(self, commands):
sync_group_num = dynamixel.groupSyncWrite(self.__port_num, PROTOCOL, GOAL_POSITION, 2)
for servo_id, goal_position in commands:
success = dynamixel.groupSyncWriteAddParam(sync_group_num, servo_id, int(goal_position), 2)
if not success:
raise IOError("failed writing to group sync write")
dynamixel.groupSyncWriteTxPacket(sync_group_num)
dxl_comm_result = dynamixel.getLastTxRxResult(self.__port_num, PROTOCOL)
if dxl_comm_result != 0:
error_desciption = get_tx_rx_result_description(dxl_comm_result)
raise IOError("dxl_comm_result: " + error_desciption)
dynamixel.groupSyncWriteClearParam(sync_group_num)
def _syncWrite(self, servos, addr, info_len, values=None):
''' this is an adaptation from dynamixel's sdk for
the sync_write '''
SW = dxl.groupSyncWrite(self.socket, PROTOCOL_VERSION, addr, info_len)
for i, s in enumerate(servos):
if (values is None):
dxl.groupSyncWriteAddParam(SW, s.servo_id, s.goalValue,
info_len)
else:
dxl.groupSyncWriteAddParam(SW, s.servo_id, values[i], info_len)
dxl.groupSyncWriteTxPacket(SW) #does the sync write
dxl.groupSyncWriteClearParam(SW) #clears buffer
def _syncWrite(self, servos, addr, info_len, values=None):
''' this is an adaptation from dynamixel's sdk for
the sync_write '''
SW = dxl.groupSyncWrite(self.socket, PROTOCOL_VERSION, addr, info_len)
for i, s in enumerate(servos):
if(values is None):
dxl.groupSyncWriteAddParam(SW, s.servo_id, s.goalValue, info_len)
else:
dxl.groupSyncWriteAddParam(SW, s.servo_id, values[i], info_len)
dxl.groupSyncWriteTxPacket(SW) #does the sync write
dxl.groupSyncWriteClearParam(SW) #clears buffer
def __init__(self, DeviceName):
self.portHandler = dynamixel.portHandler(DeviceName)
self.packetHandler = dynamixel.packetHandler()
self.dxl_comm_result = COMM_TX_FAIL
self.dxl_error = 0
self.ADDR_MX_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model
self.ADDR_MX_GOAL_POSITION = 116
self.ADDR_MX_PRESENT_POSITION = 132
self.ADDR_MX_SPEED = 104
self.ADDR_MX_PRESENT_SPEED = 104
self.dxl_addparam_result = 0
self.LEN_MX_GOAL_POSITION = 4
self.LEN_MX_PRESENT_POSITION = 4
# Protocol version
self.PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel
self.groupSync = dynamixel.groupSyncWrite(self.portHandler, self.PROTOCOL_VERSION, self.ADDR_MX_GOAL_POSITION, self.LEN_MX_GOAL_POSITION)
self.groupSyncSpeed = dynamixel.groupSyncWrite(self.portHandler,self.PROTOCOL_VERSION, self.ADDR_MX_SPEED, self.LEN_MX_GOAL_POSITION)
# Default setting
self.BAUDRATE = 57600
self.TORQUE_ENABLE = 1 # Value for enabling the torque
self.TORQUE_DISABLE = 0 # Value for disabling the torque
self.DXL_MINIMUM_POSITION_VALUE = -1000000 # Dynamixel will rotate between this value
self.DXL_MAXIMUM_POSITION_VALUE = 1000000 # 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.)
self.DXL_MOVING_STATUS_THRESHOLD = 10
if(dynamixel.openPort(self.portHandler)):
print "Port open done"
else:
print("Failed to open port")
quit()
if(dynamixel.setBaudRate(self.portHandler, self.BAUDRATE)):
print "Change baudrate succeeded"
else:
print "Cannot add baudrate"
quit()
## Parametros de la ecuacion de Choset (helix_rolling)
amplitud_par = 60
amplitud_impar = 60
offset_par = 0
offset_impar = 0
desfase = 90
dtheta_dn = 10
dtheta_dt = -18
port_num = dynamixel.portHandler(DEVICENAME)
dynamixel.packetHandler()
## Inicializa miembros de la estructura de puntero de datos de paquetesInicia
group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION,
P_GOAL_POSITION_L, GOAL_POSITION_LEN)
## Se abre el puerto para proceder al envío de paquetes
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
print("Press any key to terminate...")
getch()
quit()
## Establece la tasa de transmision
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
ESC_ASCII_VALUE = 0x1b
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION,
ADDR_MX_GOAL_POSITION,
LEN_MX_GOAL_POSITION)
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE,
DXL_MAXIMUM_POSITION_VALUE] # Goal position
dxl_error = 0 # Dynamixel error
dxl1_present_position = 0 # Present position
dxl2_present_position = 0
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
ESC_ASCII_VALUE = 0x1b
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_XM430_GOAL_POSITION, LEN_XM430_GOAL_POSITION)
# Initialize Groupsyncread Structs for Present Position
groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_XM430_PRESENT_POSITION, LEN_XM430_PRESENT_POSITION)
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_addparam_result = 0 # AddParam result
dxl_getdata_result = 0 # GetParam result
dxl_goal_position = [
[2048, 2048, 3072, 2048, 2048, 2048, 2700], # Init v
[1649, 2028, 1626, 1527, 2683, 2426, 2000], # Bow v
[1853, 2677, 2373, 1731, 2446, 2340, 2000], # Move v
[1853, 2677, 2373, 1731, 2446, 2340, 2560], # Grab
[2048, 2048, 3072, 2048, 2048, 2048, 2560], # Center
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 3 # Dynamixel moving status threshold
ESC_ASCII_VALUE = 0x1b
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_TORQUE, LEN_MX_GOAL_TORQUE)
dxl_comm_result = COMM_TX_FAIL # Communication result
def callback(data):
rospy.loginfo(data.data)
dxl_torqcon(data.data)
def dxl_torqcon(goal_torq):
# Add Dynamixels goal position value to the Syncwrite storage
for i in xrange(6):#
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, i+1, goal_torq[i], LEN_MX_GOAL_TORQUE)).value
print(dxl_addparam_result)
if dxl_addparam_result != 1:
print(dxl_addparam_result)
print("[ID:%03d] groupSyncWrite addparam failed" % (i+1))
getch()
quit()
# Set port baudrate
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Initialize Groupsyncwrite instance
groupwrite_acceleration_limit = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_XM430_ACCELERATION_LIMIT, LEN_XM430_ACCELERATION_LIMIT)
# Add Dynamixel #1~#5 acceleration limit value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL1_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID))
quit()
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL2_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))
quit()
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL3_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL3_ID))
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 3 # Dynamixel moving status threshold
ESC_ASCII_VALUE = 0x1b
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_TORQUE, LEN_MX_GOAL_TORQUE)
torque_mode_on = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_TORQUE_CONT, LEN_MX_TORQUE_CTRL)
dxl_comm_result = COMM_TX_FAIL # Communication result
def callback(data):
rospy.loginfo(data.data)
dxl_torcon(data.data)
def dxl_torcon(goal_torq):
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, 1, ADDR_MX_GOAL_TORQUE, goal_torq[0])
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
ESC_ASCII_VALUE = 0x1b
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION,
ADDR_PRO_INDIRECTDATA_FOR_WRITE,
LEN_PRO_INDIRECTDATA_FOR_WRITE)
# Initialize Groupsyncread Structs for Present Position
groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION,
ADDR_PRO_INDIRECTDATA_FOR_READ,
LEN_PRO_INDIRECTDATA_FOR_READ)
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_addparam_result = 0 # AddParam result
dxl_getdata_result = 0 # GetParam result
dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE,
DXL_MAXIMUM_POSITION_VALUE] # Goal position
dxl_error = 0 # Dynamixel error
ESC_ASCII_VALUE = 0x1b
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION)
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position
dxl_error = 0 # Dynamixel error
dxl1_present_position = 0 # Present position
dxl2_present_position = 0
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
print("Press any key to terminate...")
ESC_ASCII_VALUE = 0x1b COMM_SUCCESS = 0 # Communication Success result value COMM_TX_FAIL = -1001 # Communication Tx Failed # Initialize PortHandler Structs # Set the port path # Get methods and members of PortHandlerLinux or PortHandlerWindows port_num = dynamixel.portHandler(DEVICENAME) # Initialize PacketHandler Structs dynamixel.packetHandler() # Initialize Groupsyncwrite instance groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE) # Initialize Groupsyncread Structs for Present Position groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ) index = 0 dxl_comm_result = COMM_TX_FAIL # Communication result dxl_addparam_result = 0 # AddParam result dxl_getdata_result = 0 # GetParam result dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position dxl_error = 0 # Dynamixel error dxl_moving = 0 # Dynamixel moving status dxl_led_value = [0x00, 0xFF] # Dynamixel LED value dxl_present_position = 0 # Present position
