def infer(self, in_dims, out_dims, x):
if self.t < max(self.model.imodel.fmodel.k, self.model.imodel.k):
raise ExplautoBootstrapError
if in_dims == self.m_dims and out_dims == self.s_dims: # forward
return array(self.model.predict_effect(tuple(x)))
elif in_dims == self.s_dims and out_dims == self.m_dims: # inverse
if self.mode == 'explore':
self.mean_explore = array(self.model.infer_order(tuple(x)))
r = self.mean_explore
r[self.sigma_expl > 0] = np.random.normal(r[self.sigma_expl > 0], self.sigma_expl[self.sigma_expl > 0])
res = bounds_min_max(r, self.m_mins, self.m_maxs)
return res
else: # exploit'
return array(self.model.infer_order(tuple(x)))
elif out_dims == self.m_dims[len(self.m_dims)/2:]: # dm = i(M, S, dS)
assert len(x) == len(in_dims)
m = x[:self.m_ndims/2]
s = x[self.m_ndims/2:][:self.s_ndims/2]
ds = x[self.m_ndims/2:][self.s_ndims/2:]
self.mean_explore = array(self.model.imodel.infer_dm(m, s, ds))
if self.mode == 'explore':
r = np.random.normal(self.mean_explore, self.sigma_expl[out_dims])
res = bounds_min_max(r, self.m_mins[out_dims], self.m_maxs[out_dims])
return res
else:
return self.mean_explore
else:
raise NotImplementedError
def infer(self, in_dims, out_dims, x):
if self.t < max(self.model.imodel.fmodel.k, self.model.imodel.k):
raise ExplautoBootstrapError
if in_dims == self.m_dims and out_dims == self.s_dims: # forward
return array(self.model.predict_effect(tuple(x)))
elif in_dims == self.s_dims and out_dims == self.m_dims: # inverse
if not self.bootstrapped_s:
# If only one distinct point has been observed in the sensory space, then we output a random motor command
return rand_bounds(np.array([self.m_mins, self.m_maxs]))[0]
else:
if self.mode == 'explore':
self.mean_explore = array(self.model.infer_order(tuple(x)))
r = self.mean_explore
r[self.sigma_expl > 0] = np.random.normal(
r[self.sigma_expl > 0],
self.sigma_expl[self.sigma_expl > 0])
res = bounds_min_max(r, self.m_mins, self.m_maxs)
return res
else: # exploit'
return array(self.model.infer_order(tuple(x)))
elif out_dims == self.m_dims[len(self.m_dims) /
2:]: # dm = i(M, S, dS)
if not self.bootstrapped_s:
# If only one distinct point has been observed in the sensory space, then we output a random motor command
return rand_bounds(
np.array([
self.m_mins[self.m_ndims / 2:],
self.m_maxs[self.m_ndims / 2:]
]))[0]
else:
assert len(x) == len(in_dims)
m = x[:self.m_ndims / 2]
s = x[self.m_ndims / 2:][:self.s_ndims / 2]
ds = x[self.m_ndims / 2:][self.s_ndims / 2:]
self.mean_explore = array(self.model.imodel.infer_dm(m, s, ds))
if self.mode == 'explore':
r = np.random.normal(self.mean_explore,
self.sigma_expl[out_dims])
res = bounds_min_max(r, self.m_mins[out_dims],
self.m_maxs[out_dims])
return res
else:
return self.mean_explore
else:
raise NotImplementedError
def infer(self, in_dims, out_dims, x):
if self.t < max(self.model.imodel.fmodel.k, self.model.imodel.k):
res = rand_bounds(np.array([self.m_mins,
self.m_maxs]))[0]
return res, None
if in_dims == self.m_dims and out_dims == self.s_dims: # forward
return array(self.model.predict_effect(tuple(x)))
elif in_dims == self.s_dims and out_dims == self.m_dims: # inverse
if not self.bootstrapped_s:
# If only one distinct point has been observed in the sensory space, then we output a random motor command
res = rand_bounds(np.array([self.m_mins,
self.m_maxs]))[0]
sp = array(self.model.predict_effect(tuple(res)))
return res, sp
else:
self.mean_explore = array(self.model.infer_order(tuple(x)))
if self.mode == 'explore':
r = self.mean_explore
r[self.sigma_expl > 0] = np.random.normal(r[self.sigma_expl > 0], self.sigma_expl[self.sigma_expl > 0])
res = bounds_min_max(r, self.m_mins, self.m_maxs)
sp = array(self.model.predict_effect(tuple(res)))
return res, sp
else: # exploit'
res = array(self.model.infer_order(tuple(x)))
sp = array(self.model.predict_effect(tuple(res)))
return res, sp
else:
raise NotImplementedError
def infer(self, in_dims, out_dims, x):
if self.t < max(self.model.imodel.fmodel.k, self.model.imodel.k):
raise ExplautoBootstrapError
if in_dims == self.m_dims and out_dims == self.s_dims: # forward
return array(self.model.predict_effect(tuple(x))), -1
elif in_dims == self.s_dims and out_dims == self.m_dims: # inverse
if not self.bootstrapped_s:
# If only one distinct point has been observed in the sensory space, then we output a random motor command
return rand_bounds(np.array([self.m_mins, self.m_maxs]))[0], -1
else:
self.mean_explore = array(self.model.infer_order(tuple(x)))
idx = -1
# Check if nearest s was demonstrated
if np.linalg.norm(self.mean_explore) == 0:
idx = self.model.imodel.fmodel.dataset.nn_y(x)[1][0]
#print "demonstrated idx", idx
if self.mode == 'explore':
r = self.mean_explore
r[self.sigma_expl > 0] = np.random.normal(
r[self.sigma_expl > 0],
self.sigma_expl[self.sigma_expl > 0])
res = bounds_min_max(r, self.m_mins, self.m_maxs)
return res, idx
else: # exploit'
return array(self.model.infer_order(tuple(x))), idx
else:
raise NotImplementedError
def w_to_trajectory(self, w):
normalized_traj = bounds_min_max(
self.motor_dmp.trajectory(np.array(w) * self.max_params),
self.n_dmps * [-1.], self.n_dmps * [1.])
return ((normalized_traj - np.array([-1.] * self.n_dmps)) /
2.) * (self.bounds_motors_max -
self.bounds_motors_min) + self.bounds_motors_min
def sensory_trajectory_msg_to_list(self, state):
def flatten(list2d):
return [element2 for element1 in list2d for element2 in element1]
state_dict = {}
state_dict['hand'] = flatten([
(point.hand.pose.position.x, point.hand.pose.position.y,
point.hand.pose.position.z) for point in state.points
])
state_dict['joystick_1'] = flatten(
[point.joystick_1.axes for point in state.points])
state_dict['joystick_2'] = flatten(
[point.joystick_2.axes for point in state.points])
state_dict['ergo'] = flatten([(point.ergo.angle,
float(point.ergo.extended))
for point in state.points])
state_dict['ball'] = flatten([(point.ball.angle,
float(point.ball.extended))
for point in state.points])
state_dict['light'] = [point.color.data for point in state.points]
state_dict['sound'] = [point.sound.data for point in state.points]
self.context = {
'ball': state_dict['ball'][0],
'ergo': state_dict['ergo'][0]
}
rospy.loginfo("Context {}".format(self.context))
assert len(state_dict['hand']) == 30, len(state_dict['hand'])
assert len(state_dict['joystick_1']) == 20, len(
state_dict['joystick_1'])
assert len(state_dict['joystick_2']) == 20, len(
state_dict['joystick_2'])
assert len(state_dict['ergo']) == 20, len(state_dict['ergo'])
assert len(state_dict['ball']) == 20, len(state_dict['ball'])
assert len(state_dict['light']) == 10, len(state_dict['light'])
assert len(state_dict['sound']) == 10, len(state_dict['sound'])
# Concatenate all these values in a huge 132-float list
s_bounded = np.array([self.context['ergo'], self.context['ball']] + [
value for space in [
'hand', 'joystick_1', 'joystick_2', 'ergo', 'ball', 'light',
'sound'
] for value in state_dict[space]
])
s_normalized = ((s_bounded - self.bounds_sensory_min) /
self.bounds_sensory_diff) * 2 + np.array([-1.] * 132)
s_normalized = bounds_min_max(s_normalized, 132 * [-1.], 132 * [1.])
# print "context", s_bounded[:2], s_normalized[:2]
# print "hand", s_bounded[2:32], s_normalized[2:32]
# print "joystick_1", s_bounded[32:52], s_normalized[32:52]
# print "joystick_2", s_bounded[52:72], s_normalized[52:72]
# print "ergo", s_bounded[72:92], s_normalized[72:92]
# print "ball", s_bounded[92:112], s_normalized[92:112]
# print "light", s_bounded[112:122], s_normalized[112:122]
# print "sound", s_bounded[122:132], s_normalized[122:132]
return list(s_normalized)
def infer(self, in_dims, out_dims, x):
if self.t < max(self.model.imodel.fmodel.k, self.model.imodel.k):
raise ExplautoBootstrapError
if in_dims == self.m_dims and out_dims == self.s_dims: # forward
return array(self.model.predict_effect(tuple(x)))
elif in_dims == self.s_dims and out_dims == self.m_dims: # inverse
if not self.bootstrapped_s:
# If only one distinct point has been observed in the sensory space, then we output a random motor command
return rand_bounds(np.array([self.m_mins, self.m_maxs]))[0]
else:
if self.mode == "explore":
self.mean_explore = array(self.model.infer_order(tuple(x)))
r = self.mean_explore
r[self.sigma_expl > 0] = np.random.normal(
r[self.sigma_expl > 0], self.sigma_expl[self.sigma_expl > 0]
)
res = bounds_min_max(r, self.m_mins, self.m_maxs)
return res
else: # exploit'
return array(self.model.infer_order(tuple(x)))
elif out_dims == self.m_dims[len(self.m_dims) / 2 :]: # dm = i(M, S, dS)
if not self.bootstrapped_s:
# If only one distinct point has been observed in the sensory space, then we output a random motor command
return rand_bounds(np.array([self.m_mins[self.m_ndims / 2 :], self.m_maxs[self.m_ndims / 2 :]]))[0]
else:
assert len(x) == len(in_dims)
m = x[: self.m_ndims / 2]
s = x[self.m_ndims / 2 :][: self.s_ndims / 2]
ds = x[self.m_ndims / 2 :][self.s_ndims / 2 :]
self.mean_explore = array(self.model.imodel.infer_dm(m, s, ds))
if self.mode == "explore":
r = np.random.normal(self.mean_explore, self.sigma_expl[out_dims])
res = bounds_min_max(r, self.m_mins[out_dims], self.m_maxs[out_dims])
return res
else:
return self.mean_explore
else:
raise NotImplementedError
def sensory_trajectory_msg_to_list(self, state):
s_torso = list(state.s_response_torso_sound.data)
s_baxter = list(state.s_response_baxter_sound.data)
#print "raw_sound_torso"
#print s_torso
#print "raw_sound_baxter"
#print s_baxter
if len(s_torso) == 0:
s_torso = [0.] * self.sound_torso_size
if len(s_baxter) == 0:
s_baxter = [0.] * self.sound_baxter_size
return bounds_min_max(np.array(s_torso + s_baxter),
self.sound_space_size * [-1.],
self.sound_space_size * [1.])
def inverse_idx(self, idx):
#print "Retrieve joystick demonstration"
s = self.model.imodel.fmodel.dataset.get_y(idx)
#print "s demo", s
_, idxs = self.model.imodel.fmodel.dataset.nn_y(s, k=1000)
# Find nearest s that was not a demo
for idx in idxs:
m = self.model.imodel.fmodel.dataset.get_x(idx)
if np.linalg.norm(m) > 0:
break
# print "nn idx", idx
# print "snn", self.model.imodel.fmodel.dataset.get_y(idx)
# print "m", m
r = m
r[self.sigma_expl > 0] = np.random.normal(r[self.sigma_expl > 0], self.sigma_expl[self.sigma_expl > 0])
res = bounds_min_max(r, self.m_mins, self.m_maxs)
return res
def compute_sensori_effect(self, m):
t = time.time()
m_arm = m[:21]
m_diva = m[21:]
assert np.linalg.norm(m_arm) * np.linalg.norm(m_diva) == 0.
if np.linalg.norm(m_arm) > 0.:
cmd = "arm"
else:
cmd = "diva"
self.purge_logs()
arm_traj = self.arm.update(m_arm)
#print "arm traj", arm_traj
if cmd == "diva":
diva_traj = self.diva.update(m_diva)
self.diva_traj = diva_traj
self.produced_sound = self.analysis_sound(self.diva_traj)
if self.produced_sound is not None:
self.count_produced_sounds[self.produced_sound] += 1
if self.produced_sound in self.human_sounds[:3]:
self.count_parent_give_object += 1
else:
diva_traj = np.zeros((50,2))
self.produced_sound = None
for i in range(self.timesteps):
# Arm
arm_x, arm_y, arm_angle = arm_traj[i]
# Tool
if not self.current_tool[3]:
if self.is_hand_free() and ((arm_x - self.current_tool[0]) ** 2. + (arm_y - self.current_tool[1]) ** 2. < self.handle_tol_sq):
self.current_tool[0] = arm_x
self.current_tool[1] = arm_y
self.current_tool[2] = np.mod(arm_angle + self.handle_noise * np.random.randn() + 1, 2) - 1
self.compute_tool()
self.current_tool[3] = 1
else:
self.current_tool[0] = arm_x
self.current_tool[1] = arm_y
self.current_tool[2] = np.mod(arm_angle + self.handle_noise * np.random.randn() + 1, 2) - 1
self.compute_tool()
self.logs_tool.append([self.current_tool[:2],
self.current_tool[2],
self.tool_end_pos,
self.current_tool[3]])
if cmd == "arm":
# Toy 1
if self.current_toy1[2] == 1 or (self.is_hand_free() and ((arm_x - self.current_toy1[0]) ** 2 + (arm_y - self.current_toy1[1]) ** 2 < self.object_tol_hand_sq)):
self.current_toy1[0] = arm_x
self.current_toy1[1] = arm_y
self.current_toy1[2] = 1
if self.current_toy1[2] == 2 or ((not self.current_toy1[2] == 1) and self.is_tool_free() and ((self.tool_end_pos[0] - self.current_toy1[0]) ** 2 + (self.tool_end_pos[1] - self.current_toy1[1]) ** 2 < self.object_tol_tool_sq)):
self.current_toy1[0] = self.tool_end_pos[0]
self.current_toy1[1] = self.tool_end_pos[1]
self.current_toy1[2] = 2
self.logs_toy1.append([self.current_toy1])
# Toy 2
if self.current_toy2[2] == 1 or (self.is_hand_free() and ((arm_x - self.current_toy2[0]) ** 2 + (arm_y - self.current_toy2[1]) ** 2 < self.object_tol_hand_sq)):
self.current_toy2[0] = arm_x
self.current_toy2[1] = arm_y
self.current_toy2[2] = 1
if self.current_toy2[2] == 2 or ((not self.current_toy2[2] == 1) and self.is_tool_free() and ((self.tool_end_pos[0] - self.current_toy2[0]) ** 2 + (self.tool_end_pos[1] - self.current_toy2[1]) ** 2 < self.object_tol_tool_sq)):
self.current_toy2[0] = self.tool_end_pos[0]
self.current_toy2[1] = self.tool_end_pos[1]
self.current_toy2[2] = 2
self.logs_toy2.append([self.current_toy2])
# Toy 3
if self.current_toy3[2] == 1 or (self.is_hand_free() and ((arm_x - self.current_toy3[0]) ** 2 + (arm_y - self.current_toy3[1]) ** 2 < self.object_tol_hand_sq)):
self.current_toy3[0] = arm_x
self.current_toy3[1] = arm_y
self.current_toy3[2] = 1
if self.current_toy3[2] == 2 or ((not self.current_toy3[2] == 1) and self.is_tool_free() and ((self.tool_end_pos[0] - self.current_toy3[0]) ** 2 + (self.tool_end_pos[1] - self.current_toy3[1]) ** 2 < self.object_tol_tool_sq)):
self.current_toy3[0] = self.tool_end_pos[0]
self.current_toy3[1] = self.tool_end_pos[1]
self.current_toy3[2] = 2
self.logs_toy3.append([self.current_toy3])
self.logs_caregiver.append([self.current_caregiver])
else:
# parent gives object if label is produced
if self.produced_sound == self.human_sounds[0]:
self.current_toy1 = self.caregiver_moves_obj(self.current_caregiver, self.current_toy1)
elif self.produced_sound == self.human_sounds[1]:
self.current_toy2 = self.caregiver_moves_obj(self.current_caregiver, self.current_toy2)
elif self.produced_sound == self.human_sounds[2]:
self.current_toy3 = self.caregiver_moves_obj(self.current_caregiver, self.current_toy3)
self.logs_toy1.append([self.current_toy1])
self.logs_toy2.append([self.current_toy2])
self.logs_toy3.append([self.current_toy3])
self.logs_caregiver.append([self.current_caregiver])
# if i in [0, 10, 20, 30, 40, 49]:
# self.hand = self.hand + [arm_x, arm_y]
# self.tool = self.tool + self.current_tool[:2]
# self.toy1 = self.toy1 + self.current_toy1[:2]
# self.toy2 = self.toy2 + self.current_toy2[:2]
# self.toy3 = self.toy3 + self.current_toy3[:2]
if i in [0, 12, 24, 37, 49]:
self.hand = self.hand + [arm_x, arm_y]
self.tool = self.tool + self.current_tool[:2]
self.toy1 = self.toy1 + self.current_toy1[:2]
self.toy2 = self.toy2 + self.current_toy2[:2]
self.toy3 = self.toy3 + self.current_toy3[:2]
self.caregiver = self.caregiver + self.current_caregiver
if self.arm.gui:
if i % 5 == 0:
self.plot_step(self.ax, i)
if cmd == "arm":
# parent gives label if object is touched by hand
if self.current_toy1[2] == 1:
label = self.give_label("toy1")
elif self.current_toy2[2] == 1:
label = self.give_label("toy2")
elif self.current_toy3[2] == 1:
label = self.give_label("toy3")
else:
label = self.give_label("random")
self.sound = label
#print "parent sound", label, self.sound
else:
self.sound = [f for formants in diva_traj[[0, 12, 24, 37, 49]] for f in formants]
self.sound = self.sound[0::2] + self.sound[1::2]
# Sort dims
self.hand = self.hand[0::2] + self.hand[1::2]
self.tool = self.tool[0::2] + self.tool[1::2]
self.toy1 = self.toy1[0::2] + self.toy1[1::2]
self.toy2 = self.toy2[0::2] + self.toy2[1::2]
self.toy3 = self.toy3[0::2] + self.toy3[1::2]
self.caregiver = self.caregiver[0::2] + self.caregiver[1::2]
#self.sound = self.sound[0::2] + self.sound[1::2]
# Analysis
if np.linalg.norm(m[21:]) > 0:
self.count_diva += 1
else:
self.count_arm += 1
if self.current_tool[3]:
self.count_tool += 1
if self.current_toy1[2] == 1:
self.count_toy1_by_hand += 1
elif self.current_tool[3] and self.current_toy1[2] == 2:
self.count_toy1_by_tool += 1
if self.current_toy2[2] == 1:
self.count_toy2_by_hand += 1
elif self.current_tool[3] and self.current_toy2[2] == 2:
self.count_toy2_by_tool += 1
if self.current_toy3[2] == 1:
self.count_toy3_by_hand += 1
elif self.current_tool[3] and self.current_toy3[2] == 2:
self.count_toy3_by_tool += 1
self.count_parent_give_label = self.count_toy1_by_hand + self.count_toy2_by_hand + self.count_toy3_by_hand
if cmd == "arm":
self.time_arm += time.time() - t
self.time_arm_per_it = self.time_arm / self.count_arm
else:
self.time_diva += time.time() - t
self.time_diva_per_it = self.time_diva / self.count_diva
#print "previous context", len(self.current_context), self.current_context
#print "s_hand", len(self.hand), self.hand
#print "s_tool", len(self.tool), self.tool
#print "s_toy1", len(self.toy1), self.toy1
#print "s_toy2", len(self.toy2), self.toy2
#print "s_toy3", len(self.toy3), self.toy3
#print "s_sound", len(self.sound), self.sound
#print "s_caregiver", len(self.caregiver), self.caregiver
self.t += 1
if self. t % 100 == 0:
self.best_vocal_errors_evolution += [self.best_vocal_errors.copy()]
if self.t % 1000 == 0:
print "best_vocal_errors", [(hs,self.best_vocal_errors[hs]) for hs in self.human_sounds]
context = self.current_context
# MAP TO STD INTERVAL
hand = [d/2 for d in self.hand]
tool = [d/2 for d in self.tool]
toy1 = [d/2 for d in self.toy1]
toy2 = [d/2 for d in self.toy2]
toy3 = [d/2 for d in self.toy3]
sound = [d - 8.5 for d in self.sound[:5]] + [d - 10.25 for d in self.sound[5:]]
caregiver = [d/2 for d in self.caregiver]
# MAP to Delta
# hand = [hand[1] - hand[0], hand[2] - hand[1], hand[3] - hand[2], hand[4] - hand[3], hand[5] - hand[4],
# hand[7] - hand[6], hand[8] - hand[7], hand[9] - hand[8], hand[10] - hand[9], hand[11] - hand[10]]
#
# tool = [tool[1] - tool[0], tool[2] - tool[1], tool[3] - tool[2], tool[4] - tool[3], tool[5] - tool[4],
# tool[7] - tool[6], tool[8] - tool[7], tool[9] - tool[8], tool[10] - tool[9], tool[11] - tool[10]]
#
# toy1 = [toy1[1] - toy1[0], toy1[2] - toy1[1], toy1[3] - toy1[2], toy1[4] - toy1[3], toy1[5] - toy1[4],
# toy1[7] - toy1[6], toy1[8] - toy1[7], toy1[9] - toy1[8], toy1[10] - toy1[9], toy1[11] - toy1[10]]
#
# toy2 = [toy2[1] - toy2[0], toy2[2] - toy2[1], toy2[3] - toy2[2], toy2[4] - toy2[3], toy2[5] - toy2[4],
# toy2[7] - toy2[6], toy2[8] - toy2[7], toy2[9] - toy2[8], toy2[10] - toy2[9], toy2[11] - toy2[10]]
#
# toy3 = [toy3[1] - toy3[0], toy3[2] - toy3[1], toy3[3] - toy3[2], toy3[4] - toy3[3], toy3[5] - toy3[4],
# toy3[7] - toy3[6], toy3[8] - toy3[7], toy3[9] - toy3[8], toy3[10] - toy3[9], toy3[11] - toy3[10]]
s = context + hand + tool + toy1 + toy2 + toy3 + sound + caregiver
#print "s_sound", sound
return bounds_min_max(s, self.conf.s_mins, self.conf.s_maxs)
def motor_primitive(self, m):
self.m = bounds_min_max(m, self.conf.m_mins, self.conf.m_maxs)
y = self.dmp.trajectory(self.m)
self.current_m = y[-1, :]
return y # y[:int(len(y) * ((n_bfs*2. - 1.)/(n_bfs*2.))), :]
def check_bounds_dmp(self, m_ag):
return bounds_min_max(m_ag, self.conf.m_mins, self.conf.m_maxs)
def w_to_trajectory(self, w):
w = w[self.arm_n_dims:]
#normalized_traj = bounds_min_max(self.motor_dmp.trajectory(np.array(w) * self.max_params), self.n_dmps * [-1.], self.n_dmps * [1.])
return bounds_min_max(
self.dmp.trajectory(np.array(w) * self.max_params),
self.n_dmps * [-1.], self.n_dmps * [1.])
def compute_motor_command(self, m_ag):
m_env = bounds_min_max(m_ag, self.conf.m_mins, self.conf.m_maxs)
return m_env
def compute_motor_command(self, m):
return bounds_min_max(m, self.conf.m_mins, self.conf.m_maxs)
def compute_motor_command(self, m):
return bounds_min_max(m, self.conf.m_mins, self.conf.m_maxs)
def check_bounds_dmp(self, m_ag):
return bounds_min_max(m_ag, self.conf.m_mins, self.conf.m_maxs)
def compute_motor_command(self, m_ag):
return bounds_min_max(self.trajectory(m_ag), self.m_mins, self.m_maxs)
def check_bounds_dmp(self, m_ag):return bounds_min_max(m_ag, self.conf.m_mins, self.conf.m_maxs) def motor_primitive(self, m): return m
def compute_dmp(self, m):
return bounds_min_max(
self.motor_dmp.trajectory(np.array(m) * self.max_params),
self.n_dmps * [-1.], self.n_dmps * [1.])
def sensory_trajectory_msg_to_list(self, state):
def flatten(list2d):
return [element2 for element1 in list2d for element2 in element1]
self.context = {
'ball': state.points[0].ball.angle,
'ergo': state.points[0].ergo.angle
}
rospy.loginfo("Context {}".format(self.context))
state_dict = {}
state_dict['hand'] = flatten([
(point.hand.pose.position.x, point.hand.pose.position.y,
point.hand.pose.position.z) for point in state.points
])
state_dict['joystick_1'] = flatten(
[point.joystick_1.axes for point in state.points])
state_dict['joystick_2'] = flatten(
[point.joystick_2.axes for point in state.points])
state_dict['ergo'] = flatten([(point.ergo.angle,
float(point.ergo.extended))
for point in state.points])
state_dict['ball'] = flatten([(point.ball.angle,
float(point.ball.extended))
for point in state.points])
#state_dict['ergo'] = flatten([((point.ergo.angle - self.context['ergo']) / 2., float(point.ergo.extended)) for point in state.points])
#state_dict['ball'] = flatten([((point.ball.angle - self.context['ball']) / 2., float(point.ball.extended)) for point in state.points])
state_dict['light'] = [point.color.data for point in state.points]
state_dict['sound'] = [point.sound.data for point in state.points]
state_dict['hand_right'] = flatten([(0., 0., 0.) for _ in range(10)])
state_dict['base'] = flatten([(0., 0., 0.) for _ in range(10)])
state_dict['arena'] = flatten([(0., 0.) for _ in range(10)])
state_dict['obj1'] = flatten([(0., 0.) for _ in range(10)])
state_dict['obj2'] = flatten([(0., 0.) for _ in range(10)])
state_dict['obj3'] = flatten([(0., 0.) for _ in range(10)])
rdm_power = 0.1
rdm2_x = list(np.cumsum(rdm_power * (np.random.random(10) - 0.5)))
rdm2_y = list(np.cumsum(rdm_power * (np.random.random(10) - 0.5)))
state_dict['rdm1'] = flatten([(rdm2_x[i], rdm2_y[i])
for i in range(10)])
rdm3_x = list(np.cumsum(rdm_power * (np.random.random(10) - 0.5)))
rdm3_y = list(np.cumsum(rdm_power * (np.random.random(10) - 0.5)))
state_dict['rdm2'] = flatten([(rdm3_x[i], rdm3_y[i])
for i in range(10)])
assert len(state_dict['hand']) == 30, len(state_dict['hand'])
assert len(state_dict['joystick_1']) == 20, len(
state_dict['joystick_1'])
assert len(state_dict['joystick_2']) == 20, len(
state_dict['joystick_2'])
assert len(state_dict['ergo']) == 20, len(state_dict['ergo'])
assert len(state_dict['ball']) == 20, len(state_dict['ball'])
assert len(state_dict['light']) == 10, len(state_dict['light'])
assert len(state_dict['sound']) == 10, len(state_dict['sound'])
# Concatenate all these values in a huge 132-float list
s_bounded = np.array([self.context['ergo'], self.context['ball']] + [
value for space in [
'hand', 'joystick_1', 'joystick_2', 'ergo', 'ball', 'light',
'sound', 'hand_right', 'base', 'arena', 'obj1', 'obj2', 'obj3',
'rdm1', 'rdm2'
] for value in state_dict[space]
])
s_normalized = ((s_bounded - self.bounds_sensory_min) /
self.bounds_sensory_diff) * 2 + np.array([-1.] * 312)
s_normalized = bounds_min_max(s_normalized, 312 * [-1.], 312 * [1.])
# print "context", s_bounded[:2], s_normalized[:2]
# print "hand", s_bounded[2:32], s_normalized[2:32]
# print "joystick_1", s_bounded[32:52], s_normalized[32:52]
# print "joystick_2", s_bounded[52:72], s_normalized[52:72]
# print "ergo", s_bounded[72:92], s_normalized[72:92]
# print "ball", s_bounded[92:112], s_normalized[92:112]
# print "light", s_bounded[112:122], s_normalized[112:122]
# print "sound", s_bounded[122:132], s_normalized[122:132]
return list(s_normalized)
