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)