def test_opposing(lr=0.5):
initial_state = torch.tensor([
[0.0, 0.0, 0.0, 1.0, 0.0, 10.0],
[-0.3, 10.0, 0.0, -1.0, -0.3, 0.0],
])
generator = socialforce.Simulator(initial_state)
truth = torch.stack(
[generator.step().state[:, 0:2].clone() for _ in range(21)]).detach()
print('============DONE WITH GENERATION===============')
v0 = torch.tensor(1.2, requires_grad=True)
sigma_v = torch.tensor(0.1, requires_grad=True)
V = socialforce.PedPedPotential(0.4, v0, sigma_v)
# training
for _ in range(100):
print('loop')
s = socialforce.Simulator(initial_state, None, V)
g = torch.stack([s.step().state[:, 0:2].clone() for _ in range(21)])
loss = (g - truth).pow(2).sum()
v0_grad, sigma_grad = torch.autograd.grad(loss, [v0, sigma_v])
print('v0', v0, v0_grad)
print('sigma', sigma_v, sigma_grad)
with torch.no_grad():
v0 -= lr * v0_grad
sigma_v -= lr * sigma_grad
assert v0.item() == pytest.approx(2.1, abs=0.01)
assert sigma_v.item() == pytest.approx(0.3, abs=0.01)
def predict(self, state):
"""
:param state:
:return:
"""
sf_state = []
self_state = state.self_state
sf_state.append((self_state.px, self_state.py, self_state.vx,
self_state.vy, self_state.gx, self_state.gy))
for human_state in state.human_states:
# approximate desired direction with current velocity
if human_state.vx == 0 and human_state.vy == 0:
gx = np.random.random()
gy = np.random.random()
else:
gx = human_state.px + human_state.vx
gy = human_state.py + human_state.vy
sf_state.append((human_state.px, human_state.py, human_state.vx,
human_state.vy, gx, gy))
sim = socialforce.Simulator(np.array(sf_state),
delta_t=self.time_step,
initial_speed=self.initial_speed,
v0=self.v0,
sigma=self.sigma)
sim.step()
action = ActionXY(sim.state[0, 2], sim.state[0, 3])
self.last_state = state
return action
def test_opposing_scipy():
initial_state = torch.tensor([
[0.0, 0.0, 0.0, 1.0, 0.0, 10.0],
[-0.3, 10.0, 0.0, -1.0, -0.3, 0.0],
])
generator = socialforce.Simulator(initial_state)
truth = torch.stack(
[generator.step().state[:, 0:2].clone() for _ in range(21)]).detach()
print('============DONE WITH GENERATION===============')
# training
def f(x):
v0 = torch.tensor(x[0], requires_grad=True)
sigma_v = float(x[1])
V = socialforce.PedPedPotential(0.4, v0, sigma_v)
s = socialforce.Simulator(initial_state, None, V)
g = torch.stack([s.step().state[:, 0:2].clone() for _ in range(21)])
# average euclidean distance loss
loss = (g - truth).pow(2).sum()
return loss
parameters = np.array([1.2, 0.1])
res = scipy.optimize.minimize(f,
parameters,
method='L-BFGS-B',
options=OPTIMIZER_OPT)
print(res)
assert res.x == pytest.approx(np.array([2.1, 0.3]), abs=0.01)
def predict(self, state):
sf_state = []
for agent_state in state:
sf_state.append((
agent_state.px,
agent_state.py,
agent_state.vx,
agent_state.vy,
agent_state.gx,
agent_state.gy,
))
sim = socialforce.Simulator(
np.array(sf_state),
delta_t=self.time_step,
initial_speed=self.initial_speed,
v0=self.v0,
sigma=self.sigma,
)
sim.step()
actions = [
ActionXY(sim.state[i, 2], sim.state[i, 3])
for i in range(len(state))
]
del sim
return actions
def test_walkway(n):
pos_left = ((np.random.random((n, 2)) - 0.5) * 2.0) * np.array([25.0, 5.0])
pos_right = ((np.random.random((n, 2)) - 0.5) * 2.0) * np.array([25.0, 5.0])
x_vel_left = np.random.normal(1.34, 0.26, size=(n, 1))
x_vel_right = np.random.normal(-1.34, 0.26, size=(n, 1))
x_destination_left = 100.0 * np.ones((n, 1))
x_destination_right = -100.0 * np.ones((n, 1))
zeros = np.zeros((n, 1))
state_left = np.concatenate(
(pos_left, x_vel_left, zeros, x_destination_left, zeros), axis=-1)
state_right = np.concatenate(
(pos_right, x_vel_right, zeros, x_destination_right, zeros), axis=-1)
initial_state = np.concatenate((state_left, state_right))
space = [
np.array([(x, 5) for x in np.linspace(-25, 25, num=5000)]),
np.array([(x, -5) for x in np.linspace(-25, 25, num=5000)]),
]
s = socialforce.Simulator(initial_state, socialforce.PedSpacePotential(space))
states = []
for _ in range(250):
state = s.step().state
# periodic boundary conditions
state[state[:, 0] > 25, 0] -= 50
state[state[:, 0] < -25, 0] += 50
states.append(state.copy())
states = np.stack(states)
with visualize(states, space, 'docs/walkway_{}.gif'.format(n)) as _:
pass
def f(x):
V.set_parameters(torch.from_numpy(x))
s = socialforce.Simulator(initial_state, None, V)
g = torch.stack([s.step().state[:, 0:2].clone() for _ in range(21)])
# average euclidean distance loss
diff = g - truth
loss = torch.mul(diff, diff).sum(dim=-1).mean()
return float(loss)
def f(x):
v0 = torch.tensor(x[0], requires_grad=True)
sigma_v = float(x[1])
V = socialforce.PedPedPotential(0.4, v0, sigma_v)
s = socialforce.Simulator(initial_state, None, V)
g = torch.stack([s.step().state[:, 0:2].clone() for _ in range(21)])
# average euclidean distance loss
loss = (g - truth).pow(2).sum()
return loss
def test_separator():
initial_state = np.array([
[-10.0, -0.0, 1.0, 0.0, 10.0, 0.0],
])
space = [
np.array([(i, i) for i in np.linspace(-1, 4.0)]),
]
s = socialforce.Simulator(initial_state, socialforce.PedSpacePotential(space))
states = np.stack([s.step().state.copy() for _ in range(80)])
# visualize
with visualize(states, space, 'docs/separator.gif') as ax:
ax.set_xlim(-10, 10)
def generate_sf_trajectory(sim_scene,
num_ped,
sf_params=[0.5, 2.1, 0.3],
end_range=0.2):
""" Simulating Scenario using SF """
## Default: (0.5, 2.1, 0.3)
sampling_rate = 1
## Circle Crossing
if sim_scene == 'circle_crossing':
fps = 10
sampling_rate = fps / 2.5
trajectories, positions, goals, speed = generate_circle_crossing(
num_ped)
else:
raise NotImplementedError
initial_state = np.array([[
positions[i][0], positions[i][1], speed[i][0], speed[i][1],
goals[i][0], goals[i][1]
] for i in range(num_ped)])
ped_ped = PedPedPotential(1. / fps, v0=sf_params[1], sigma=sf_params[2])
field_of_view = FieldOfView()
s = socialforce.Simulator(initial_state,
ped_ped=ped_ped,
field_of_view=field_of_view,
delta_t=1. / fps,
tau=sf_params[0])
# run
reaching_goal = [False] * num_ped
done = False
count = 0
#Simulate a scene
while not done and count < 500:
count += 1
position = np.stack(s.step().state.copy())
for i in range(len(initial_state)):
if count % sampling_rate == 0:
trajectories[i].append((position[i, 0], position[i, 1]))
# check if this agent reaches the goal
if np.linalg.norm(position[i, :2] -
np.array(goals[i])) < end_range:
reaching_goal[i] = True
else:
s.state[i, :4] = position[i, :4]
done = all(reaching_goal)
return trajectories, count
def test_crossing():
log = logging.getLogger('test_crossing')
agentNum = 30
state = []
for episode in range(10):
for num in range(agentNum):
v1 = random.uniform(-0.5, 0.5)
v2 = random.uniform(-0.5, 0.5)
p1 = random.uniform(0, 10)
p2 = random.uniform(0, 10)
p3 = random.uniform(0, 10)
p4 = random.uniform(0, 10)
p = [p1, p2, v1, v2, p3, p4]
if num == 0:
state = np.array([p])
else:
state = np.append(state, [p], 0)
log.debug(state)
print('hello')
initial_state2 = np.array([
[0.0, 0.0, 0.5, 0.5, 10.0, 10.0],
[10.0, 0.3, 0.5, 0.5, 0.0, 10.0],
])
initial_state = np.array(state)
print('state:', state)
print('initial state:', initial_state)
print('initial state2:', initial_state2)
s = socialforce.Simulator(initial_state)
states = np.stack([s.step().state.copy() for _ in range(50)])
# visualize
print('')
filename = 'crossing' + str(episode) + '.png'
with socialforce.show.canvas('docs/cross/' + filename) as ax:
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
for ped in range(agentNum):
x = states[:, ped, 0]
y = states[:, ped, 1]
#ax.plot(x, y, '-o', label='ped {}'.format(ped), markersize=2.5)
ax.plot(x, y, '-o', markersize=2.5)
ax.legend()
def test_f_ab():
s = socialforce.Simulator()
initial_state = s.normalize_state([
[0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
[1.0, 0.0, 0.0, 0.0, 1.0, 1.0],
])
force_at_unit_distance = 0.25 # confirmed below
assert s.f_ab(initial_state).detach().numpy() == pytest.approx(np.array([[
[0.0, 0.0],
[-force_at_unit_distance, 0.0],
], [
[force_at_unit_distance, 0.0],
[0.0, 0.0],
]]),
abs=0.05)
def test_fab():
initial_state = np.array([
[0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
[1.0, 0.0, 0.0, 0.0, 1.0, 1.0],
])
s = socialforce.Simulator(initial_state)
force_at_unit_distance = 0.25 # TODO confirm
assert s.f_ab() == pytest.approx(np.array([[
[0.0, 0.0],
[-force_at_unit_distance, 0.0],
], [
[force_at_unit_distance, 0.0],
[0.0, 0.0],
]]),
abs=0.05)
def test_opposing():
initial_state = np.array([
[0.0, 0.0, 1.0, 0.0, 0.0, 10.0],
[-0.3, 10.0, -1.0, 0.0, -0.3, 0.0],
])
s = socialforce.Simulator(initial_state)
states = np.stack([s.step().state.copy() for _ in range(21)])
# visualize
print('')
with socialforce.show.canvas('docs/opposing.png') as ax:
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
for ped in range(2):
x = states[:, ped, 0]
y = states[:, ped, 1]
ax.plot(x, y, '-o', label='ped {}'.format(ped), markersize=2.5)
ax.legend()
def test_gate():
initial_state = np.array([
[-9.0, -0.0, 1.0, 0.0, 10.0, 0.0],
[-10.0, -1.5, 1.0, 0.0, 10.0, 0.0],
[-10.0, -2.0, 1.0, 0.0, 10.0, 0.0],
[-10.0, -2.5, 1.0, 0.0, 10.0, 0.0],
[-10.0, -3.0, 1.0, 0.0, 10.0, 0.0],
[10.0, 1.0, -1.0, 0.0, -10.0, 0.0],
[10.0, 2.0, -1.0, 0.0, -10.0, 0.0],
[10.0, 3.0, -1.0, 0.0, -10.0, 0.0],
[10.0, 4.0, -1.0, 0.0, -10.0, 0.0],
[10.0, 5.0, -1.0, 0.0, -10.0, 0.0],
])
space = [
np.array([(0.0, y) for y in np.linspace(-10, -0.7, 1000)]),
np.array([(0.0, y) for y in np.linspace(0.7, 10, 1000)]),
]
s = socialforce.Simulator(initial_state, socialforce.PedSpacePotential(space))
states = np.stack([s.step().state.copy() for _ in range(150)])
with visualize(states, space, 'docs/gate.gif') as _:
pass
def test_opposing_mlp_scipy():
torch.manual_seed(42)
np.random.seed(42)
initial_state = torch.tensor([
[0.0, 0.0, 0.0, 1.0, 0.0, 10.0],
[-0.3, 10.0, 0.0, -1.0, -0.3, 0.0],
])
generator = socialforce.Simulator(initial_state)
truth = torch.stack(
[generator.step().state[:, 0:2].clone() for _ in range(21)]).detach()
print('============DONE WITH GENERATION===============')
V = socialforce.PedPedPotentialMLP(0.4)
parameters = V.get_parameters().clone().detach().numpy()
initial_parameters = parameters.copy()
# training
def f(x):
V.set_parameters(torch.from_numpy(x))
s = socialforce.Simulator(initial_state, None, V)
g = torch.stack([s.step().state[:, 0:2].clone() for _ in range(21)])
# average euclidean distance loss
diff = g - truth
loss = torch.mul(diff, diff).sum(dim=-1).mean()
return float(loss)
res = scipy.optimize.minimize(f,
parameters,
method='L-BFGS-B',
options=OPTIMIZER_OPT)
print(res)
# make plots of result
visualize('docs/mlp_scipy_', V, initial_parameters, res.x)
def gate(n, door_width, velocity, printGif, printPng):
door_x = 0.0 # 门的横坐标
destination = [3.0, 0.0] # 目的地坐标
range_x = [-10, door_x] # 初始状态人群位置的x范围
range_y = [-6.0, 6.0] # 初始状态人群位置的y范围
vel_x = velocity # x方向速度
vel_y = velocity # y方向速度
x_pos = np.random.random((n, 1)) * np.array([range_x[0]])
y_pos = ((np.random.random((n, 1)) - 0.5) * 2.0) * np.array(([range_y[1]]))
x_vel = np.full((n, 1), vel_x)
y_vel = np.full((n, 1), vel_y)
x_dest = np.full((n, 1), destination[0])
y_dest = np.full((n, 1), destination[1])
initial_state = np.concatenate(
(x_pos, y_pos, x_vel, y_vel, x_dest, y_dest), axis=-1)
print(initial_state)
space = [
np.array([(door_x, y)
for y in np.linspace(-10, -door_width / 2, 1000)]),
np.array([(door_x, y) for y in np.linspace(door_width / 2, 10, 1000)]),
]
s = socialforce.Simulator(initial_state,
socialforce.PedSpacePotential(space))
# 判断是否所有人都通过了门
states = []
i = 0
while True:
i += 1
state = s.step().state.copy()
states.append(state)
end = True
for sta in state:
if sta[0] < 0:
end = False
else:
sta[0] += 5.0
if end:
# print("terminate when time is ", i)
break
states = np.stack(states)
if printGif:
# 生成动态图，比较慢so我先注释掉
with visualize(states, space,
'out/gate{}-{}.gif'.format(n, door_width)) as _:
pass
if printPng:
# 生成路线图
with socialforce.show.canvas('out/gate{}-{}.png'.format(
n, door_width)) as ax:
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
for ped in range(len(initial_state)):
x = states[:, ped, 0]
y = states[:, ped, 1]
ax.plot(x, y, '-o')
ax.set_title("evacuate time {}".format(i))
ax.legend()
return i
def get_scene(self,
nr_agents,
nr_scenes,
agent_states,
threshold,
setting="random"):
"""
Function to generate each scene of the dataset.
The agents enter the scenario at one side of the setting and leave it at another side. An implementation of the Social Force Model is used to
describe the agents motion behavior including the interactions between the agents.
:param nr_agents: Number of agents in each scene
:param nr_scenes: Number of scenes of the dataset
:param agent_states: Numpy array of shape N x 6 for N initial agents in the dataset. Determines the initial states of the agents in the dataset.
The row of the i-th agent needs to be of the form: [x, y, v_x, v_y, d_x, d_y]. If the states should be initialized randomly, use agent_states = [] as input.
:param threshold: Specifies threshold for agents getting out of the scene (in meters)
:param setting: Either random or meeting
"""
print("Start generating scenes for dataset %s..." % (self.scenario))
# Initialize scene
if setting == "meeting":
self.initialize = meeting(self.scenario, self.v_max, self.v_min,
self.a, self.b, self.tau)
else:
self.initialize = random_initialization(self.scenario, self.v_max,
self.v_min, self.a, self.b,
self.tau)
self.initial_state = self.initialize_agents(self.v_max, self.v_min,
nr_agents, agent_states,
self.a, self.b, setting)
# Initialize simulator
self.s = socialforce.Simulator(self.initial_state,
self.V0,
self.sigma,
self.delta_t,
self.tau,
beta=self.beta)
# Get Array for scene numbers and agents
scene = np.zeros(nr_agents)
agents = np.arange(nr_agents) + 1
# Concatenate arrays with respective x-y-coordinates
dataset = np.concatenate((scene.reshape(
nr_agents, 1), agents.reshape(nr_agents, 1), self.s.state[:, 0:2]),
axis=1)
# Specify file location
root_path_experiments = os.path.abspath(
os.path.join(os.path.dirname(__file__), "..", "..", "Experiments"))
if not os.path.exists(root_path_experiments):
raise FileNotFoundError(errno.ENOENT, strerror(errno.ENOENT),
"Can not find Experiments folder!")
root_path_dataset = os.path.abspath(
os.path.join(
root_path_experiments, "datasets",
str(self.scenario) + "simulated" + "_" +
str(self.parameterinfo), str(self.phase)))
if not os.path.exists(root_path_dataset):
os.makedirs(root_path_dataset)
# Create background image
fig, ax = plt.subplots()
ax.grid(linestyle='dotted')
ax.set_aspect(1.0, 'datalim')
ax.set_axisbelow(True)
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_ylim(
[self.initialize.total_y_min - 2, self.initialize.total_y_max + 2])
ax.set_xlim(
[self.initialize.total_x_min - 2, self.initialize.total_x_max + 2])
ax.set_title(self.scenario + " V0: " + str(self.V0) + " - sigma: " +
str(self.sigma))
fig.set_tight_layout(True)
fig.savefig(root_path_dataset + "//" + str(self.scenario) +
"simulated.jpg")
plt.close()
# Create txt-file for dataset
txt_file = os.path.join(
root_path_dataset,
str(self.phase) + "_" + str(self.scenario) + "simulated.txt")
print("Create dataset as txt-file for scenario: " + str(self.scenario))
file = open(txt_file, "w")
np.savetxt(file, dataset, fmt="%.2f", delimiter="\t")
file.close()
file = open(txt_file, "a")
# Calculate states of each agent for every scene in the dataset
states = []
states.append(self.s.state.copy())
agent_list = []
for _ in range(nr_scenes):
state = self.s.step().state
agent_list.append(agents.copy())
scene += 1
# Avoid oscillations of pedestrians -> Agents that have been at the same place for a longer period of time are exit the scene and new agents enter it
oscillation_length = 20 # corresponds to: 20 * 0.4 = 8 seconds
if _ >= oscillation_length:
dist = np.linalg.norm(states[_][:, 0:2] -
states[_ - oscillation_length][:, 0:2],
axis=1)
agent_cond = agent_list[_] == agent_list[_ -
oscillation_length]
add_cond = (dist[:] < 0.8) & agent_cond
else:
add_cond = np.ones((state.shape[0])) > 1
# Boundary conditions - Determine when agents leave the scene
condition = (
state[:, 0] > self.initialize.total_x_max + threshold) | (
state[:, 0] < self.initialize.total_x_min - threshold) | (
state[:, 1] > self.initialize.total_y_max + threshold
) | (state[:, 1] <
self.initialize.total_y_min - threshold) | add_cond
agents[np.argwhere(condition)] = (
max(agents) + np.arange(len(np.argwhere(condition))) +
1).reshape(-1, 1)
# Generate new Agents when old Agents left the scene
if len(state[condition]) != 0:
state[condition] = (np.stack(
self.initialize.initialize_state(agents[np.argwhere(
condition)][i, :])
for i in range(len(state[condition])))).squeeze()
dataset = np.concatenate((scene.reshape(
nr_agents, 1), agents.reshape(nr_agents, 1), state[:, 0:2]),
axis=1)
# Write states to txt-file
np.savetxt(file, dataset, fmt="%.2f", delimiter="\t")
states.append(state.copy())
file.close()
states = np.stack(states)
return states
def predict(input_paths,
dest_dict=None,
dest_type='interp',
sf_params=[0.5, 2.1, 0.3],
predict_all=True,
n_predict=12,
obs_length=9):
pred_length = n_predict
def init_states(input_paths, start_frame, dest_dict, dest_type):
initial_state = []
for i, _ in enumerate(input_paths):
path = input_paths[i]
ped_id = path[0].pedestrian
past_path = [t for t in path if t.frame <= start_frame]
past_frames = [t.frame for t in path if t.frame <= start_frame]
future_path = [t for t in path if t.frame > start_frame]
len_path = len(past_path)
## To consider agent or not consider.
if start_frame in past_frames:
curr = past_path[-1]
## Velocity
if len_path >= 4:
stride = 3
prev = past_path[-4]
else:
stride = len_path - 1
prev = past_path[-len_path]
[v_x, v_y] = vel_state(prev, curr, stride)
## Destination
if dest_type == 'true':
if dest_dict is not None:
[d_x, d_y] = dest_dict[ped_id]
else:
raise ValueError
elif dest_type == 'interp':
[d_x, d_y] = dest_state(past_path, len_path)
elif dest_type == 'vel':
[d_x, d_y] = [pred_length * v_x, pred_length * v_y]
elif dest_type == 'pred_end':
[d_x, d_y] = [future_path[-1].x, future_path[-1].y]
else:
raise NotImplementedError
## Initialize State
initial_state.append([curr.x, curr.y, v_x, v_y, d_x, d_y])
return np.array(initial_state)
def vel_state(prev, curr, stride):
if stride == 0:
return [0, 0]
diff = np.array([curr.x - prev.x, curr.y - prev.y])
theta = np.arctan2(diff[1], diff[0])
speed = np.linalg.norm(diff) / (stride * 0.4)
return [speed * np.cos(theta), speed * np.sin(theta)]
def dest_state(path, length):
if length == 1:
return [path[-1].x, path[-1].y]
x = [t.x for t in path]
y = [t.y for t in path]
time = list(range(length))
f = interp1d(x=time, y=[x, y], fill_value='extrapolate')
return f(time[-1] + pred_length)
multimodal_outputs = {}
primary = input_paths[0]
neighbours_tracks = []
frame_diff = primary[1].frame - primary[0].frame
start_frame = primary[obs_length - 1].frame
first_frame = primary[obs_length - 1].frame + frame_diff
# initialize
initial_state = init_states(input_paths, start_frame, dest_dict, dest_type)
fps = 20
sampling_rate = int(fps / 2.5)
if len(initial_state) != 0:
# run
ped_ped = PedPedPotential(1. / fps,
v0=sf_params[1],
sigma=sf_params[2])
field_of_view = FieldOfView()
s = socialforce.Simulator(initial_state,
ped_ped=ped_ped,
field_of_view=field_of_view,
delta_t=1. / fps,
tau=sf_params[0])
states = np.stack([
s.step().state.copy() for _ in range(pred_length * sampling_rate)
])
## states : pred_length x num_ped x 7
states = np.array(
[s for num, s in enumerate(states) if num % sampling_rate == 0])
else:
## Stationary
past_path = [t for t in input_paths[0] if t.frame == start_frame]
states = np.stack([[[past_path[0].x, past_path[0].y]]
for _ in range(pred_length)])
# predictions
primary_track = states[:, 0, 0:2]
neighbours_tracks = states[:, 1:, 0:2]
## Primary Prediction Only
if not predict_all:
neighbours_tracks = []
# Unimodal Prediction
multimodal_outputs[0] = primary_track, neighbours_tracks
return multimodal_outputs
def test_circle_mlp(n, lr=0.1, delta_t=0.2):
torch.manual_seed(42)
np.random.seed(42)
# ped0 always left to right
ped0 = np.array([-5.0, 0.0, 1.0, 0.0, 5.0, 0.0])
generator_initial_states = []
for theta in np.random.rand(n) * 2.0 * math.pi:
# ped1 at a random angle with +/-20% speed variation
c, s = np.cos(theta), np.sin(theta)
r = np.array([[c, -s], [s, c]])
ped1 = np.concatenate((
np.matmul(r, ped0[0:2]),
np.matmul(r, ped0[2:4]), # * (0.8 + random.random() * 0.4)),
np.matmul(r, ped0[4:6]),
))
generator_initial_states.append(
torch.tensor(np.stack((ped0, ped1))).float())
if n == 1: # override for n=1
generator_initial_states = [
torch.tensor([
[0.0, 0.0, 0.0, 1.0, 0.0, 10.0],
[-0.3, 10.0, 0.0, -1.0, -0.3, 0.0],
])
]
# generator_v0 = 1.5 if n != 1 else 2.1
generator_ped_ped = socialforce.PedPedPotential(delta_t, 2.1)
true_scenes = []
for initial_state in generator_initial_states:
generator = socialforce.Simulator(initial_state,
ped_ped=generator_ped_ped,
delta_t=delta_t)
true_scenes.append(
[generator.step().state.clone().detach() for _ in range(42)])
true_experience = socialforce.Optimizer.scenes_to_experience(true_scenes)
print('============DONE WITH GENERATION===============')
V = socialforce.PedPedPotentialMLP(delta_t)
initial_parameters = V.get_parameters().clone().detach().numpy()
def simulator_factory(initial_state):
s = socialforce.Simulator(initial_state, ped_ped=V, delta_t=delta_t)
s.desired_speeds = generator.desired_speeds
s.max_speeds = generator.max_speeds
return s
opt = socialforce.Optimizer(simulator_factory, V.parameters(),
true_experience)
for i in range(100 // n):
loss = opt.epoch()
print('epoch {}: {}'.format(i, loss))
# make plots of result
visualize('docs/mlp_circle_n{}_'.format(n),
V,
initial_parameters,
V.get_parameters().clone(),
V_gen=generator_ped_ped)
def predict(input_paths, predict_all=False):
def init_states(input_paths, start_frame):
initial_state = []
for i in range(len(input_paths)):
path = input_paths[i]
past_path = [t for t in path if t.frame <= start_frame]
past_frames = [t.frame for t in path if t.frame <= start_frame]
l = len(past_path)
## To consider agent or not consider.
if (start_frame in past_frames) and l >= 4:
curr = past_path[-1]
prev = past_path[-4]
[v_x, v_y] = vel_state(prev, curr, 3)
if np.linalg.norm([v_x, v_y]) < 1e-6:
continue
[d_x, d_y] = dest_state(past_path, l-1)
if np.linalg.norm([curr.x - d_x, curr.y - d_y]) < 1e-6:
continue
initial_state.append([curr.x, curr.y, v_x, v_y, d_x, d_y])
return np.array(initial_state)
def vel_state(prev, curr, l):
diff = np.array([curr.x - prev.x, curr.y - prev.y])
theta = np.arctan2(diff[1], diff[0])
speed = np.linalg.norm(diff) / (l * 0.4)
return [speed*np.cos(theta), speed*np.sin(theta)]
def dest_state(path, l):
x = [t.x for t in path]
y = [t.y for t in path]
time = [i for i in range(l+1)]
f = interp1d(x=time, y=[x, y], fill_value='extrapolate')
return f(time[-1] + 12)
multimodal_outputs = {}
primary = input_paths[0]
neighbours_tracks = []
frame_diff = primary[1].frame - primary[0].frame
start_frame = primary[8].frame
first_frame = primary[8].frame + frame_diff
# initialize
initial_state = init_states(input_paths, start_frame)
# run
s = socialforce.Simulator(initial_state)
states = np.stack([s.step().state.copy() for _ in range(12)])
## states : 12 x num_ped x 7
# predictions
for i in range(states.shape[1]):
ped_id = input_paths[i][0].pedestrian
if i == 0:
primary_track = [trajnettools.TrackRow(first_frame + j * frame_diff, ped_id, x, y)
for j, (x, y) in enumerate(states[:, i, 0:2])]
else:
neighbours_tracks.append([trajnettools.TrackRow(first_frame + j * frame_diff, ped_id, x, y)
for j, (x, y) in enumerate(states[:, i, 0:2])])
## Primary Prediction
if not predict_all:
neighbours_tracks = []
# Unimodal Prediction
multimodal_outputs[0] = primary_track, neighbours_tracks
return multimodal_outputs
def get_scene(self, agent_states):
"""
Generate Sequence of Scene with obstacle-setting of the desired Input-Scene. For this, the input-scene-op.jpg image is read in an the obstacles are approximated accordingly.
:param scene: Name of input-scene
:param nr_agents: Number of agents that will be constantly in scene. Whenever an agent is leaving the scene, a new agent will spawn
:param nr_scenes: Number of Frames/Scenes in Sequence
:param image_path: Path to input-image for obstacle approximation
:param dataset_path: Path to output-txt-file (generated dataset)
:param create_background: Boolean - if True, create background image for sequence
:param create_video: Boolean - if True, create .mp4-file which visualizes the sequence
:return: Dataset (txt-file) and if chosen video and background image
"""
# agents have state vectors of the form (x, y, v_x, v_y, d_x, d_y, [tau])
print("Start generating scenes for dataset %s..." % (self.scenario))
# Initialize state for each agent
self.initial_state = self.initialize_agents(self.v_max, self.v_min, self.nr_agents, agent_states, self.a, self.b, setting="random")
# Get boundaries
initialize = random_initialization(self.scenario, self.v_max, self.v_min, a=self.a, b=self.b, tau=0.5)
self.total_y_min = initialize.total_y_min
self.total_y_max = initialize.total_y_max
self.total_x_min = initialize.total_x_min
self.total_x_max = initialize.total_x_max
# Initialize space according to input image
space = self.initialize_space(self.image_path, self.create_background, self.scaling)
# Get Socialforce Simulator
s = socialforce.Simulator(self.initial_state, ped_space=socialforce.PedSpacePotential(space, u0=self.U0, r=self.r), v0=self.V0, sigma=self.sigma)
# Get Array for scene numbers and agents
scene = np.zeros(self.nr_agents)
agents = np.arange(self.nr_agents) + 1
# Concatenate arrays with respective x-y-coordinates
dataset = np.concatenate((scene.reshape(self.nr_agents, 1), agents.reshape(self.nr_agents, 1), s.state[:, 0:2]), axis=1)
# Create txt-file for dataset
print("Create dataset as txt-file for scenario: %s" % (self.scenario))
file = open(self.dataset_path, "w")
np.savetxt(file, dataset, fmt="%.2f", delimiter="\t")
file.close()
file = open(self.dataset_path, "a")
# Calculate states of each agent for every scene in the dataset
states = []
agent_list = []
states.append(s.state.copy())
for _ in range(self.nr_scenes):
state = s.step().state
agent_list.append(agents.copy())
scene += 1
# Avoid oscillations of pedestrians -> Agents that have been at the same place for a longer period of time are exit the scene and new agents enter it
oscillation_length = 20 # corresponds to: 20 * 0.4 = 8 seconds
if _ >= oscillation_length:
dist = np.linalg.norm(states[_][:, 0:2] - states[_ - oscillation_length][:, 0:2], axis=1)
agent_cond = agent_list[_] == agent_list[_ - oscillation_length]
add_cond = (dist[:] < 0.8) & agent_cond
else:
add_cond = np.ones((state.shape[0])) > 1
# Boundary conditions - Determine when agents leave the scene
condition = (state[:, 0] > self.total_x_max + self.threshold) | (state[:, 0] < self.total_x_min - self.threshold) | (state[:, 1] > self.total_y_max + self.threshold) | (state[:, 1] < self.total_y_min - self.threshold) | add_cond
agents[np.argwhere(condition)] = (max(agents) + np.arange(len(np.argwhere(condition))) + 1).reshape(-1, 1)
# Generate new Agents when old Agents left the scene
if len(state[condition]) != 0:
state[condition] = (np.stack(initialize.initialize_state(agents[np.argwhere(condition)][i, :]) for i in range(len(state[condition])))).squeeze()
dataset = np.concatenate((scene.reshape(self.nr_agents, 1), agents.reshape(self.nr_agents, 1), state[:, 0:2]), axis=1)
np.savetxt(file, dataset, fmt="%.2f", delimiter="\t")
states.append(state.copy())
file.close()
states = np.stack(states)
print("Dataset created.")
# Show animation of simulation
if self.show_animation:
print("Start creating video for visualization of simulated scenario " + str(self.scenario) + "...")
with self.animate_scenes(states=states, space=space, sim=s) as _:
pass
print("Video created.")
def simulator_factory(initial_state):
s = socialforce.Simulator(initial_state, ped_ped=V, delta_t=delta_t)
s.desired_speeds = generator.desired_speeds
s.max_speeds = generator.max_speeds
return s
def reset(self, phase='test', test_case=None):
"""
Set px, py, gx, gy, vx, vy, theta for robot and humans
:return:
"""
if self.robot is None:
raise AttributeError('robot has to be set!')
assert phase in ['train', 'val', 'test']
if test_case is not None:
self.case_counter[phase] = test_case
self.global_time = 0
if phase == 'test':
self.human_times = [0] * self.human_num
else:
self.human_times = [0] * (
self.human_num if self.robot.policy.multiagent_training else 1)
#if not self.robot.policy.multiagent_training:
# self.train_val_sim = 'circle_crossing'
if self.config.get('humans', 'policy') == 'trajnet':
raise NotImplementedError
else:
counter_offset = {
'train':
self.case_capacity['val'] + self.case_capacity['test'],
'val': 0,
'test': self.case_capacity['val']
}
self.robot.set(0, -self.circle_radius, 0, self.circle_radius, 0, 0,
np.pi / 2)
if self.case_counter[phase] >= 0:
#np.random.seed(counter_offset[phase] + self.case_counter[phase])
if phase in ['train', 'val']:
human_num = self.human_num if self.robot.policy.multiagent_training else 1
self.generate_random_human_position(
human_num=human_num, rule=self.train_val_sim)
else:
self.generate_random_human_position(
human_num=self.human_num, rule=self.test_sim)
# case_counter is always between 0 and case_size[phase]
self.case_counter[phase] = (self.case_counter[phase] +
1) % self.case_size[phase]
else:
assert phase == 'test'
if self.case_counter[phase] == -1:
# for debugging purposes
self.human_num = 3
self.humans = [
Human(self.config, 'humans')
for _ in range(self.human_num)
]
self.humans[0].set(0, -6, 0, 5, 0, 0, np.pi / 2)
self.humans[1].set(-5, -5, -5, 5, 0, 0, np.pi / 2)
self.humans[2].set(5, -5, 5, 5, 0, 0, np.pi / 2)
else:
raise NotImplementedError
###
if self.domain_settings is not None:
for human in self.humans:
human.modify_policy(self.domain_settings)
for agent in [self.robot] + self.humans:
agent.time_step = self.time_step
agent.policy.time_step = self.time_step
self.states = list()
if hasattr(self.robot.policy, 'action_values'):
self.action_values = list()
if hasattr(self.robot.policy, 'get_attention_weights'):
self.attention_weights = list()
# get current observation
if self.robot.sensor == 'coordinates':
ob = [human.get_observable_state() for human in self.humans]
elif self.robot.sensor == 'RGB':
raise NotImplementedError
self.simulator = socialforce.Simulator(
self.humans_to_sf_state(self.humans, init=True),
ped_ped=socialforce.PedPedPotential(self.delta_t, self.v0,
self.sigma),
delta_t=self.delta_t,
tau=self.tau)
return ob
def get_scene(self,
nr_agents,
nr_scenes,
agent_states,
threshold,
setting="random"):
"""
Method overriding. In this case agents do not leave the scenario at a specific side of the scene, but remain seq_len time steps in the scene.
After seq_len time steps all agents are replaced by an equal number of new agents. An implementation of the Social Force Model is used to
describe the agents motion behavior including the interactions between the agents.
:param nr_agents: Number of agents in each scene
:param nr_scenes: Number of scenes of the dataset
:param agent_states: Numpy array of shape N x 6 for N initial agents in the dataset. Determines the initial states of the agents in the dataset.
The row of the i-th agent needs to be of the form: [x, y, v_x, v_y, d_x, d_y]. If the states should be initialized randomly, use agent_states = [] as input.
:param threshold: Specifies the threshold for agents getting out of the scene (in meters)
:param setting: Either random or meeting
"""
if setting == "meeting":
self.initialize = meeting(self.scenario, self.v_max, self.v_min,
self.a, self.b, self.tau)
else:
self.initialize = random_initialization(self.scenario, self.v_max,
self.v_min, self.a, self.b,
self.tau)
self.initial_state = self.initialize_agents(self.v_max, self.v_min,
nr_agents, agent_states,
self.a, self.b, setting)
self.s = socialforce.Simulator(self.initial_state,
self.V0,
self.sigma,
self.delta_t,
self.tau,
beta=self.beta)
# Get Array for scene numbers and agents
scene = np.zeros(nr_agents)
agents = np.arange(nr_agents) + 1
# Concatenate arrays with respective x-y-coordinates
dataset = np.concatenate((scene.reshape(
nr_agents, 1), agents.reshape(nr_agents, 1), self.s.state[:, 0:2]),
axis=1)
# Specify file location
root_path_experiments = os.path.abspath(
os.path.join(os.path.dirname(__file__), "..", "..", "Experiments"))
if not os.path.exists(root_path_experiments):
raise FileNotFoundError(errno.ENOENT, strerror(errno.ENOENT),
"Can not find Experiments folder!")
root_path_dataset = os.path.abspath(
os.path.join(
root_path_experiments, "datasets",
str(self.scenario) + "simulated" + "_" +
str(self.parameterinfo), str(self.phase)))
if not os.path.exists(root_path_dataset):
os.makedirs(root_path_dataset)
# Create background image
fig, ax = plt.subplots()
ax.grid(linestyle='dotted')
ax.set_aspect(1.0, 'datalim')
ax.set_axisbelow(True)
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_ylim(
[self.initialize.total_y_min - 2, self.initialize.total_y_max + 2])
ax.set_xlim(
[self.initialize.total_x_min - 2, self.initialize.total_x_max + 2])
ax.set_title(self.scenario + " V0: " + str(self.V0) + " - sigma: " +
str(self.sigma))
fig.set_tight_layout(True)
fig.savefig(root_path_dataset + "//" + str(self.scenario) +
"simulated.jpg")
plt.close()
txt_file = os.path.join(
root_path_dataset,
str(self.phase) + "_" + str(self.scenario) + "simulated.txt")
print("Create dataset as txt-file for scene " + str(self.scenario))
file = open(txt_file, "w")
np.savetxt(file, dataset, fmt="%.2f", delimiter="\t")
file.close()
file = open(txt_file, "a")
# Repeat for each scene and update according steps of agents
states = []
states.append(self.s.state.copy())
for _ in range(nr_scenes):
state = self.s.step().state
scene += 1
# Periodic Boundary condition
if _ != 0 and (_ + 1) % self.seq_len == 0:
# Generate new Agents after seq_len timesteps
state[:] = np.stack(
self.initialize.initialize_state(i)
for i in range(len(state))).squeeze()
# Update IDs
agents += len(agents)
dataset = np.concatenate((scene.reshape(
nr_agents, 1), agents.reshape(nr_agents, 1), state[:, 0:2]),
axis=1)
np.savetxt(file, dataset, fmt="%.2f", delimiter="\t")
states.append(state.copy())
file.close()
states = np.stack(states)
return states
