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)))
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 bootstrap(self, expe, n, bootstap_range_div):
conf = make_configuration(
expe.ag.conf.m_centers - expe.ag.conf.m_ranges /
(2 * bootstap_range_div), expe.ag.conf.m_centers +
expe.ag.conf.m_ranges / (2 * bootstap_range_div),
expe.ag.conf.s_centers - expe.ag.conf.s_ranges /
(2 * bootstap_range_div), expe.ag.conf.s_centers +
expe.ag.conf.s_ranges / (2 * bootstap_range_div))
m_rand = rand_bounds(conf.m_bounds, n=n)
for m in m_rand:
m[-expe.ag.dmp.
n_dmps:] = expe.ag.dmp.default[:expe.ag.dmp.
n_dmps] + conf.m_ranges[
-expe.ag.dmp.n_dmps:] * randn(
expe.ag.dmp.n_dmps)
mov = expe.ag.motor_primitive(m)
s = expe.env.update(mov, log=True)
s = expe.ag.sensory_primitive(s)
expe.ag.sensorimotor_model.update(m, s)
expe.ag.emit('choice', array([nan] * len(expe.ag.expl_dims)))
expe.ag.emit('inference', m)
expe.ag.emit('movement', mov)
expe.ag.emit('perception', s)
expe._update_logs()
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 infer(self, expl_dims, inf_dims, choice):
"""Employs the sensorimotor model to infer the result of an input.
Explauto-like syntax.
Args:
expl_dims, inf_dims (list[int]): Together determine if an inverse
or forward prediction is made.
choice (ndarray): input to inference engine.
Returns:
The sensorimotor prediction of choice given expl_dims and inf_dims.
"""
go_random = False
if self._n_bootstrap >= self.n_bootstrap_sm:
try:
inference = self.sensorimotor_model.infer(expl_dims,
inf_dims,
choice.flatten())
except ExplautoBootstrapError:
logger.warning('Sensorimotor model not bootstrapped yet')
go_random = True
else:
go_random = True
logger.warning('Sensorimotor model still missing samples.')
if go_random:
inference = rand_bounds(self.conf.bounds[:, inf_dims]).flatten()
return inference
def random_goal_babbling(trial, iterations):
env = ArmStickBalls()
np.random.seed(trial)
explored_s = []
res = []
sigma_explo_ratio = 0.05
sm_model = SensorimotorModel.from_configuration(env.conf,
'nearest_neighbor',
'default')
m = env.random_motor()
s = env.update(m)
sm_model.update(m, s)
for iteration in range(iterations):
if (not sm_model.bootstrapped_s) or random() < 0.2:
m = env.random_motor()
else:
s_goal = rand_bounds(env.conf.s_bounds)[0]
m = sm_model.model.infer_order(tuple(s_goal))
m = normal(m, sigma_explo_ratio)
s = env.update(
m
) # observe the sensory effect s (36D): the trajectory of all objects
sm_model.update(m, s) # update sensorimotor model
if (len(explored_s) == 0) or abs(s[17] - 0.6) > 0.001:
explored_s += [s]
if (iteration + 1) % (iterations / 10) == 0:
res += [
int(
compute_explo(array(explored_s)[:, [14, 17]],
array([-2., -2.]),
array([2., 2.]),
gs=grid_size))
]
return res
def compute_sensori_effect(self, joint_pos_env):
effect = SimpleArmEnvironment.compute_sensori_effect(
self, joint_pos_env)
if inside(effect, self.cloudAreaMin, self.cloudAreaMax):
effect = rand_bounds(
np.vstack((self.cloudAreaMin, self.cloudAreaMax)))[0]
return effect
# return self.cloudAreaMin
else:
return effect
def choose(self, context_ms=None):
""" Returns a point chosen by the interest model
"""
try:
if self.context_mode is None:
x = self.interest_model.sample()
else:
x = np.hstack((context_ms, self.interest_model.sample_given_context(context_ms, range(self.context_mode["context_n_dims"]))))
except ExplautoBootstrapError:
x = rand_bounds(self.conf.bounds[:, self.expl_dims]).flatten()
return x
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, expl_dims, inf_dims, x, pref='', n=1, explore=True):
try:
if self.n_bootstrap > 0:
self.n_bootstrap -= 1
raise ExplautoBootstrapError
mode = "explore" if explore else "exploit"
if n == 1:
self.sensorimotor_model.mode = mode
m = self.sensorimotor_model.infer(expl_dims, inf_dims, x.flatten())
else:
self.sensorimotor_model.mode = mode
m = []
for _ in range(n):
m.append(self.sensorimotor_model.infer(expl_dims, inf_dims, x.flatten()))
# self.emit(pref + 'inference' + '_' + self.mid, m)
except ExplautoBootstrapError:
if n == 1:
m = rand_bounds(self.conf.bounds[:, inf_dims]).flatten()
else:
m = rand_bounds(self.conf.bounds[:, inf_dims], n)
return m
def infer(self, expl_dims, inf_dims, x, pref='', n=1, explore=True):
try:
if self.n_bootstrap > 0:
self.n_bootstrap -= 1
raise ExplautoBootstrapError
mode = "explore" if explore else "exploit"
if n == 1:
self.sensorimotor_model.mode = mode
m = self.sensorimotor_model.infer(expl_dims, inf_dims, x.flatten())
else:
self.sensorimotor_model.mode = mode
m = []
for _ in range(n):
m.append(self.sensorimotor_model.infer(expl_dims, inf_dims, x.flatten()))
self.emit(pref + 'inference' + '_' + self.mid, m)
except ExplautoBootstrapError:
if n == 1:
m = rand_bounds(self.conf.bounds[:, inf_dims]).flatten()
else:
m = rand_bounds(self.conf.bounds[:, inf_dims], n)
return m
def motor_babbling(self, arm=False, audio=False):
self.m = rand_bounds(self.conf.m_bounds)[0]
if arm:
r = 1.
self.last_cmd = "arm"
elif audio:
r = 0.
self.last_cmd = "diva"
else:
r = np.random.random()
if r < 0.5:
self.m[:self.arm_n_dims] = 0.
self.last_cmd = "diva"
else:
self.m[self.arm_n_dims:] = 0.
self.last_cmd = "arm"
return self.m
def bootstrap(self, expe, n, bootstap_range_div):
conf = make_configuration(expe.ag.conf.m_centers - expe.ag.conf.m_ranges/(2 * bootstap_range_div),
expe.ag.conf.m_centers + expe.ag.conf.m_ranges/(2 * bootstap_range_div),
expe.ag.conf.s_centers - expe.ag.conf.s_ranges/(2 * bootstap_range_div),
expe.ag.conf.s_centers + expe.ag.conf.s_ranges/(2 * bootstap_range_div))
m_rand = rand_bounds(conf.m_bounds, n=n)
for m in m_rand:
m[-expe.ag.dmp.n_dmps:] = expe.ag.dmp.default[:expe.ag.dmp.n_dmps] + conf.m_ranges[-expe.ag.dmp.n_dmps:] * randn(expe.ag.dmp.n_dmps)
mov = expe.ag.motor_primitive(m)
s = expe.env.update(mov, log=True)
s = expe.ag.sensory_primitive(s)
expe.ag.sensorimotor_model.update(m, s)
expe.ag.emit('choice', array([nan] * len(expe.ag.expl_dims)))
expe.ag.emit('inference', m)
expe.ag.emit('movement', mov)
expe.ag.emit('perception', s)
expe._update_logs()
def run(self):
env = VrepEnvironment(self.robot, **conf)
im = InterestModel.from_configuration(env.conf, env.conf.m_dims, 'random')
sm = NearestNeighbor(env.conf, sigma_ratio=1. / 16.)
ag = Agent(env.conf, sm, im)
m_rand = rand_bounds(env.conf.m_bounds, n=N)
self.tc = []
for i, m in enumerate(m_rand):
mov = ag.motor_primitive(m)
s = env.compute_sensori_effect(mov)
self.tc.append(s)
env.robot.reset_simulation()
def infer(self, expl_dims, inf_dims, x, pref='', mode='sg'):
""" Use the sensorimotor model to compute the expected value on inf_dims given that the value on expl_dims is x.
.. note:: This corresponds to a prediction if expl_dims=self.conf.m_dims and inf_dims=self.conf.s_dims and to inverse prediction if expl_dims=self.conf.s_dims and inf_dims=self.conf.m_dims.
"""
try:
if self.n_bootstrap > 0:
self.n_bootstrap -= 1
raise ExplautoBootstrapError
m = self.sensorimotor_model.infer(expl_dims,
inf_dims,
x.flatten())
self.emit(pref + 'inference' + '_' + self.mid, m)
#print "module", self.mid, "inference"
except ExplautoBootstrapError:
#logger.warning('Sensorimotor model not bootstrapped yet')
m = rand_bounds(self.conf.bounds[:, inf_dims]).flatten()
return m
def motor_babbling(self, arm=False, audio=False):
self.m = rand_bounds(self.conf.m_bounds)[0]
if arm:
r = 1.
self.last_cmd = "arm"
elif audio:
r = 0.
self.last_cmd = "diva"
else:
r = np.random.random()
module = None
if r > self.arm_goal_selection:
self.m[:self.arm_n_dims] = 0.
self.last_cmd = "diva"
module = "diva_babbling"
else:
self.m[self.arm_n_dims:] = 0.
self.last_cmd = "arm"
module = "arm_babbling"
self.chosen_modules.append(module)
return self.m, self.last_cmd
def infer(self, expl_dims, inf_dims, x, pref='', mode='sg'):
""" Use the sensorimotor model to compute the expected value on inf_dims given that the value on expl_dims is x.
.. note:: This corresponds to a prediction if expl_dims=self.conf.m_dims and inf_dims=self.conf.s_dims and to inverse prediction if expl_dims=self.conf.s_dims and inf_dims=self.conf.m_dims.
"""
try:
if self.n_bootstrap > 0:
self.n_bootstrap -= 1
raise ExplautoBootstrapError
m, idx = self.sensorimotor_model.infer(expl_dims, inf_dims,
x.flatten())
self.last_module_to_credit = idx
#print "infer", snn, sp
# self.snn = snn
# self.sp = sp
#print "goal", x, "mode", self.sensorimotor_model.mode, "m", m
self.emit(pref + 'inference' + '_' + self.mid, m)
#print "module", self.mid, "inference"
except ExplautoBootstrapError:
#logger.warning('Sensorimotor model not bootstrapped yet')
m = rand_bounds(self.conf.bounds[:, inf_dims]).flatten()
return m
def choose(self):
"""Returns a point chosen by the interest model.
Explauto-like syntax.
Returns:
An ndarray of length len(self.expl_dims) sampled from the interest
model.
"""
go_random = False
# check the amount of samples
if self._n_bootstrap >= self.n_bootstrap_im:
# catch ExplautoBootstrapError
try:
choice = self.interest_model.sample()
except ExplautoBootstrapError:
go_random = True
logger.warning('Interest model not bootstrapped yet')
else:
go_random = True
logger.warning('Interest model still missing samples.')
if go_random:
choice = rand_bounds(self.conf.bounds[:, self.expl_dims]).flatten()
return choice
def produce(self, context_ms=None):
for mid in self.modules.keys():
self.last_space_children_choices[mid] = Queue.Queue()
# mid = self.choose_babbling_module(mode=self.choice, weight_by_level=self.llb)
# self.mid_control = mid
# action = self.produce_module(mid, n_explo_points=self.n_explo_points, context_ms=context_ms)
# self.action = action
# #print "Action", action.n_iterations, "mid", mid
# #self.action.print_action()
# m_seq = action.get_m_seq(len(self.conf.m_dims))
s_space = self.choose_interesting_space(mode=self.choice)
#print "chosen s_space", s_space
if s_space == "s_o":
s = - np.array([0.] + context_ms)
else:
s = rand_bounds(np.array([self.conf.mins[self.config.s_spaces[s_space]], self.conf.maxs[self.config.s_spaces[s_space]]]))[0]
#print "m_seq", m_seq
self.m_seq = self.inverse(s_space, s, babbling=True, context=context_ms)
#print "Produce ", self.t
self.t = self.t + 1
return self.m_seq
def random_goal_babbling(trial, iterations):
env = ArmStickBalls()
np.random.seed(trial)
explored_s = []
res = []
sigma_explo_ratio = 0.05
sm_model = SensorimotorModel.from_configuration(env.conf, 'nearest_neighbor', 'default')
m = env.random_motor()
s = env.update(m)
sm_model.update(m, s)
for iteration in range(iterations):
if (not sm_model.bootstrapped_s) or random() < 0.2:
m = env.random_motor()
else:
s_goal = rand_bounds(env.conf.s_bounds)[0]
m = sm_model.model.infer_order(tuple(s_goal))
m = normal(m, sigma_explo_ratio)
s = env.update(m) # observe the sensory effect s (36D): the trajectory of all objects
sm_model.update(m, s) # update sensorimotor model
if (len(explored_s) == 0) or abs(s[17] - 0.6) > 0.001:
explored_s += [s]
if (iteration+1) % (iterations/10) == 0:
res += [int(compute_explo(array(explored_s)[:,[14,17]], array([-2., -2.]), array([2., 2.]), gs=grid_size))]
return res
def motor_babbling(self, n=1):
if n == 1:
return rand_bounds(self.conf.m_bounds)[0]
else:
return rand_bounds(self.conf.m_bounds, n)
def motor_babbling(self):
return rand_bounds(self.conf.m_bounds)[0]
# l2 = len(list2)
#
# print "Number of total explored cells in S_Magnetic1", l1
# print "Number of total explored cells in S_HookLoop1", l2
#
#
#
# v = np.linspace(-1.35, 1.35, 10)
#
# idx1 = np.random.randint(l1, size=n_testcases)
# idx2 = np.random.randint(l2, size=n_testcases)
#
# testcases1 = np.array(list1)[idx1] + np.random.random_sample((n_testcases,6)) * 0.3 - 0.15
# testcases2 = np.array(list2)[idx2] + np.random.random_sample((n_testcases,6)) * 0.3 - 0.15
testcases = rand_bounds(np.array([[-1.5,-1.5],[1.5,1.5]]), n=n_testcases)
testcases1 = testcases
testcases2 = testcases
print testcases1
print testcases2
with open(log_dir + 'testcases1.pickle', 'wb') as f:
cPickle.dump(testcases1, f)
with open(log_dir + 'testcases2.pickle', 'wb') as f:
cPickle.dump(testcases2, f)
def goal_babbling(self):
s = rand_bounds(self.conf.s_bounds)[0]
m = self.sm.infer(self.conf.s_dims, self.conf.m_dims, s)
return m
def motor_babbling(self, n=1):
if n == 1:
return rand_bounds(self.conf.m_bounds)[0]
else:
return rand_bounds(self.conf.m_bounds, n)
def motor_babbling(self):
return rand_bounds(self.conf.m_bounds)[0]
def goal_babbling(self):
s = rand_bounds(self.conf.s_bounds)[0]
m = self.sm.infer(self.conf.s_dims, self.conf.m_dims, s)
return m
import sys
import numpy as np
import time
from explauto.utils import rand_bounds
sys.path.append('../')
from src.environment.arm_diva_env import CogSci2017Environment
import pyaudio
env = CogSci2017Environment()
pa = pyaudio.PyAudio()
stream = pa.open(format=pyaudio.paFloat32, channels=1, rate=11025, output=True)
for i in range(100):
m = rand_bounds(env.conf.m_bounds)[0][21:]
diva_traj = env.diva.update(m)
sound = env.diva.sound_wave(env.diva.art_traj)
print "sound", sound
time.sleep(1)
#stream.write(sound.astype(np.float32).tostring())
