def __init__(self, device, channels, code):
"""
One single six-axis sensor.
Device: An instance of PAIO.AIODevice()
Channels: The channel indicies for the sensor in the following order:
[forceX, forceY, forceZ, torqueX, torqueY, torqueZ]
"""
self.device = device
self.channels = channels
self.code = code
# Dirty ctypes list for device interface
self._measurements_c = (c_float * 6)()
# Good Python list for public interface
self.measurements = [0, 0, 0, 0, 0, 0]
self.zeroes = [0, 0, 0, 0, 0, 0]
# It is always assumed that total_force is updated alongside centre_of_pressure,
# and that it is ready to go whenever the observer is notified.
self.total_force = 0
self.centre_of_pressure = Observable([0, 0])
def __init__(self, generator, fp):
KillableThread.__init__(self)
self.fp = fp
self.fps = 10
self.generator = generator
self.seconds_per_point = 10
self.seconds_between_points = 5
self._current_time = 0
self._currently_sampling = Observable()
self._currently_sampling.set(False)
self._time_when_sampling_started = 0
self._time_when_sampling_stopped = 0
def __init__(self):
threading.Thread.__init__(self)
self.dead = False
self.fps = 60
# This lock is necessary so that we do not close the pygame display while
# the loop() method is running; doing so will result in a "video system not
# initialized" error.
# We also use the lock to prevent calling pygame.init() and pygame.quit()
# concurrently.
self.pygame_lock = threading.Lock()
self._draw_tasks_lock = threading.Lock()
self._draw_tasks = []
self.keypress = Observable()
def __init__(self, sensor, mat = None): """ @argument mat - A 3x3 matrix to transform the vector [x; y; 1] into world coordinates. The matrix [[1, 0, 0], [0, 1, 0], [0, 0, 1]] corresponds to the identity. """ self.sensor = sensor if mat == 'i': self.mat = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] elif mat == 'z': self.mat = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] else: self.mat = mat self.total_force = 0 self.centre_of_pressure = Observable([0, 0]) sensor.centre_of_pressure.add_listener(self.on_update)
class CalibrationProgramThread(KillableThread):
"""
This thread is a calibration utility that takes many samples per second,
and automatically switches through the grid points.
"""
def __init__(self, generator, fp):
KillableThread.__init__(self)
self.fp = fp
self.fps = 10
self.generator = generator
self.seconds_per_point = 10
self.seconds_between_points = 5
self._current_time = 0
self._currently_sampling = Observable()
self._currently_sampling.set(False)
self._time_when_sampling_started = 0
self._time_when_sampling_stopped = 0
def loop(self):
if not self.dead:
self._current_time += 1.0 / self.fps
if self._currently_sampling.get():
# Stop sampling after a certain time
twss = self._time_when_sampling_started
if self._current_time - twss > self.seconds_per_point:
# Disable sampling
self.fp.forces_after_calibration.remove_listener(
self.generator.take_sample)
if not self.generator.grid.hasMorePoints:
self.kill()
self.generator.grid.next()
self._time_when_sampling_stopped = self._current_time
self._currently_sampling.set(False)
else:
# Start sampling after certain time
twss = self._time_when_sampling_stopped
if self._current_time - twss > self.seconds_between_points:
# Enable sampling
self.fp.forces_after_calibration.add_listener(
self.generator.take_sample)
self._time_when_sampling_started = self._current_time
self._currently_sampling.set(True)
class PyGameThread(threading.Thread):
"""
Invoke a thread of drawing pygame graphics in a new window.
"""
options = pygame.HWSURFACE | pygame.DOUBLEBUF | pygame.RESIZABLE
def __init__(self):
threading.Thread.__init__(self)
self.dead = False
self.fps = 60
# This lock is necessary so that we do not close the pygame display while
# the loop() method is running; doing so will result in a "video system not
# initialized" error.
# We also use the lock to prevent calling pygame.init() and pygame.quit()
# concurrently.
self.pygame_lock = threading.Lock()
self._draw_tasks_lock = threading.Lock()
self._draw_tasks = []
self.keypress = Observable()
def kill(self):
with self.pygame_lock:
self.dead = True
pygame.quit()
def add_draw_task(self, t):
with self._draw_tasks_lock:
self._draw_tasks.append(t)
@_exception_wrapper
def run(self):
with self.pygame_lock:
pygame.init()
self.screen = pygame.display.set_mode((500, 500), self.options)
clock = pygame.time.Clock()
pygame.display.set_caption(
'Six-Axis Force Sensors - Weighted Centre of Pressure')
s = pygame.Surface((32, 32))
pygame.display.set_icon(s)
while True:
clock.tick(60)
with self.pygame_lock:
if self.dead:
return
self.loop()
def loop(self):
pygame.event.pump()
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.pygame_lock.release()
self.kill()
return
elif event.type == pygame.VIDEORESIZE:
self.screen = pygame.display.set_mode(event.dict['size'],
self.options)
elif event.type == pygame.KEYDOWN:
self.keypress.set(event.key)
self.screen.fill((255, 255, 255))
width = self.screen.get_width()
height = self.screen.get_height()
# Do some drawing
with self._draw_tasks_lock:
for task in self._draw_tasks:
task(width, height, self.screen, pygame)
# Double buffer flip puts drawn graphics on the screen
pygame.display.flip()
@staticmethod
def query_exceptions():
while not _exceptions.empty():
e, traceback = _exceptions.get()
# One could replace this with a non-terminating utility, to print
# multiple errors; otherwise there isn't much use for the queue.
sys.stderr.write(traceback)
raise e
e.task_done()
class SixAxis(object):
def __init__(self, device, channels, code):
"""
One single six-axis sensor.
Device: An instance of PAIO.AIODevice()
Channels: The channel indicies for the sensor in the following order:
[forceX, forceY, forceZ, torqueX, torqueY, torqueZ]
"""
self.device = device
self.channels = channels
self.code = code
# Dirty ctypes list for device interface
self._measurements_c = (c_float * 6)()
# Good Python list for public interface
self.measurements = [0, 0, 0, 0, 0, 0]
self.zeroes = [0, 0, 0, 0, 0, 0]
# It is always assumed that total_force is updated alongside centre_of_pressure,
# and that it is ready to go whenever the observer is notified.
self.total_force = 0
self.centre_of_pressure = Observable([0, 0])
def update(self):
old_measurement = self.measurements[:]
self._update_measurements()
for old, new in zip(old_measurement, self.measurements):
# Only update everything if there was a change
if old != new:
self._update_centre_of_pressure()
return
print "skipped"
def _update_measurements(self):
ptr = cast(self._measurements_c, c_voidp).value
for (i, channel) in enumerate(self.channels):
slot = cast(ptr + (4 * i), POINTER(c_float))
self.device.AioSingleAiEx(c_short(channel), slot)
val = self._measurements_c[i]
self.measurements[i] = val
def _update_centre_of_pressure(self):
zipped = zip(self.measurements, self.zeroes)
after_zeroing = [m - z for m, z in zipped]
after_calibration = []
for i, x in enumerate(after_zeroing):
# Calibration equation defined on page 10 of "Single Element
# Multi-Component Transducer" instructions, model MC2.5A-1K-6278.
v = x / (EXCITATION_VOLTAGE * SENSITIVITY_TERMS[self.code][i] *
GAIN * 1e-6)
after_calibration.append(v)
F_x, F_y, F_z, M_x, M_y, M_z = after_calibration
# Given that the force is straight downwards (might not be the case with dynamics)
# we arrive at these equations and rearrange them:
# M_x = F_z * y
# M_y = F_z * x
self.total_force = F_z
if F_z == 0:
return
cop = self.centre_of_pressure.get()
scale = 0.5
cop[0] = scale * M_y / F_z
cop[1] = scale * M_x / F_z
self.centre_of_pressure.notify_all()
def set_zero(self):
for i in range(6):
self.zeroes[i] = self.measurements[i]
class SixAxisWorld(object): """ Keeps track of a single real-world position and orientation of a sensor and its coordinate system. """ def __init__(self, sensor, mat = None): """ @argument mat - A 3x3 matrix to transform the vector [x; y; 1] into world coordinates. The matrix [[1, 0, 0], [0, 1, 0], [0, 0, 1]] corresponds to the identity. """ self.sensor = sensor if mat == 'i': self.mat = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] elif mat == 'z': self.mat = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] else: self.mat = mat self.total_force = 0 self.centre_of_pressure = Observable([0, 0]) sensor.centre_of_pressure.add_listener(self.on_update) def on_update(self, cop): """ On update of underlying sensor, apply transformations, then set own cop. """ self.total_force = self.sensor.total_force cop3 = 1 new_cop = ( cop[0] * self.mat[0][0] + cop[1] * self.mat[0][1] + cop3 * self.mat[0][2], cop[0] * self.mat[1][0] + cop[1] * self.mat[1][1] + cop3 * self.mat[1][2], cop[0] * self.mat[2][0] + cop[1] * self.mat[2][1] + cop3 * self.mat[2][2] ) l = self.centre_of_pressure.get() l[0] = new_cop[0] l[1] = new_cop[1] self.centre_of_pressure.notify_all() def set_x(self, x): self.mat[0][2] = x def set_y(self, y): self.mat[1][2] = y
def __init__(self):
### Hard-coded, set-up-specific configuration follows
device0 = AIODevice(b'AIO000')
device0.Init()
device0.AioSetAiInputMethod(c_short(0))
device0.AioSetAiRangeAll(aio.PM25)
device1 = AIODevice(b'AIO001')
device1.Init()
device1.AioSetAiInputMethod(c_short(0))
device1.AioSetAiRangeAll(aio.PM25)
self.M5170 = SixAxis(device0, [0, 1, 2, 3, 4, 5], 'M5170')
self.M5238 = SixAxis(device0, [6, 7, 8, 9, 10, 11], 'M5238')
self.M5239 = SixAxis(device0, [12, 13, 14, 15, 16, 17], 'M5239')
self.M5240 = SixAxis(device0, [18, 19, 20, 21, 22, 23], 'M5240')
self.sensors = [self.M5239, self.M5170, self.M5240, self.M5238]
self.world_sensors = [SixAxisWorld(s, 'z') for s in self.sensors]
## The order of self.sensors/self.world_sensors is defined as follows:
##
## (Top-down view)
##
## 0 2
## ______ ______
## | X | | X |
## | | | | | |
## |___ X | | X __ |
##
## 1 3
##
## Coords:
##
## ---> +X
## |
## |
## V
## +Y
# Distance between two adjacent sensors on the same plate (m)
sensor_diff = 7.68e-2
# Distance between plates (m)
stance_width = 15e-2
self.world_sensors[0].set_x(-stance_width / 2)
self.world_sensors[1].set_x(-stance_width / 2)
self.world_sensors[2].set_x(stance_width / 2)
self.world_sensors[3].set_x(stance_width / 2)
self.world_sensors[0].set_y(-sensor_diff / 2)
self.world_sensors[1].set_y(sensor_diff / 2)
self.world_sensors[2].set_y(-sensor_diff / 2)
self.world_sensors[3].set_y(sensor_diff / 2)
self.world_sensors[0].mat[1][0] = -1
self.world_sensors[0].mat[0][1] = 1
self.world_sensors[1].mat[1][0] = -1
self.world_sensors[1].mat[0][1] = -1
self.world_sensors[2].mat[1][0] = 1
self.world_sensors[2].mat[0][1] = 1
self.world_sensors[3].mat[1][0] = 1
self.world_sensors[3].mat[0][1] = 1
###
self.total_force = 0
self.centre_of_pressure = Observable([0, 0])
for s in self.sensors:
s.update()
s.set_zero()
class WorldSensorConfiguration(object):
"""
Keeps track of real-world positions and orientations of sensors alongside their
coordinate systems.
"""
def __init__(self):
### Hard-coded, set-up-specific configuration follows
device0 = AIODevice(b'AIO000')
device0.Init()
device0.AioSetAiInputMethod(c_short(0))
device0.AioSetAiRangeAll(aio.PM25)
device1 = AIODevice(b'AIO001')
device1.Init()
device1.AioSetAiInputMethod(c_short(0))
device1.AioSetAiRangeAll(aio.PM25)
self.M5170 = SixAxis(device0, [0, 1, 2, 3, 4, 5], 'M5170')
self.M5238 = SixAxis(device0, [6, 7, 8, 9, 10, 11], 'M5238')
self.M5239 = SixAxis(device0, [12, 13, 14, 15, 16, 17], 'M5239')
self.M5240 = SixAxis(device0, [18, 19, 20, 21, 22, 23], 'M5240')
self.sensors = [self.M5239, self.M5170, self.M5240, self.M5238]
self.world_sensors = [SixAxisWorld(s, 'z') for s in self.sensors]
## The order of self.sensors/self.world_sensors is defined as follows:
##
## (Top-down view)
##
## 0 2
## ______ ______
## | X | | X |
## | | | | | |
## |___ X | | X __ |
##
## 1 3
##
## Coords:
##
## ---> +X
## |
## |
## V
## +Y
# Distance between two adjacent sensors on the same plate (m)
sensor_diff = 7.68e-2
# Distance between plates (m)
stance_width = 15e-2
self.world_sensors[0].set_x(-stance_width / 2)
self.world_sensors[1].set_x(-stance_width / 2)
self.world_sensors[2].set_x(stance_width / 2)
self.world_sensors[3].set_x(stance_width / 2)
self.world_sensors[0].set_y(-sensor_diff / 2)
self.world_sensors[1].set_y(sensor_diff / 2)
self.world_sensors[2].set_y(-sensor_diff / 2)
self.world_sensors[3].set_y(sensor_diff / 2)
self.world_sensors[0].mat[1][0] = -1
self.world_sensors[0].mat[0][1] = 1
self.world_sensors[1].mat[1][0] = -1
self.world_sensors[1].mat[0][1] = -1
self.world_sensors[2].mat[1][0] = 1
self.world_sensors[2].mat[0][1] = 1
self.world_sensors[3].mat[1][0] = 1
self.world_sensors[3].mat[0][1] = 1
###
self.total_force = 0
self.centre_of_pressure = Observable([0, 0])
for s in self.sensors:
s.update()
s.set_zero()
def update(self):
for s in self.sensors:
s.update()
# Weighted sum of world coordinates of sensors
accumulator = [0, 0]
force_accum = 0
for w in self.world_sensors:
if w.sensor == self.M5170:
continue
l = w.centre_of_pressure.get()
f = w.sensor.total_force
accumulator[0] += l[0] * f
accumulator[1] += l[1] * f
force_accum += f
self.total_force = force_accum
# Division by zero
if force_accum == 0:
return
accumulator[0] /= force_accum
accumulator[1] /= force_accum
cop = self.centre_of_pressure.get()
cop[0] = accumulator[0]
cop[1] = accumulator[1]
self.centre_of_pressure.notify_all()
def test_observable_notify(): c = Observable() c.set(5) assert c.get() == 5 triggered = [False] def f(c): triggered[0] = True c.add_listener(f) c.set(10) assert triggered[0] triggered = [False] c.remove_listener(f) c.set(20) assert not triggered[0] c.add_listener(f) c.notify_all() assert triggered[0]