Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def parse_controller_node(controller_node):
sync_read = True if controller_node.getAttribute('sync_read') == 'True' else False
port = controller_node.getAttribute('port')
dxl_io = pypot.dynamixel.DxlIO(port, use_sync_read=sync_read, error_handler_cls=pypot.dynamixel.BaseErrorHandler)
changed_angle_limits = {}
motors_node = controller_node.getElementsByTagName('DxlMotor')
dxl_motors = []
for motor_node in motors_node:
m, angle_limit = parse_motor_node(motor_node)
# We suppose here that they won't be any timeout
old_limits = dxl_io.get_angle_limit((m.id, ))[0]
d = numpy.linalg.norm(numpy.asarray(angle_limit) - numpy.asarray(old_limits))
if d > 1:
logging.warning('changes angle limit of motor {} to {}'.format(m.id, angle_limit))
changed_angle_limits[m.id] = angle_limit
dxl_motors.append(m)
if dxl.get_return_delay_time(ids)[0] != 0:
raise IOError('Make sure your motor has return delay time set to 0')
print('Start testing !')
print('Using "normal" pypot package...')
def rw_pypot():
dxl.get_present_position(ids)
dxl.set_goal_position(dict(zip(ids, itertools.repeat(0))))
dt = timeit(rw_pypot, args.N, os.path.join(bp, 'rw_pypot.list'))
print('in {}ms'.format(dt * 1000))
print('Using pref-forged packet...')
c_get = [c for c in DxlIO._AbstractDxlIO__controls
if c.name == 'present position'][0]
c_set = [c for c in DxlIO._AbstractDxlIO__controls
if c.name == 'goal position'][0]
pos = dxl_code(0, c_set.length)
rp = DxlReadDataPacket(ids[0], c_get.address, c_get.length)
sp = DxlSyncWritePacket(c_set.address, c_set.length, ids[:1] + list(pos))
def rw_forged_packet():
dxl._send_packet(rp)
dxl._send_packet(sp, wait_for_status_packet=False)
dt = timeit(rw_forged_packet, args.N, os.path.join(bp, 'rw_forged.list'))
print('in {}ms'.format(dt * 1000))
# Wait for the motor to "reboot..."
for _ in range(10):
with DxlIOPort(args.port, baudrate=factory_baudrate) as io:
if io.ping(1):
break
time.sleep(.5)
else:
print('Could not communicate with the motor...')
print('Make sure one (and only one) is connected and try again')
sys.exit(1)
# Switch to 1M bauds
if args.type.startswith('MX') or args.type.startswith('SR'):
print('Changing to 1M bauds...')
with DxlIO(args.port, baudrate=factory_baudrate) as io:
io.change_baudrate({1: 1000000})
time.sleep(.5)
print('Done!')
# Change id
print('Changing id to {}...'.format(args.id))
if args.id != 1:
with DxlIOPort(args.port) as io:
io.change_id({1: args.id})
time.sleep(.5)
check(io.ping(args.id),
'Could not change id to {}'.format(args.id))
print('Done!')
def get_dxl_connection(port, baudrate, protocol="MX"):
global __dxl_io
__lock.acquire()
if protocol == "XL":
__dxl_io = pypot.dynamixel.Dxl320IO(port, baudrate, use_sync_read=False)
else:
__dxl_io = pypot.dynamixel.DxlIO(port, baudrate, use_sync_read=False)
return __dxl_io
parser = argparse.ArgumentParser()
parser.add_argument('-l', '--log-folder',
type=str, required=True)
parser.add_argument('-N', type=int, required=True)
args = parser.parse_args()
bp = args.log_folder
if os.path.exists(bp):
raise IOError('Folder already exists: {}'.format(bp))
os.mkdir(bp)
available_ports = get_available_ports()
port = available_ports[0]
print('Connecting on port {}.'.format(port))
dxl = DxlIO(port)
print('Scanning motors on the bus...')
ids = dxl.scan()
print('Found {}'.format(ids))
ids = ids[:1]
print('Will use id: {}'.format(ids))
if dxl.get_return_delay_time(ids)[0] != 0:
raise IOError('Make sure your motor has return delay time set to 0')
print('Start testing !')
print('Using "normal" pypot package...')
def rw_pypot():
dxl.get_present_position(ids)
args = parser.parse_args()
check(1 <= args.id <= 253,
'Motor id must be in range [1:253]')
check(available_ports,
'Could not find an available serial port!')
protocol = 2 if args.type in 'XL-320' else 1
DxlIOPort = DxlIO if protocol == 1 else Dxl320IO
# Factory Reset
print('Factory reset...')
if protocol == 1:
for br in [57600, 1000000]:
with DxlIO(args.port, baudrate=br) as io:
io.factory_reset()
else:
with Dxl320IO(args.port, baudrate=1000000, timeout=0.01) as io:
io.factory_reset(ids=range(253))
print('Done!')
factory_baudrate = 57600 if args.type.startswith('MX') else 1000000
# Wait for the motor to "reboot..."
for _ in range(10):
with DxlIOPort(args.port, baudrate=factory_baudrate) as io:
if io.ping(1):
break
time.sleep(.5)
else: