class Pedestrian(BoxComponent):
def __init__(
self,
init_state=[start_walk_lane[0], start_walk_lane[1], -np.pi / 2,
0], # (x, y, theta, gait)
number_of_gaits=6,
gait_length=4,
gait_progress=0,
film_dim=(1, 6),
prim_queue=None, # primitive queue
pedestrian_type='3',
):
# init_state: initial state by default
self.name = 'Pedestrian {}'.format(id(self))
self.state = np.array(init_state, dtype="float")
self.alive_time = 0
self.is_dead = False
self.number_of_gaits = film_dim[0] * film_dim[1]
self.gait_length = gait_length
self.gait_progress = gait_progress
self.film_dim = film_dim
self.pedestrian_type = random.choice(['1', '2', '3', '4', '5', '6'])
if prim_queue == None:
self.prim_queue = Queue()
else:
prim_queue = prim_queue
self.fig = '../imglib/pedestrians/walking' + pedestrian_type + '.png'
self.medic = '../imglib/pedestrians/medic' + '.png'
self.all_pedestrian_types = {'1', '2', '3', '4', '5', '6'}
self.dt = 0.1
async def next(self, inputs, dt):
"""
The pedestrian advances forward
"""
#await trio.sleep(0)
if self.is_dead:
if self.fig != self.medic:
self.fig = self.medic
else:
dee_theta, vee = inputs
self.state[2] += dee_theta # update heading of pedestrian
self.state[0] += vee * np.cos(
self.state[2]) * self.dt # update x coordinate of pedestrian
self.state[1] += vee * np.sin(
self.state[2]) * self.dt # update y coordinate of pedestrian
distance_travelled = vee * self.dt # compute distance travelled during dt
gait_change = (self.gait_progress +
distance_travelled / self.gait_length
) // 1 # compute number of gait change
self.gait_progress = (self.gait_progress +
distance_travelled / self.gait_length) % 1
self.state[3] = int(
(self.state[3] + gait_change) % self.number_of_gaits)
self.alive_time += self.dt
async def extract_primitive(self):
#TODO: rewrite the comment below
"""
This function updates the primitive queue and picks the next primitive to be applied. When there is no more primitive in the queue, it will
return False
"""
while self.prim_queue.len() > 0:
if self.prim_queue.top(
)[1] < 1: # if the top primitive hasn't been exhausted
prim_data, prim_progress = self.prim_queue.top() # extract it
return prim_data, prim_progress
else:
self.prim_queue.pop() # pop it
return False
async def prim_next(self):
#await trio.sleep(0)
if await self.extract_primitive(
) == False: # if there is no primitive to use
await self.next((0, 0), self.dt)
else:
prim_data, prim_progress = await self.extract_primitive(
) # extract primitive data and primitive progress from prim
start, finish, vee = prim_data # extract data from primitive
total_distance, _ = await self.get_walking_displacement(
start, finish)
if prim_progress == 0: # ensure that starting position is correct at start of primitive
self.state[0] = start[0]
self.state[1] = start[1]
if start == finish: #waiting mode
remaining_distance = 0
self.state[3] = 0 # reset gait
if self.prim_queue.len(
) > 1: # if current not at last primitive
last_prim_data, last_prim_progress = self.prim_queue.bottom(
) # extract last primitive
_, last_finish, vee = last_prim_data
_, heading = await self.get_walking_displacement(
self.state, last_finish)
if self.state[2] != heading:
self.state[2] = heading
else: # if in walking mode
remaining_distance, heading = await self.get_walking_displacement(
self.state, finish)
if self.state[2] != heading:
self.state[2] = heading
if vee * self.dt > remaining_distance and remaining_distance != 0:
await self.next((0, remaining_distance / self.dt), self.dt)
else:
await self.next((0, vee), self.dt)
if total_distance != 0:
prim_progress += self.dt / (total_distance / vee)
self.prim_queue.replace_top(
(prim_data, prim_progress)) # update primitive queue
async def get_walking_displacement(self, start, finish):
dx = finish[0] - start[0]
dy = finish[1] - start[1]
distance = np.linalg.norm(np.array([dx, dy]))
heading = np.arctan2(dy, dx)
return distance, heading
async def start_ped(self, start_walk, end_walk):
print('Start Pedestrian')
self.start_walk_lane = start_walk
self.end_walk_lane = end_walk
self.prim_queue.enqueue(
((self.start_walk_lane, self.end_walk_lane, 60), 0))
async def ped_walk(self):
while (self.state[0] <
self.end_walk_lane[0]): # if not at the destination
#print('Pedestrian is walking')
#print(self.state)
await self.prim_next()
await trio.sleep(0)
async def run(self, start_walk, end_walk):
await self.start_ped(start_walk, end_walk)
async with trio.open_nursery() as nursery:
nursery.start_soon(self.ped_walk)
await trio.sleep(0)
class KinematicCar():
'''Kinematic car class
init_state is [vee, theta, x, y], where vee, theta, x, y are the velocity, orientation, and
coordinates of the car respectively
'''
def __init__(
self,
init_state=[0, 0, 0, 0],
length=50, # length of vehicle in pixels
acc_max=9.81, # maximum acceleration of vehicle
acc_min=-9.81, # maximum deceleration of vehicle
steer_max=0.5, # maximum steering input in radians
steer_min=-0.5, # minimum steering input in radians
vee_max=100, # maximum velocity
is_honking=False, # the car is honking
color='blue', # color of the car
plate_number=None, # license plate number
# queue of primitives, each item in the queue has the form (prim_id, prim_progress) where prim_id is the primitive ID and prim_progress is the progress of the primitive)
prim_queue=None,
fuel_level=float('inf')): # TODO: fuel level of the car
if color not in car_colors:
raise Exception("This car color doesn't exist!")
self._length = length
self._vee_max = vee_max
self.acc_range = (acc_min, acc_max)
self.steer_range = (steer_min, steer_max)
self.alive_time = 0
self.plate_number = plate_number
#self.init_state = np.array(init_state, dtype='float')
self.state = np.array(init_state, dtype='float')
self.color = color
# self.new_unpause = False
# self.new_pause = False
# extended state required for Bastian's primitive computation
self.extended_state = None
self.is_honking = is_honking
if prim_queue == None:
self.prim_queue = Queue()
else:
self.prim_queue = prim_queue
self.fuel_level = fuel_level
self.fig = Image.open(car_figs[color])
def state_dot(self, state, time, acc, steer):
"""
This function defines the system dynamics
Inputs
acc: acceleration input
steer: steering input
"""
# if already at maximum speed, can't no longer accelerate
if abs(state[0]) >= self._vee_max and sign(acc) == sign(state[0]):
vee_dot = 0
else:
vee_dot = saturation_filter(acc, self.acc_range[0],
self.acc_range[1])
theta_dot = state[0] / self._length * tan(
saturation_filter(steer, self.steer_range[0], self.steer_range[1]))
x_dot = state[0] * cos(state[1])
y_dot = state[0] * sin(state[1])
dstate = [vee_dot, theta_dot, x_dot, y_dot]
return dstate
def toggle_honk(self):
self.is_honking = not self.is_honking
def next(self, inputs, dt):
"""
next is a function that updates the current position of the car when inputs are applied for a duration of dt
Inputs:
inputs: acceleration and steering inputs
dt: integration time
Outputs:
None - the states of the car will get updated
"""
acc, steer = inputs
# take only the real part of the solution
self.state = odeint(self.state_dot,
self.state,
t=(0, dt),
args=(acc, steer))[1]
self.fuel_level -= abs(
acc) * dt # fuel decreases linearly with acceleration
self.alive_time += dt
# fix floating
if abs(acc) < 0.1:
self.state[0] = 0
def extract_primitive(self):
"""
This function updates the primitive queue and picks the next primitive to be applied. When there is no more primitive in the queue, it will
return False
"""
while self.prim_queue.len() > 0:
# if the top primitive hasn't been exhausted
if self.prim_queue.top()[1] < 1:
prim_id, prim_progress = self.prim_queue.top() # extract it
return prim_id, prim_progress
else:
self.prim_queue.pop() # pop it
return False
def prim_next(self, dt):
"""
updates with primitive, if no primitive available, update with next with zero inputs
Inputs:
dt: integration time
Outputs:
None - the states of the car will get updated
"""
if self.extract_primitive(
) == False: # if there is no primitive to use
self.next((0, 0), dt)
else:
prim_id, prim_progress = self.extract_primitive()
# TODO: make this portion of the code more automated
if prim_id > -1:
# load primitive data
list_of_key = ['x0', 'x_ref', 'u_ref', 'alpha', 'K']
x0, x_ref, u_ref, alpha, K = get_bunch_prim_data(
prim_id, list_of_key)
if prim_progress == 0: # compute initial extended state
x_real = self.state.reshape((-1, 1))
x1 = x_real - x0
x2 = np.matmul(np.linalg.inv(diag([4, 0.02, 4, 4])), x1)
# initial state, consisting of actual and virtual states for the controller
self.extended_state = (np.vstack((x_real, x0, x1, x2)))[:,
0]
num_of_inputs = 2
G_u = np.diag(
[175, 1.29]
) # this diagonal matrix encodes the size of input set (a constraint)
dist = get_disturbance()
k = int(prim_progress *
params.num_subprims) # calculate primitive waypoint
q1 = K[k, 0].reshape((-1, 1), order='F')
q2 = 0.5 * (x_ref[:, k + 1] + x_ref[:, k]).reshape(-1, 1)
q3 = u_ref[:, k].reshape(-1, 1)
q4 = u_ref[:, k].reshape(-1, 1)
q5 = np.matmul(
G_u,
alpha[k * num_of_inputs:(k + 1) * num_of_inputs]).reshape(
(-1, 1), order='F')
# parameters for the controller
q = np.vstack((q1, q2, q3, q4, q5))
self.extended_state = odeint(func=prim_state_dot,
y0=self.extended_state,
t=[0, dt],
args=(dist, q))[-1, :]
self.state = self.extended_state[0:len(self.state)]
self.alive_time += dt
prim_progress = prim_progress + dt / get_prim_data(
prim_id, 't_end')[0]
self.prim_queue.replace_top((prim_id, prim_progress))
else: # if is stopping primitive
self.next((0, 0), dt)
class KinematicCar:
"""Kinematic Car Class
init_state is [vee, theta, x, y], where vee, theta, x, y are the velocity, orientation, and
coordinates of the car respectively
"""
def __init__(
self,
init_state=[0, 0, 0, 0],
L=50, # length of vehicle
a_max=9.81, # maximum acceleration of vehicle
a_min=-9.81, # maximum deceleration of vehicle
nu_max=0.5, # maximum steering input in radians/sec
nu_min=-0.5, # minimum steering input in radians/sec)
vee_max=100, # maximum velocity
is_honking=False, # the car is honking
color='blue', # color of the car
# queue of primitives, each item in the queue has the form (prim_id, prim_progress) where prim_id is the primitive ID and prim_progress is the progress of the primitive)
prim_queue=None,
fuel_level=float('inf')
): # TODO: fuel level of the car - FUTURE FEATURE)
if color != 'blue' and color != 'gray':
raise Exception("Color must either be blue or gray!")
self.color = color
self.fig = Image.open(car_figs[color])
self.params = (L, a_max, a_min, nu_max, nu_min, vee_max)
self.alive_time = 0
self.state = np.array(init_state, dtype='float')
# extended state required for Bastian's primitive computation
self.extended_state = None
self.is_honking = is_honking
self.prim_queue = Queue() if prim_queue is None else prim_queue
self.fuel_level = fuel_level
def state_dot(self, state, t, a, nu):
"""
state_dot is a function that defines the system dynamics
Inputs
state: current state
t: current time
a: acceleration input
nu: steering input
"""
(L, a_max, a_min, nu_max, nu_min, vee_max) = self.params
dstate_dt = np.zeros(np.shape(state))
dstate_dt[0] = saturation_filter(a, a_max, a_min)
# if already at maximum speed, can't no longer accelerate
if np.abs(state[1]) >= vee_max and np.sign(a) == np.sign(state[1]):
dstate_dt[1] = 0
else:
dstate_dt[1] = state[0] / L * \
tan(saturation_filter(nu, nu_max, nu_min))
dstate_dt[2] = state[0] * cos(state[1])
dstate_dt[3] = state[0] * sin(state[1])
return dstate_dt
def toggle_honk(self):
self.is_honking = not self.is_honking
def next(self, inputs, dt):
"""
next is a function that updates the current position of the car when inputs are applied for a duration of dt
Inputs:
inputs: acceleration and steering inputs
dt: integration time
Outputs:
None - the states of the car will get updated
"""
a, nu = inputs
# take only the real part of the solution
self.state = odeint(self.state_dot,
self.state,
t=(0, dt),
args=(a, nu))[1]
# fuel decreases linearly with acceleration/deceleration
self.fuel_level -= np.abs(a) * dt
# update alive time
self.alive_time += dt
# TODO: temporary fix to floating problem
if a == 0:
self.state[0] = np.sign(self.state[0]) * \
abs(self.state[0]) * dt * 0.05
def extract_primitive(self):
# TODO: rewrite the comment below
"""
This function updates the primitive queue and picks the next primitive to be applied. When there is no more primitive in the queue, it will
return False
"""
while self.prim_queue.len() > 0:
# if the top primitive hasn't been exhausted
if self.prim_queue.top()[1] < 1:
prim_id, prim_progress = self.prim_queue.top() # extract it
return prim_id, prim_progress
else:
self.prim_queue.pop() # pop it
return False
def prim_next(self, dt):
"""
updates with primitive, if no primitive available, update with next with zero inputs
Inputs:
dt: integration time
Outputs:
None - the states of the car will get updated
"""
# TODO: implement compatibility check with primitive to make sure that the params & dynamics match (essentially comparing prim_car and the
# kinematic model)
if self.extract_primitive(
) == False: # if there is no primitive to use
self.next((0, 0), dt)
else:
prim_id, prim_progress = self.extract_primitive()
# load primitive data TODO: make this portion of the code more automated
# the primitive corresponding to the primitive number
prim = mat['MA3'][prim_id, 0]
t_end = prim['t_end'][0, 0][0, 0] # extract duration of primitive
# number of subintervals encoded in primitive
N = prim['K'][0, 0].shape[0]
# this diagonal matrix encodes the size of input set (a constraint)
G_u = np.diag([175, 1.29])
nu = 2 # number of inputs
nx = 4 # number of states
if prim_progress == 0: # compute initial extended state
x1 = self.state.reshape((-1, 1))
x2 = prim['x0'][0, 0]
x3 = x1 - prim['x0'][0, 0]
x4 = np.matmul(np.linalg.inv(np.diag([4, 0.02, 4, 4])),
(x1 - prim['x0'][0, 0]))
# initial state, consisting of actual state and virtual states for the controller
self.extended_state = (np.vstack((x1, x2, x3, x4)))[:, 0]
k = int(prim_progress * N) # calculate primitive waypoint
dist = get_disturbance()
q1 = prim['K'][0, 0][k, 0].reshape((-1, 1), order='F')
q2 = 0.5 * (prim['x_ref'][0, 0][:, k + 1] +
prim['x_ref'][0, 0][:, k]).reshape(-1, 1)
q3 = prim['u_ref'][0, 0][:, k].reshape(-1, 1)
q4 = prim['u_ref'][0, 0][:, k].reshape(-1, 1)
q5 = np.matmul(G_u, prim['alpha'][0,
0][k * nu:(k + 1) * nu]).reshape(
(-1, 1), order='F')
# parameters for the controller
q = np.vstack((q1, q2, q3, q4, q5))
self.extended_state = odeint(func=prim_state_dot,
y0=self.extended_state,
t=[0, dt],
args=(dist, q))[-1, :]
self.state = self.extended_state[0:4]
# update alive time
self.alive_time += dt
# update progress
prim_progress = prim_progress + dt / t_end
self.prim_queue.replace_top((prim_id, prim_progress))
class Pedestrian:
def __init__(
self,
init_state=[0, 0, 0, 0], # (x, y, theta, gait)
number_of_gaits=6,
gait_length=4,
gait_progress=0,
film_dim=(1, 6),
prim_queue=None, # primitive queue
pedestrian_type='3',
name=None,
age=20):
"""
Pedestrian class
"""
# init_state: initial state by default (x = 0, y = 0, theta = 0, gait = 0)
self.vee_max = 50
self.alive_time = 0
self.is_dead = False
self.id = 0
self.destination = (-1, -1)
self.state = np.array(init_state, dtype="float")
self.monitor_state = 0
self.number_of_gaits = film_dim[0] * film_dim[1]
self.gait_length = gait_length
self.gait_progress = gait_progress
self.film_dim = film_dim
self.name = name
self.age = age
self.pedestrian_type = pedestrian_type
if prim_queue == None:
self.prim_queue = Queue()
else:
prim_queue = prim_queue
self.fig = dir_path + '/imglib/pedestrians/walking' + pedestrian_type + '.png'
def next(self, inputs, dt):
"""
The pedestrian advances forward
"""
if self.is_dead:
if self.fig != medic:
self.fig = medic
else:
dee_theta, vee = inputs
self.state[2] += dee_theta # update heading of pedestrian
self.state[0] += vee * cos(
self.state[2]) * dt # update x coordinate of pedestrian
self.state[1] += vee * sin(
self.state[2]) * dt # update y coordinate of pedestrian
distance_travelled = vee * dt # compute distance travelled during dt
gait_change = (self.gait_progress +
distance_travelled / self.gait_length
) // 1 # compute number of gait change
self.gait_progress = (self.gait_progress +
distance_travelled / self.gait_length) % 1
self.state[3] = int(
(self.state[3] + gait_change) % self.number_of_gaits)
self.alive_time += dt
def extract_primitive(self):
#TODO: rewrite the comment below
"""
This function updates the primitive queue and picks the next primitive to be applied. When there is no more primitive in the queue, it will
return False
"""
while self.prim_queue.len() > 0:
if self.prim_queue.top(
)[1] < 1: # if the top primitive hasn't been exhausted
prim_data, prim_progress = self.prim_queue.top() # extract it
return prim_data, prim_progress
else:
self.prim_queue.pop() # pop it
return False
def prim_next(self, dt):
if self.extract_primitive(
) == False: # if there is no primitive to use
self.next((0, 0), dt)
else:
prim_data, prim_progress = self.extract_primitive(
) # extract primitive data and primitive progress from prim
start, finish, vee = prim_data # extract data from primitive
total_distance, _ = self.get_walking_displacement(start, finish)
if prim_progress == 0: # ensure that starting position is correct at start of primitive
self.state[0] = start[0]
self.state[1] = start[1]
if start == finish: #waiting mode
remaining_distance = 0
self.state[3] = 0 # reset gait
if self.prim_queue.len(
) > 1: # if current not at last primitive
last_prim_data, last_prim_progress = self.prim_queue.bottom(
) # extract last primitive
_, last_finish, vee = last_prim_data
_, heading = self.get_walking_displacement(
self.state, last_finish)
if self.state[2] != heading:
self.state[2] = heading
else: # if in walking mode
remaining_distance, heading = self.get_walking_displacement(
self.state, finish)
if self.state[2] != heading:
self.state[2] = heading
if vee * dt > remaining_distance and remaining_distance != 0:
self.next((0, remaining_distance / dt), dt)
else:
self.next((0, vee), dt)
if total_distance != 0:
prim_progress += dt / (total_distance / vee)
self.prim_queue.replace_top(
(prim_data, prim_progress)) # update primitive queue
def get_walking_displacement(self, start, finish):
dx = finish[0] - start[0]
dy = finish[1] - start[1]
distance = np.linalg.norm(np.array([dx, dy]))
heading = arctan2(dy, dx)
return distance, heading
# cross the street if light is green, or if the pedestrian just crossed the street, continue walking and ignore traffic light color
def continue_walking(self, lane1, lane2, direction, remaining_time):
person_xy = (self.state[0], self.state[1])
walking = False
theta = self.state[2]
self.monitor_state = 4
if person_xy in (lane1 + lane2) and remaining_time != -1:
self.monitor_state = 3
prim_data, prim_progress = self.prim_queue.get_element_at_index(-2)
start, finish, vee = prim_data
remaining_distance, _ = self.get_walking_displacement(
start, finish) # in this scenario, person_xy = start
vee = max(vee, remaining_distance / remaining_time)
if vee <= self.vee_max:
walking = True
self.monitor_state = 2
# if the pedestrian isn't going fast enough, increase speed to cross street before light turns red
self.prim_queue.replace_element_at_index(
((start, finish, vee), prim_progress), -2)
elif (person_xy == lane1[0]
or person_xy == lane2[0]) and theta == direction[0]:
walking = True
self.monitor_state = 1
elif (person_xy == lane1[1]
or person_xy == lane2[1]) and theta == direction[1]:
walking = True
self.monitor_state = 1
return walking
# if the pedestrian isn't going fast enough, increase speed to cross street before light turns red
def walk_faster(self, remaining_time):
prim_data, prim_progress = self.prim_queue.top()
start, finish, vee = prim_data
remaining_distance, _ = self.get_walking_displacement(
self.state, finish)
if (remaining_distance / vee) > remaining_time:
vee = remaining_distance / remaining_time
if (vee <= self.vee_max):
self.prim_queue.replace_top(
((start, finish, vee), prim_progress))
class Pedestrian:
def __init__(
self,
init_state=[0, 0, 0, 0], # (x, y, theta, gait)
number_of_gaits=6,
gait_length=4,
gait_progress=0,
film_dim=(1, 6),
prim_queue=None, # primitive queue
pedestrian_type='3'): # three types 1 or 2 or 3
"""
Pedestrian class
"""
# init_state: initial state by default (x = 0, y = 0, theta = 0, gait = 0)
self.alive_time = 0
self.state = np.array(init_state, dtype="float")
self.number_of_gaits = film_dim[0] * film_dim[1]
self.gait_length = gait_length
self.gait_progress = gait_progress
self.film_dim = film_dim
if prim_queue == None:
self.prim_queue = Queue()
else:
prim_queue = prim_queue
self.fig = dir_path + '/imglib/pedestrians/walking' + pedestrian_type + '.png'
def next(self, inputs, dt):
"""
The pedestrian advances forward
"""
dee_theta, vee = inputs
self.state[2] += dee_theta # update heading of pedestrian
# update x coordinate of pedestrian
self.state[0] += vee * np.cos(self.state[2]) * dt
# update y coordinate of pedestrian
self.state[1] += vee * np.sin(self.state[2]) * dt
distance_travelled = vee * dt # compute distance travelled during dt
gait_change = (self.gait_progress + distance_travelled /
self.gait_length) // 1 # compute number of gait change
self.gait_progress = (self.gait_progress +
distance_travelled / self.gait_length) % 1
self.state[3] = int(
(self.state[3] + gait_change) % self.number_of_gaits)
def visualize(self):
# convert gait number to i, j coordinates of subfigure
current_gait = self.state[3]
i = current_gait % self.film_dim[1]
j = current_gait // self.film_dim[1]
img = Image.open(self.fig)
width, height = img.size
sub_width = width / self.film_dim[1]
sub_height = height / self.film_dim[0]
lower = (i * sub_width, (j - 1) * sub_height)
upper = ((i + 1) * sub_width, j * sub_height)
area = (lower[0], lower[1], upper[0], upper[1])
cropped_img = img.crop(area)
return cropped_img
def extract_primitive(self):
# TODO: rewrite the comment below
"""
This function updates the primitive queue and picks the next primitive to be applied. When there is no more primitive in the queue, it will
return False
"""
while self.prim_queue.len() > 0:
# if the top primitive hasn't been exhausted
if self.prim_queue.top()[1] < 1:
prim_data, prim_progress = self.prim_queue.top() # extract it
return prim_data, prim_progress
else:
self.prim_queue.pop() # pop it
return False
def prim_next(self, dt):
if self.extract_primitive(
) == False: # if there is no primitive to use
self.next((0, 0), dt)
else:
# extract primitive data and primitive progress from prim
prim_data, prim_progress = self.extract_primitive()
start, finish, t_end = prim_data # extract data from primitive
if prim_progress == 0: # ensure that starting position is correct at start of primitive
self.state[0] = start[0]
self.state[1] = start[1]
if start == finish: # waiting mode
remaining_distance = 0
self.state[3] = 0 # reset gait
if self.prim_queue.len(
) > 1: # if current not at last primitive
last_prim_data, last_prim_progress = self.prim_queue.bottom(
) # extract last primitive
last_start, last_finish, last_t_end = last_prim_data
dx_last = last_finish[0] - self.state[0]
dy_last = last_finish[1] - self.state[1]
heading = np.arctan2(dy_last, dx_last)
if self.state[2] != heading:
self.state[2] = heading
else: # if in walking mode
dx = finish[0] - self.state[0]
dy = finish[1] - self.state[1]
heading = np.arctan2(dy, dx)
if self.state[2] != heading:
self.state[2] = heading
remaining_distance = np.linalg.norm(np.array([dx, dy]))
remaining_time = (1 - prim_progress) * t_end
vee = remaining_distance / remaining_time
self.next((0, vee), dt)
prim_progress += dt / t_end
self.prim_queue.replace_top(
(prim_data, prim_progress)) # update primitive queue
class Pedestrian:
def __init__(
self,
init_state=[2908, 665, -pi / 2, 0], # (x, y, theta, gait)
number_of_gaits=6,
gait_length=4,
gait_progress=0,
film_dim=(1, 6),
prim_queue=None, # primitive queue
pedestrian_type='3',
):
# init_state: initial state by default
#self.vee_max = 50
self.alive_time = 0
self.is_dead = False
self.state = np.array(init_state, dtype="float")
self.number_of_gaits = film_dim[0] * film_dim[1]
self.gait_length = gait_length
self.gait_progress = gait_progress
self.film_dim = film_dim
self.pedestrian_type = random.choice(['1', '2', '3', '4', '5', '6'])
if prim_queue == None:
self.prim_queue = Queue()
else:
prim_queue = prim_queue
self.fig = dir_path + '/imglib/pedestrians/walking' + pedestrian_type + '.png'
def next(self, inputs, dt):
"""
The pedestrian advances forward
"""
if self.is_dead:
if self.fig != medic:
self.fig = medic
else:
dee_theta, vee = inputs
self.state[2] += dee_theta # update heading of pedestrian
self.state[0] += vee * cos(
self.state[2]) * dt # update x coordinate of pedestrian
self.state[1] += vee * sin(
self.state[2]) * dt # update y coordinate of pedestrian
distance_travelled = vee * dt # compute distance travelled during dt
gait_change = (self.gait_progress +
distance_travelled / self.gait_length
) // 1 # compute number of gait change
self.gait_progress = (self.gait_progress +
distance_travelled / self.gait_length) % 1
self.state[3] = int(
(self.state[3] + gait_change) % self.number_of_gaits)
self.alive_time += dt
def extract_primitive(self):
#TODO: rewrite the comment below
"""
This function updates the primitive queue and picks the next primitive to be applied. When there is no more primitive in the queue, it will
return False
"""
while self.prim_queue.len() > 0:
if self.prim_queue.top(
)[1] < 1: # if the top primitive hasn't been exhausted
prim_data, prim_progress = self.prim_queue.top() # extract it
return prim_data, prim_progress
else:
self.prim_queue.pop() # pop it
return False
def prim_next(self, dt):
if self.extract_primitive(
) == False: # if there is no primitive to use
self.next((0, 0), dt)
else:
prim_data, prim_progress = self.extract_primitive(
) # extract primitive data and primitive progress from prim
start, finish, vee = prim_data # extract data from primitive
total_distance, _ = self.get_walking_displacement(start, finish)
if prim_progress == 0: # ensure that starting position is correct at start of primitive
self.state[0] = start[0]
self.state[1] = start[1]
if start == finish: #waiting mode
remaining_distance = 0
self.state[3] = 0 # reset gait
if self.prim_queue.len(
) > 1: # if current not at last primitive
last_prim_data, last_prim_progress = self.prim_queue.bottom(
) # extract last primitive
_, last_finish, vee = last_prim_data
_, heading = self.get_walking_displacement(
self.state, last_finish)
if self.state[2] != heading:
self.state[2] = heading
else: # if in walking mode
remaining_distance, heading = self.get_walking_displacement(
self.state, finish)
if self.state[2] != heading:
self.state[2] = heading
if vee * dt > remaining_distance and remaining_distance != 0:
self.next((0, remaining_distance / dt), dt)
else:
self.next((0, vee), dt)
if total_distance != 0:
prim_progress += dt / (total_distance / vee)
self.prim_queue.replace_top(
(prim_data, prim_progress)) # update primitive queue
def get_walking_displacement(self, start, finish):
dx = finish[0] - start[0]
dy = finish[1] - start[1]
distance = np.linalg.norm(np.array([dx, dy]))
heading = arctan2(dy, dx)
return distance, heading