class DynamixelPositionControl(object):
"""Dynamixel read & write class."""
def __init__(self, cfg):
# Dynamixel Setting
rospy.loginfo("Dynamixel Position Controller Created")
self.cfg = cfg
self.portHandler = PortHandler(self.cfg["DEVICENAME"])
self.packetHandler = PacketHandler(self.cfg["PROTOCOL_VERSION"])
self.groupSyncWrite = GroupSyncWrite(
self.portHandler,
self.packetHandler,
self.cfg["ADDR_GOAL_POSITION"],
self.cfg["LEN_GOAL_POSITION"],
)
self.groupBulkReadPosition = GroupBulkRead(self.portHandler,
self.packetHandler)
self.groupBulkReadVelocity = GroupBulkRead(self.portHandler,
self.packetHandler)
self.groupBulkReadCurrent = GroupBulkRead(self.portHandler,
self.packetHandler)
# Port Open
if self.portHandler.openPort():
print("Succeeded to open the port")
else:
print("Failed to open the port")
quit()
# Set port baudrate
if self.portHandler.setBaudRate(self.cfg["BAUDRATE"]):
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
quit()
self.packetHandler.write1ByteTxRx(self.portHandler,
self.cfg["DXL1_ID"],
self.cfg["ADDR_OP_MODE"], 3)
self.packetHandler.write1ByteTxRx(self.portHandler,
self.cfg["DXL2_ID"],
self.cfg["ADDR_OP_MODE"], 3)
self.packetHandler.write1ByteTxRx(self.portHandler,
self.cfg["DXL3_ID"],
self.cfg["ADDR_OP_MODE"], 3)
self.packetHandler.write1ByteTxRx(self.portHandler,
self.cfg["DXL4_ID"],
self.cfg["ADDR_OP_MODE"], 3)
self.groupBulkReadPosition.addParam(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadPosition.addParam(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadPosition.addParam(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadPosition.addParam(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadVelocity.addParam(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadVelocity.addParam(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadVelocity.addParam(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadVelocity.addParam(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadCurrent.addParam(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.groupBulkReadCurrent.addParam(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.groupBulkReadCurrent.addParam(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.groupBulkReadCurrent.addParam(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
# Enable Dynamixel Torque
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL1_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_ENABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL2_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_ENABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL3_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_ENABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL4_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_ENABLE"],
)
# ROS Publisher
self.joint_states_pub = rospy.Publisher(
"/open_manipulator/joint_states_real", JointState, queue_size=3)
# ROS Subcriber
self.joint_pos_command_sub = rospy.Subscriber(
"/open_manipulator/joint_position/command",
Float64MultiArray,
self.joint_command_cb,
)
self.joint_states = JointState()
self.dxl_present_position = np.zeros(4)
self.dxl_present_velocity = np.zeros(4)
self.dxl_present_current = np.zeros(4)
self.q_desired = np.zeros(4)
self.dxl_goal_position = [
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
]
self.read_dxl()
for i in range(4):
self.dxl_goal_position[i] = [
DXL_LOBYTE(DXL_LOWORD(int(self.dxl_present_position[i]))),
DXL_HIBYTE(DXL_LOWORD(int(self.dxl_present_position[i]))),
DXL_LOBYTE(DXL_HIWORD(int(self.dxl_present_position[i]))),
DXL_HIBYTE(DXL_HIWORD(int(self.dxl_present_position[i]))),
]
self.r = rospy.Rate(100)
try:
while not rospy.is_shutdown():
self.read_dxl()
self.write_dxl()
self.r.sleep()
except KeyboardInterrupt:
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL1_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_DISABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL2_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_DISABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL3_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_DISABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL4_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_DISABLE"],
)
def joint_command_cb(self, joint_desired):
""" Transform subscribed joint command to dynamixel byte information."""
i = 0
while i < 4:
self.q_desired[i] = joint_desired.data[i]
dxl_command = int(
rad2deg(self.q_desired[i]) / self.cfg["DXL_RESOLUTION"] +
self.cfg["DXL_POS_OFFSET"])
if dxl_command > self.cfg["CW_LIMIT"]:
dxl_command = self.cfg["CW_LIMIT"]
elif dxl_command < self.cfg["CCW_LIMIT"]:
dxl_command = self.cfg["CCW_LIMIT"]
self.dxl_goal_position[i] = [
DXL_LOBYTE(DXL_LOWORD(dxl_command)),
DXL_HIBYTE(DXL_LOWORD(dxl_command)),
DXL_LOBYTE(DXL_HIWORD(dxl_command)),
DXL_HIBYTE(DXL_HIWORD(dxl_command)),
]
i += 1
def read_dxl(self):
""" Read dynamixel position, velocity, current value and publish through ROS."""
self.groupBulkReadPosition.txRxPacket()
self.dxl_present_position[0] = self.groupBulkReadPosition.getData(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.dxl_present_position[1] = self.groupBulkReadPosition.getData(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.dxl_present_position[2] = self.groupBulkReadPosition.getData(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.dxl_present_position[3] = self.groupBulkReadPosition.getData(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadVelocity.txRxPacket()
self.dxl_present_velocity[0] = self.groupBulkReadVelocity.getData(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.dxl_present_velocity[1] = self.groupBulkReadVelocity.getData(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.dxl_present_velocity[2] = self.groupBulkReadVelocity.getData(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.dxl_present_velocity[3] = self.groupBulkReadVelocity.getData(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadCurrent.txRxPacket()
self.dxl_present_current[0] = self.groupBulkReadVelocity.getData(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.dxl_present_current[1] = self.groupBulkReadVelocity.getData(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.dxl_present_current[2] = self.groupBulkReadVelocity.getData(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.dxl_present_current[3] = self.groupBulkReadVelocity.getData(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
for i in range(4):
if self.dxl_present_velocity[i] > 2**(
8 * self.cfg["ADDR_PRESENT_VELOCITY"] / 2):
self.dxl_present_velocity[i] = self.dxl_present_velocity[
i] - 2**(8 * self.cfg["ADDR_PRESENT_VELOCITY"])
if self.dxl_present_current[i] > 2**(
8 * self.cfg["LEN_PRESENT_CURRENT"] / 2):
self.dxl_present_current[i] = self.dxl_present_current[
i] - 2**(8 * self.cfg["LEN_PRESENT_CURRENT"])
q_current = [
0.0,
0.0,
deg2rad(
(self.dxl_present_position[0] - self.cfg["DXL_POS_OFFSET"]) *
self.cfg["DXL_RESOLUTION"]),
deg2rad(
(self.dxl_present_position[1] - self.cfg["DXL_POS_OFFSET"]) *
self.cfg["DXL_RESOLUTION"]),
deg2rad(
(self.dxl_present_position[2] - self.cfg["DXL_POS_OFFSET"]) *
self.cfg["DXL_RESOLUTION"]),
deg2rad(
(self.dxl_present_position[3] - self.cfg["DXL_POS_OFFSET"]) *
self.cfg["DXL_RESOLUTION"]),
]
qdot_current = [
0.0,
0.0,
rpm2rad(self.dxl_present_velocity[0] *
self.cfg["DXL_VELOCITY_RESOLUTION"]),
rpm2rad(self.dxl_present_velocity[1] *
self.cfg["DXL_VELOCITY_RESOLUTION"]),
rpm2rad(self.dxl_present_velocity[2] *
self.cfg["DXL_VELOCITY_RESOLUTION"]),
rpm2rad(self.dxl_present_velocity[3] *
self.cfg["DXL_VELOCITY_RESOLUTION"]),
]
motor_current = [
0.0,
0.0,
self.dxl_present_current[0] * self.cfg["DXL_TO_CURRENT"],
self.dxl_present_current[1] * self.cfg["DXL_TO_CURRENT"],
self.dxl_present_current[2] * self.cfg["DXL_TO_CURRENT"],
self.dxl_present_current[3] * self.cfg["DXL_TO_CURRENT"],
]
self.joint_states.position = q_current
self.joint_states.velocity = qdot_current
self.joint_states.effort = motor_current
self.joint_states_pub.publish(self.joint_states)
def write_dxl(self):
""" Write joint command to dynamixel."""
self.groupSyncWrite.addParam(self.cfg["DXL1_ID"],
self.dxl_goal_position[0])
self.groupSyncWrite.addParam(self.cfg["DXL2_ID"],
self.dxl_goal_position[1])
self.groupSyncWrite.addParam(self.cfg["DXL3_ID"],
self.dxl_goal_position[2])
self.groupSyncWrite.addParam(self.cfg["DXL4_ID"],
self.dxl_goal_position[3])
self.groupSyncWrite.txPacket()
self.groupSyncWrite.clearParam()
def error_check(self, dxl_comm_result, dxl_error):
""" Check dynamixel error."""
if dxl_comm_result != self.cfg["COMM_SUCCESS"]:
print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % self.packetHandler.getRxPacketError(dxl_error))
print('Initialised U2D2 at {}'.format(path))
break
else:
print('Could not initialise U2D2 at {}'.format(path))
# Could not find a USB device in range
if PORT_HANDLER is None:
print('Could not initialise U2D2! Maybe increase port range?')
exit()
# Protocol 1 packet handler:
PROTOCOL_ONE = PacketHandler(1.0)
# Protocol 2 packet handler:
PROTOCOL_TWO = PacketHandler(2.0)
if PORT_HANDLER.openPort():
pass
else:
print("Failed to open the port")
if PORT_HANDLER.setBaudRate(1000000):
pass
else:
print("Failed to change the baudrate")
class Dynamixel:
"""
An object representing a single Dynamixel
The Dynamixel class contains the ability to read or write to any address
class DynamixelPortAdapter:
def __init__(self):
self.control_table = pd.read_csv('../../XL430_W250_control_table.csv',
sep=';',
index_col=3)
self.BAUD_RATE = 57600 # Dynamixel default baud_rate : 57600
self.DEVICE_NAME = '/dev/ttyUSB0' # Check which port is being used on your controller
self.PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel
self.packet_handler = None
self.port_handler = None
def init_communication(self):
self.port_handler = PortHandler(self.DEVICE_NAME)
try:
if not self.port_handler.openPort():
raise self.NoRobotException(
"Failed to open the port for device: " + self.DEVICE_NAME)
except SerialException as e:
raise self.NoRobotException(str(e))
if not self.port_handler.setBaudRate(self.BAUD_RATE):
raise Exception("Failed to change the baudrate to: " +
str(self.BAUD_RATE))
self.packet_handler = PacketHandler(self.PROTOCOL_VERSION)
def write(self, dxl_id, name, value):
"""
Write value to address of name using dxl_sdk.
Check input value against min and max given by control table reference.
:param self:
:param int dxl_id: dxl_id of dxl
:param str name: name of control table entry
:param int value: value to write to control table
:return: None
:rtype: None
"""
value = int(value)
size = self.control_table.loc[name, 'Size [byte]']
addr = self.control_table.loc[name, 'Address']
dxl_comm_result, dxl_error = self.write_given_size(
addr, dxl_id, size, value)
mode = 'write'
self.handle_return_messages(dxl_comm_result, dxl_error, dxl_id, mode,
name, value)
def write_given_size(self, addr, dxl_id, size, value):
if size == 1:
dxl_comm_result, dxl_error = self.packet_handler.write1ByteTxRx(
self.port_handler, dxl_id, addr, value)
elif size == 2:
dxl_comm_result, dxl_error = self.packet_handler.write2ByteTxRx(
self.port_handler, dxl_id, addr, value)
elif size == 4:
dxl_comm_result, dxl_error = self.packet_handler.write4ByteTxRx(
self.port_handler, dxl_id, addr, value)
else:
raise Exception('\'size [byte]\' was not 1,2 or 4, got: ' +
str(size))
return dxl_comm_result, dxl_error
def read(self, dxl_id, name):
"""
Write value to address of name using dxl_sdk.
Check input value against min and max given by control table reference.
:param self:
:param int dxl_id: dxl_id of dxl
:param str name: name of control table entry
:return: value read from control table
:rtype: int
"""
size = self.control_table.loc[name, 'Size [byte]']
addr = self.control_table.loc[name, 'Address']
dxl_comm_result, dxl_error, value = self.read_given_size(
addr, dxl_id, size)
mode = 'read'
self.handle_return_messages(dxl_comm_result, dxl_error, dxl_id, mode,
name, value)
return value
def read_given_size(self, addr, dxl_id, size):
if size == 1:
value, dxl_comm_result, dxl_error = self.packet_handler.read1ByteTxRx(
self.port_handler, dxl_id, addr)
value = int(np.int8(value))
elif size == 2:
value, dxl_comm_result, dxl_error = self.packet_handler.read2ByteTxRx(
self.port_handler, dxl_id, addr)
value = int(np.int16(value))
elif size == 4:
value, dxl_comm_result, dxl_error = self.packet_handler.read4ByteTxRx(
self.port_handler, dxl_id, addr)
value = int(np.int32(value))
else:
raise Exception('\'size [byte]\' was not 1,2 or 4, got: ' +
str(size))
return dxl_comm_result, dxl_error, value
def handle_return_messages(self, dxl_comm_result, dxl_error, dxl_id, mode,
name, value):
error_msg = f"{mode} dxl_id {dxl_id} and address {name} gave error: "
if dxl_comm_result != COMM_SUCCESS:
raise Exception(error_msg +
self.packet_handler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
raise Exception(error_msg +
self.packet_handler.getRxPacketError(dxl_error))
else:
logger.debug(
f'{name} has been successfully been {mode} as {value} on dxl {dxl_id}'
)
class NoRobotException(BaseException):
pass