a0 = np.array([0.])
angle0 = np.random.random((1,))*2*np.pi # np.zeros((1,)) #
angle_v0 = np.array([0.])
angle_a0 = np.array([0.])
pm = PointMass(position0, p0, v0, a0, angle0, angle_v0, angle_a0)
### Run Simulation ###
# while running:
for _ in range(500):
# event = pygame.event.poll()
# if event.type == pygame.QUIT:
# running = 0
# screen.fill(bgcolor)
if t == 0:
pm.acceleration = np.random.random((1,)) * 500
pm.angle_acceleration = (np.random.random((1,))-0.5) * 10 * np.pi # np.zeros((1,)) #
# print pm.angle_acceleration
elif t < T:
# probly better to alloc zeros right at the start
pm.acceleration = np.append(pm.acceleration, pm.acceleration[-1])
pm.angle_acceleration = np.append(pm.angle_acceleration, pm.angle_acceleration[-1])
# print pm.angle_acceleration
else:
pm.acceleration = np.append(pm.acceleration, a0.reshape((1,)))
pm.angle_acceleration = np.append(pm.angle_acceleration, angle_a0.reshape((1,)))
pm.motion(dt)
# boundaries(pm, width, height)
# pm.display(screen)
# target.display(screen)
running = 0
screen.fill(bgcolor)
if t == 0:
### angle_start, x_start, y_start, angle_end, x_end, y_end, acceleration, angle_acceleration, T
feature_pre = np.asarray(
(pm.angle[0], pm.pos[0], pm.pos[1], target.angle[0],
target.pos[0], target.pos[1]))
# standardize feature
feature = (feature_pre - data_mean[0:6]) / data_std[0:6]
print feature_pre, feature
# predict motor_commands
motor_commands = inverse.predict(feature.reshape([1, 6]))
print "motor_commands", motor_commands, data_std[6], data_mean[6]
# Go back to original scale of motor_commands
pm.acceleration = (np.array([motor_commands[0][0]]) *
data_std[6]) + data_mean[6]
print "pm.acceleration", pm.acceleration
pm.angle_acceleration = np.array([motor_commands[0][1]
]) * data_std[7] + data_mean[7]
elif t < T:
pm.acceleration = np.append(pm.acceleration, pm.acceleration[-1])
pm.angle_acceleration = np.append(pm.angle_acceleration,
pm.angle_acceleration[-1])
# print pm.angle_acceleration
else:
pm.acceleration = np.append(pm.acceleration, a0.reshape((1, )))
pm.angle_acceleration = np.append(pm.angle_acceleration,
angle_a0.reshape((1, )))
pm.motion(dt)
# boundaries(pm, width, height)
event = pygame.event.poll()
if event.type == pygame.QUIT:
running = 0
screen.fill(bgcolor)
if t == 0:
### angle_start, x_start, y_start, angle_end, x_end, y_end, acceleration, angle_acceleration, T
feature_pre = np.asarray((pm.angle[0], pm.pos[0], pm.pos[1], target.angle[0], target.pos[0], target.pos[1]))
# standardize feature
feature = (feature_pre - data_mean[0:6]) / data_std[0:6]
print feature_pre, feature
# predict motor_commands
motor_commands = inverse.predict(feature.reshape([1, 6]))
print "motor_commands", motor_commands, data_std[6], data_mean[6]
# Go back to original scale of motor_commands
pm.acceleration = (np.array([motor_commands[0][0]]) * data_std[6]) + data_mean[6]
print "pm.acceleration", pm.acceleration
pm.angle_acceleration = np.array([motor_commands[0][1]]) * data_std[7] + data_mean[7]
elif t < T:
pm.acceleration = np.append(pm.acceleration, pm.acceleration[-1])
pm.angle_acceleration = np.append(pm.angle_acceleration, pm.angle_acceleration[-1])
# print pm.angle_acceleration
else:
pm.acceleration = np.append(pm.acceleration, a0.reshape((1,)))
pm.angle_acceleration = np.append(pm.angle_acceleration, angle_a0.reshape((1,)))
pm.motion(dt)
# boundaries(pm, width, height)
pm.display(screen)
target.display(screen)
pygame.display.flip()
a0 = np.array([0.])
angle0 = np.random.random((1, )) * 2 * np.pi # np.zeros((1,)) #
angle_v0 = np.array([0.])
angle_a0 = np.array([0.])
pm = PointMass(position0, p0, v0, a0, angle0, angle_v0, angle_a0)
### Run Simulation ###
# while running:
for _ in range(500):
# event = pygame.event.poll()
# if event.type == pygame.QUIT:
# running = 0
# screen.fill(bgcolor)
if t == 0:
pm.acceleration = np.random.random((1, )) * 500
pm.angle_acceleration = (np.random.random(
(1, )) - 0.5) * 10 * np.pi # np.zeros((1,)) #
# print pm.angle_acceleration
elif t < T:
# probly better to alloc zeros right at the start
pm.acceleration = np.append(pm.acceleration, pm.acceleration[-1])
pm.angle_acceleration = np.append(pm.angle_acceleration,
pm.angle_acceleration[-1])
# print pm.angle_acceleration
else:
pm.acceleration = np.append(pm.acceleration, a0.reshape((1, )))
pm.angle_acceleration = np.append(pm.angle_acceleration,
angle_a0.reshape((1, )))
pm.motion(dt)
