def sample_path_xy(path, index):
"""
Read a sample (x,y) of the given path at a given array-index
"""
if path['buffer_type'] == 'dy.memory':
y = dy.memory_read(memory=path['Y'], index=index)
x = dy.memory_read(memory=path['X'], index=index)
return x, y
elif path['buffer_type'] == 'circular_buffer':
x = cb.read_from_absolute_index(path['X'], index)
y = cb.read_from_absolute_index(path['Y'], index)
return x, y
def sample_path(path, index):
"""
Read a sample of the given path at a given array-index
"""
if path['buffer_type'] == 'dy.memory':
d = dy.memory_read(memory=path['D'], index=index)
x = dy.memory_read(memory=path['X'], index=index)
y = dy.memory_read(memory=path['Y'], index=index)
psi = dy.memory_read(memory=path['PSI'], index=index)
K = dy.memory_read(memory=path['K'], index=index)
return d, x, y, psi, K
elif path['buffer_type'] == 'circular_buffer':
d = cb.read_from_absolute_index(path['D'], index)
x = cb.read_from_absolute_index(path['X'], index)
y = cb.read_from_absolute_index(path['Y'], index)
psi = cb.read_from_absolute_index(path['PSI'], index)
K = cb.read_from_absolute_index(path['K'], index)
return d, x, y, psi, K
def sample_path_d(path, index):
"""
Read a sample (d) of the given path at a given array-index
"""
if path['buffer_type'] == 'dy.memory':
d = dy.memory_read(memory=path['D'], index=index)
return d
elif path['buffer_type'] == 'circular_buffer':
d = cb.read_from_absolute_index(path['D'], index)
return d
def path_horizon_head_index(path):
"""
Get the current head-index position in the horizon and the distance at the head
"""
if path['buffer_type'] == 'dy.memory':
head_index = dy.int32( path['samples'] - 1 )
distance_at_the_end_of_horizon = dy.memory_read( memory=path['D'], index=head_index )
elif path['buffer_type'] == 'circular_buffer':
head_index = cb.get_current_absolute_write_index(path['D']) - 1
distance_at_the_end_of_horizon = cb.read_from_absolute_index(path['D'], head_index)
return head_index, distance_at_the_end_of_horizon
def path_horizon_tail_index(path):
"""
Get the current tail-index position in the horizon and the distance at the tail
"""
if path['buffer_type'] == 'dy.memory':
tail_index = dy.int32(0)
distance_at_the_begin_of_horizon = dy.memory_read(memory=path['D'],
index=tail_index)
elif path['buffer_type'] == 'circular_buffer':
tail_index = cb.get_absolute_minimal_index(path['D'])
distance_at_the_begin_of_horizon = cb.read_from_absolute_index(
path['D'], tail_index)
return tail_index, distance_at_the_begin_of_horizon