def sine_test(ser):
for iteration in range(SINE_ITERATIONS):
for actuator in range(5):
send_angles[actuator] = SINE_OFFSET + \
int(SINE_AMPLITUDE * sin((DEG2RAD * iteration + actuator * SINE_DELAY) * SINE_SPEED).real)
set_servo_angles(ser, send_angles)
time.sleep(0.01)
def _reset(self):
# Perform a hardware reset:
# Create a v-shape to let the ball roll down
# Wiggle each actuator to make sure it is at the bottom
# Move all actuators to a rest position
self.object_at_target = False
self.object_at_target_count = 0
# Set lowest position for actuators
# reset_values = [125, 105, 90, 105, 125]
# Reset used to randomize the new position
reset_speeds = [0.5 + np.random.rand() / 2,
0.5 + np.random.rand() / 2,
0.5 + np.random.rand() / 2,
0.5 + np.random.rand() / 2,
0.5 + np.random.rand() / 2]
hardware_interface.set_servo_speeds(self.serial, reset_speeds)
time.sleep(0.5)
reset_values = [90 + np.random.rand() * 40,
90 + np.random.rand() * 40,
90 + np.random.rand() * 40,
90 + np.random.rand() * 40,
90 + np.random.rand() * 40]
hardware_interface.set_servo_angles(self.serial, reset_values)
time.sleep(0.5)
self.time_previous = time.time()
return self._step(np.array([0, 0, 0, 0, 0]))[0] # action: zero motor speed
def slamming_test(ser):
for iteration in range(SLAMMING_ITERATIONS):
for actuator in range(5):
if iteration % 2 is 0:
send_angles[actuator] = MIN_ACTUATOR_ANGLE
else:
send_angles[actuator] = MAX_ACTUATOR_ANGLE
set_servo_angles(ser, send_angles)
time.sleep(SLAMMING_DELAY)
def vibration_test(ser):
for test_num in range(len(VIBRATION_AMPLITUDES)):
amplitude = VIBRATION_AMPLITUDES[test_num]
delay = VIBRATION_DELAY[test_num]
print('Amplitude: ' + amplitude.__str__())
for iteration in range(VIBRATION_ITERATIONS):
for actuator in range(5):
# python doesen't have xor :(
if xor(actuator % 2 is 0, iteration % 2 is 0):
send_angles[actuator] = VIBRATION_OFFSET
else:
send_angles[actuator] = VIBRATION_OFFSET + amplitude
set_servo_angles(ser, send_angles)
time.sleep(delay)
def _step(self, action):
# Uncomment to use a programmed policy
# if self.prev_state is not None:
# action = self.programmed_policy(self.prev_state)
self.count = self.count + 1
# Totally recall previous state
ouc_x = self.previous_pos[0]
ouc_y = self.previous_pos[1]
actuators_y = self.previous_pos[2:7]
# Capture an image and process it
self.camera_capture = capture.undistort(
capture.capture_frame(self.arena_camera),
np.array(CAMERA_CONFIG.camera_matrix),
np.array(CAMERA_CONFIG.dist_coefs))
ouc_list = capture.track_objects(self.camera_capture, self.ouc_params)
actuator_list = capture.track_objects(self.camera_capture,
self.actuator_params)
if len(ouc_list) != 0:
ouc_main = capture.find_largest(ouc_list)
self.ouc_ui_pos = (ouc_main[0], ouc_main[1])
# Convert pixels to mm relative to bottom left arena corner
ouc_x = ouc_main[0] * PIX2MM - BOX_TRIM_LEFT
ouc_y = (VIEWPORT_H - ouc_main[1]) * PIX2MM - BOX_TRIM_BOTTOM
else:
pass
# print('No \'ouc\' found!')
if len(actuator_list) != 0:
for i in range(5):
index = 4 - i
temp = capture.find_largest_in_area(actuator_list,
ACTUATOR_X1[index],
ACTUATOR_Y1[index],
ACTUATOR_X2[index],
ACTUATOR_Y2[index])
if temp != ():
self.actuators_ui_pos[index] = (temp[0], temp[1])
actuators_y[index] = (VIEWPORT_H -
temp[1]) * PIX2MM - BOX_TRIM_BOTTOM
actuator_list.remove(temp)
else:
pass
# print('No actuators found')
# All measurements are now in mm from the lower left corner
# Calculate velocity of actuators using the previous value
current_time = time.time()
delta_t = current_time - self.time_previous
self.time_previous = current_time
# print('Freq: ' + (1/delta_t).__str__())
# Velocities are in mm/s
object_vel = [
(ouc_x - self.previous_pos[0]) / delta_t,
(ouc_y - self.previous_pos[1]) / delta_t,
]
actuator_vel = [(actuators_y[0] - self.previous_pos[2]) / delta_t,
(actuators_y[1] - self.previous_pos[3]) / delta_t,
(actuators_y[2] - self.previous_pos[4]) / delta_t,
(actuators_y[3] - self.previous_pos[5]) / delta_t,
(actuators_y[4] - self.previous_pos[6]) / delta_t]
self.previous_pos = [
ouc_x, ouc_y, actuators_y[0], actuators_y[1], actuators_y[2],
actuators_y[3], actuators_y[4]
]
# Observation space (state)
state = [
np.clip((TARGET_POS[0] - BOX_WIDTH / 2) / (BOX_WIDTH / 2), -1, 1),
np.clip((TARGET_POS[1] - BOX_HEIGHT / 2) / (BOX_HEIGHT / 2), -1,
1),
np.clip((ouc_x - BOX_WIDTH / 2) / (BOX_WIDTH / 2), -1, 1),
np.clip((ouc_y - BOX_HEIGHT / 2) / (BOX_HEIGHT / 2), -1, 1),
np.clip(object_vel[0] / MAX_SPEED, -1, 1),
np.clip(object_vel[1] / MAX_SPEED, -1, 1),
np.clip((actuators_y[0] - ACTUATOR_TRANSLATION_MEAN) /
ACTUATOR_TRANSLATION_AMP, -1, 1),
np.clip((actuators_y[1] - ACTUATOR_TRANSLATION_MEAN) /
ACTUATOR_TRANSLATION_AMP, -1, 1),
np.clip((actuators_y[2] - ACTUATOR_TRANSLATION_MEAN) /
ACTUATOR_TRANSLATION_AMP, -1, 1),
np.clip((actuators_y[3] - ACTUATOR_TRANSLATION_MEAN) /
ACTUATOR_TRANSLATION_AMP, -1, 1),
np.clip((actuators_y[4] - ACTUATOR_TRANSLATION_MEAN) /
ACTUATOR_TRANSLATION_AMP, -1, 1),
np.clip(actuator_vel[0] / MAX_SPEED, -1, 1),
np.clip(actuator_vel[1] / MAX_SPEED, -1, 1),
np.clip(actuator_vel[2] / MAX_SPEED, -1, 1),
np.clip(actuator_vel[3] / MAX_SPEED, -1, 1),
np.clip(actuator_vel[4] / MAX_SPEED, -1, 1)
]
self.prev_state = state
assert len(state) == 16
# For debug puroposes:
# print('OUC pos: {:.2f},{:.2f}'.format(state[0], state[1]))
# print('actuator vel: {:.2f},{:.2f},{:.2f},{:.2f},{:.2f}'.format(state[9], state[10], state[11], state[12], state[13]))
# print('actuator pos: {:.2f},{:.2f},{:.2f},{:.2f},{:.2f}'.format(state[4], state[5], state[6], state[7], state[8]))
# Set motor speeds from the action outputs
send_values = [0, 0, 0, 0, 0]
for i in range(5):
send_values[i] = float(np.clip(action[i], -1.0, 1.0))
# Now make sure that if we are at the edge of the actuator movement, we don't actually move it more:
for i in range(5):
if abs(state[6 + i] - 1) < epsilon:
send_values[i] = np.clip([send_values[i]], -1.0, 0.0)[0]
continue
if abs(state[6 + i] + 1) < epsilon:
send_values[i] = np.clip([send_values[i]], 0.0, 1.0)[0]
# Temporary speed reduction until retrain
send_values = np.array(send_values) * 20 + 110
# print('Send speeds: {:.2f},{:.2f},{:.2f},{:.2f},{:.2f}'.format(
# send_values[0], send_values[1], send_values[2], send_values[3], send_values[4]))
hardware_interface.set_servo_angles(self.serial, send_values)
# Rewards
# dist_to_target = TARGET_POS[0]-self.object.position.x
# dist_to_target = np.sqrt(np.square(TARGET_POS[0] - ouc_x) + np.square(TARGET_POS[1] - ouc_y))
reward = 0
shaping = \
- 200 * np.abs(TARGET_POS[0] - ouc_x) / BOX_WIDTH \
- 200 * np.abs(TARGET_POS[1] - ouc_y) / BOX_HEIGHT \
- 50 * np.abs(state[4]) \
- 50 * np.abs(state[5])
if self.prev_shaping is not None:
reward = shaping - self.prev_shaping
self.prev_shaping = shaping
if (np.abs(ouc_x - TARGET_POS[0])) < 15:
if (np.abs(ouc_y - TARGET_POS[1])) < 15:
reward += 50
# Reduce reward for using the motor
for a in action:
reward -= 1 * np.clip(np.abs(a), 0, 1)
done = False
return np.array(state), reward, done, {}
MAX_SPEED_INPUT = 1.0
spd = 5
if __name__ == '__main__':
ser = init_serial()
if control_speed:
while True:
key = getch.getch()
if key == '\n':
continue
if key == '0':
send_angles[0] += 60
send_angles[1] += 60
set_servo_angles(ser, send_angles)
continue
if key == '=':
set_servo_angles(ser, reset_angles)
continue
if key == '1':
set_servo_speeds(ser, smol_speeds)
print(smol_speeds)
continue
if key == '!':
set_servo_speeds(ser, -np.array(smol_speeds))
print(-np.array(smol_speeds))
continue
if key == '2':
set_servo_speeds(ser, np.array(smol_speeds) * spd)
print(np.array(smol_speeds) * spd)