def identify_baud_rate(dxl_id, usb):
"""Identify the baud rate a Dynamixel servo is communicating at.
Parameters
----------
dxl_id : int
Dynamixel ID on chain. Must be [0, 25]
usb : str
the USB port, typically "/dev/something"
Returns
-------
int
the baud rate the Dynamixel is communicating at
"""
for b in BAUD_MAP.keys():
port_h = prh.PortHandler(usb)
port_h.openPort()
port_h.setBaudRate(b)
packet_h = pch.PacketHandler(2.0)
_, dxl_comm_result, _ = packet_h.ping(port_h, dxl_id)
port_h.closePort()
if dxl_comm_result == COMM_SUCCESS:
return b
return -1
def __init__(self,
dxl_id,
usb,
port_handler=None,
pt_lock=None,
verbose=False):
Device.__init__(self, verbose)
self.dxl_id = dxl_id
self.comm_errors = 0
#Make access to portHandler threadsafe
if pt_lock is None:
self.pt_lock = threading.RLock()
else:
self.pt_lock = pt_lock
self.usb = usb
#Allow sharing of port handler across multiple servos
self.packet_handler = None
try:
if port_handler is None:
self.port_handler = prh.PortHandler(usb)
self.port_handler.openPort()
self.port_handler.setBaudRate(57600)
else:
self.port_handler = port_handler
self.packet_handler = pch.PacketHandler(2.0)
except serial.SerialException as e:
print("SerialException({0}): {1}".format(e.errno, e.strerror))
self.hw_valid = self.packet_handler is not None
def __init__(self,
port,
baudrate,
feedback_echo=False,
protocol_version=2.0):
# Initialize Port and Packet Handler, also make needed variables / objects
# TODO: Should try/catch in case of errors
self.serial_mutex = Lock()
self.port_handler = port_h.PortHandler(port)
self.packet_handler = packet_h.PacketHandler(protocol_version)
self.port_handler.openPort()
self.port_handler.setBaudRate(baudrate)
self.port = port
self.baudrate = baudrate
self.dynotools = dynamixel_tools.DynamixelTools()
self.raw_to_deg_switch = {
'54024': self.raw_to_deg_pulse,
'311': self.raw_to_deg_static,
'1120': self.raw_to_deg_static,
'1020': self.raw_to_deg_static
}
self.deg_to_raw_switch = {
'54024': self.deg_to_raw_pulse,
'311': self.deg_to_raw_static,
'1120': self.deg_to_raw_static,
'1020': self.deg_to_raw_static
}
def __init__(self,
dxl_id,
usb,
port_handler=None,
pt_lock=None,
baud=57600,
logger=logging.getLogger()):
self.dxl_id = dxl_id
self.usb = usb
self.comm_errors = 0
self.last_comm_success = True
self.logger = logger
# Make access to portHandler threadsafe
self.pt_lock = threading.RLock() if pt_lock is None else pt_lock
# Allow sharing of port handler across multiple servos
self.packet_handler = None
try:
if port_handler is None:
self.port_handler = prh.PortHandler(usb)
self.port_handler.openPort()
self.port_handler.setBaudRate(baud)
else:
self.port_handler = port_handler
self.packet_handler = pch.PacketHandler(2.0)
except serial.SerialException as e:
self.logger.error("SerialException({0}): {1}".format(
e.errno, e.strerror))
self.hw_valid = self.packet_handler is not None
def get_feedback(self, servo_id):
# Port and packet handler set up
self.port_handler = port_h.PortHandler(self.port_name)
self.packet_handler = packet_h.PacketHandler(self.protocol_version)
# Set up port and baud rate
self.port_handler.openPort()
self.port_handler.setBaudRate(self.baud_rate)
return self.sdk_io.get_feedback(servo_id, self.motor_info)
def __init__(self, dxl_id, device_name, baudrate=4000000):
self.DXL_ID = dxl_id
self.BAUDRATE = baudrate
self.DEVICENAME = device_name
self.PROTOCOL_VERSION = 2.0
self.ADDR_OPERATING_MODE = 11 # 1 Byte
self.ADDR_OPERATING_MODE_LENGTH = 1 # 1 Byte
self.ADDR_PRO_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model # 1 Byte
self.ADDR_PRO_TORQUE_ENABLE_LENGTH = 1
self.ADDR_PRO_GOAL_POSITION = 116 # 4 Bytes
self.ADDR_PRO_GOAL_POSITION_LENGTH = 4
self.ADDR_PRO_PRESENT_POSITION = 132 # 4 Bytes
self.ADDR_PRO_PRESENT_POSITION_LENGTH = 4
self.ADDR_PRO_GOAL_CURRENT = 102 # 2 Bytes
self.ADDR_PRO_GOAL_CURRENT_LENGTH = 2
self.ADDR_PRO_PRESENT_CURRENT = 126 # 2 Bytes
self.ADDR_PRO_PRESENT_CURRENT_LENGTH = 2
self.ADDR_PRO_PRESENT_VELOCITY = 128 # 4 Bytes
self.ADDR_PRO_PRESENT_VELOCITY_LENGTH = 4
self.ADDR_PRO_GOAL_VELOCITY = 104 # 4 Bytes
self.ADDR_PRO_GOAL_VELOCITY_LENGTH = 4
self.portHandler = port_handler.PortHandler(self.DEVICENAME)
self.packetHandler = packet_handler.PacketHandler(self.PROTOCOL_VERSION)
# Initialize GroupSyncWrite
self.groupSyncWriteCurrent = group_sync_write.GroupSyncWrite(self.portHandler, self.packetHandler, self.ADDR_PRO_GOAL_CURRENT,
self.ADDR_PRO_GOAL_CURRENT_LENGTH)
self.groupSyncWritePosition = group_sync_write.GroupSyncWrite(self.portHandler, self.packetHandler, self.ADDR_PRO_GOAL_POSITION,
self.ADDR_PRO_GOAL_POSITION_LENGTH)
self.groupSyncWriteVelocity = group_sync_write.GroupSyncWrite(self.portHandler, self.packetHandler, self.ADDR_PRO_GOAL_VELOCITY,
self.ADDR_PRO_GOAL_VELOCITY_LENGTH)
# Initialize GroupSyncRead
self.groupSyncReadPosition = group_sync_read.GroupSyncRead(self.portHandler, self.packetHandler, self.ADDR_PRO_PRESENT_POSITION,
self.ADDR_PRO_PRESENT_POSITION_LENGTH)
self.groupSyncReadVelocity = group_sync_read.GroupSyncRead(self.portHandler, self.packetHandler, self.ADDR_PRO_PRESENT_VELOCITY,
self.ADDR_PRO_PRESENT_VELOCITY_LENGTH)
# Open Port
if self.portHandler.openPort():
print("Succeeded to open the port")
else:
print("Failed to open the port")
if self.portHandler.setBaudRate(self.BAUDRATE):
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
# GroupSyncRead addparam
for i in dxl_id:
dxl_addparam_result = self.groupSyncReadPosition.addParam(i)
if dxl_addparam_result != True:
print("[ID:%03d] groupSyncReadPosition addparam failed" % i)
dxl_addparam_result = self.groupSyncReadVelocity.addParam(i)
if dxl_addparam_result != True:
print("[ID:%03d] groupSyncReadVelocity addparam failed" % i)
print('Initialize Completed!')
def __init__(self, usb):
Device.__init__(self)
self.usb = usb
self.timer_stats = hello_utils.TimerStats()
self.pt_lock = threading.RLock()
self.port_handler = prh.PortHandler(usb)
self.port_handler.openPort()
self.port_handler.setBaudRate(57600)
self.packet_handler = pch.PacketHandler(2.0)
self.status = {}
self.motors = {}
self.readers = {}
def __init__(self, controller_namespace, controllers):
print("HELLO FROM JOINT_TRAJECTORY_CONTROLLER")
rospy.loginfo("HELLO_FROM_JOINT_TRAJECTORY_CONTROLLER")
for controller in controllers:
print(controller)
self.update_rate = 1000
self.state_update_rate = 50
self.trajectory = []
self.port_namespace = "l_arm_port" #TODO Irvin added
self.controller_namespace = controller_namespace
self.joint_names = [c.joint_name for c in controllers] # IRVIN UPDATED
self.joint_to_controller = {}
for c in controllers:
self.joint_to_controller[c.joint_name] = c
self.port_to_joints = {}
for c in controllers:
if c.port_namespace not in self.port_to_joints: self.port_to_joints[c.port_namespace] = []
self.port_to_joints[c.port_namespace].append(c.joint_name)
# self.port_to_io = {}
# for c in controllers:
# if c.port_namespace in self.port_to_io: continue
# self.port_to_io[c.port_namespace] = c.dxl_io
# NOTE: Note that if we are doing mutliple port management
# we also need to reference the different port_handler(s) and packet_handler(s)
# for each respective servo
self.port_handler = {}
self.packet_handler = {}
for c in controllers:
rospy.logwarn("C.PORTNAMESPACE" + c.port_namespace)
if c.port_namespace in self.port_handler: continue
self.port_handler[c.port_namespace] = c.port_handler
self.packet_handler[c.port_namespace] = c.packet_handler
rospy.logwarn(type(c.port_namespace))
# NOTE: Note that if we are doing mutliple port management
# we also need to reference the different port_handler(s) and packet_handler(s)
# for each respective servo
# self.packet_handler = {}
# for c in controllers:
# if c.port_namespace in self.port_namespace: continue
# self.packet_handler[c.port_namespace] = c.packet_handler
self.joint_states = dict(zip(self.joint_names, [c.joint_state for c in controllers]))
self.num_joints = len(self.joint_names)
self.joint_to_idx = dict(zip(self.joint_names, range(self.num_joints)))
self.port_handler_irvin = port_h.PortHandler("/dev/ttyUSB0")
self.packet_handler_irvin = packet_h.PacketHandler(2.0)
def connect(self):
"""
Connects up physical port with the port handler, and initialize packet handler
"""
try:
# Port and packet handler set up
self.port_handler = port_h.PortHandler(self.port_name)
self.packet_handler = packet_h.PacketHandler(self.protocol_version)
# Set up port and baud rate
self.port_handler.openPort()
self.port_handler.setBaudRate(self.baud_rate)
self.__find_motors()
except rospy.ROSInterruptException: pass
self.running = True
def __init__(self, dxl_id, usb, port_handler=None, pt_lock=None):
Device.__init__(self)
self.dxl_id = dxl_id
self.comm_errors = 0
#Make access to portHandler threadsafe
if pt_lock is None:
self.pt_lock = threading.RLock()
else:
self.pt_lock = pt_lock
self.usb = usb
#Allow sharing of port handler across multiple servos
if port_handler is None:
self.port_handler = prh.PortHandler(usb)
self.port_handler.openPort()
self.port_handler.setBaudRate(57600)
else:
self.port_handler = port_handler
self.packet_handler = pch.PacketHandler(2.0)
def __init__(self, usb, verbose=False):
Device.__init__(self, verbose)
self.usb = usb
self.timer_stats = hello_utils.TimerStats()
self.pt_lock = threading.RLock()
try:
self.port_handler = prh.PortHandler(usb)
self.port_handler.openPort()
self.port_handler.setBaudRate(57600)
self.packet_handler = pch.PacketHandler(2.0)
self.hw_valid = True
except serial.SerialException as e:
self.packet_handler = None
self.port_handler = None
self.hw_valid = False
print("SerialException({0}): {1}".format(e.errno, e.strerror))
self.status = {}
self.motors = {}
self.readers = {}
self.runstop_last = None
def __init__(self, usb, name):
Device.__init__(self, name)
self.usb = usb
self.pt_lock = threading.RLock()
try:
prh.LATENCY_TIMER = self.params['dxl_latency_timer']
self.port_handler = prh.PortHandler(usb)
self.port_handler.openPort()
self.port_handler.setBaudRate(int(self.params['baud']))
self.packet_handler = pch.PacketHandler(2.0)
self.hw_valid = True
except serial.SerialException as e:
self.packet_handler = None
self.port_handler = None
self.hw_valid = False
self.logger.error("SerialException({0}): {1}".format(
e.errno, e.strerror))
self.status = {}
self.motors = {}
self.readers = {}
self.comm_errors = DynamixelCommErrorStats(name, logger=self.logger)