def visualize(trajectories):
def state_to_dict(state):
mat = [state[0:3], state[3:6], state[6:9]]
return {'position': state[9:12], 'rotation_matrix': mat}
visualizer = Visualizer(0.046 * 50)
b = 1
for i in range(
len(trajectories[-1]['states']) + trajectories[-1]['position'] +
trajectories[b]['level']):
for j in range(b, len(trajectories)):
if trajectories[b]['position'] + trajectories[b]['level'] + 1 == i:
visualizer.add_quadrotor()
b += 1
for j in range(b):
if j == 0:
idx = i
else:
idx = i - (trajectories[j]['position'] +
trajectories[j]['level'] + 1)
if idx >= len(trajectories[j]['states']):
continue
visualizer.update(state_to_dict(trajectories[j]['states'][idx]), j)
