def compute_motor_command(self, m_ag):
m_ag = bounds_min_max(m_ag, self.conf.m_mins, self.conf.m_maxs)
if self.motor_traj_type == "DMP":
dyn_idx = range(self.n_dynamic_motor_dims *
self.n_motor_traj_points)
m_weighted = m_ag[dyn_idx] * self.max_params
if self.optim_initial_position:
m_weighted[:self.
n_dynamic_motor_dims] = m_weighted[:self.
n_dynamic_motor_dims] / self.max_params
if self.optim_end_position:
m_weighted[-self.n_dynamic_motor_dims:] = m_weighted[
-self.n_dynamic_motor_dims:] / self.max_params
m_dyn = self.motor_dmp.trajectory(m_weighted)
static_idx = range(
self.n_dynamic_motor_dims * self.n_motor_traj_points,
self.conf.m_ndims)
m_static = m_ag[static_idx]
m = [
list(m_dyn_param) + list(m_static)
for m_dyn_param in list(m_dyn)
]
else:
raise NotImplementedError
return m
def compute_motor_command(self, m_ag):
m_ag = bounds_min_max(m_ag, self.conf.m_mins, self.conf.m_maxs)
if self.motor_traj_type == "DMP":
dyn_idx = range(self.n_dynamic_motor_dims * self.n_motor_traj_points)
#print "m params", m_ag[dyn_idx] * self.max_params
m_dyn = self.motor_dmp.trajectory(m_ag[dyn_idx] * self.max_params)
#print "mov", m_dyn
static_idx = range(self.n_dynamic_motor_dims * self.n_motor_traj_points, self.conf.m_ndims)
m_static = m_ag[static_idx]
m = [list(m_dyn_param) + list(m_static) for m_dyn_param in list(m_dyn)]
else:
raise NotImplementedError
return m
def compute_motor_command(self, m_ag):
m_ag = bounds_min_max(m_ag, self.conf.m_mins, self.conf.m_maxs)
if self.motor_traj_type == "DMP":
dyn_idx = range(self.n_dynamic_motor_dims * self.n_motor_traj_points)
m_weighted = m_ag[dyn_idx] * self.max_params
if self.optim_initial_position:
m_weighted[:self.n_dynamic_motor_dims] = m_weighted[:self.n_dynamic_motor_dims] / self.max_params
if self.optim_end_position:
m_weighted[-self.n_dynamic_motor_dims:] = m_weighted[-self.n_dynamic_motor_dims:] / self.max_params
m_dyn = self.motor_dmp.trajectory(m_weighted)
static_idx = range(self.n_dynamic_motor_dims * self.n_motor_traj_points, self.conf.m_ndims)
m_static = m_ag[static_idx]
m = [list(m_dyn_param) + list(m_static) for m_dyn_param in list(m_dyn)]
else:
raise NotImplementedError
return m
def compute_sensori_effect(self, m_traj):
s = self.env.update(m_traj, reset=False, log=False)
y = np.array(s[:self.move_steps])
if self.sensori_traj_type == "DMP":
self.sensori_dmp.dmp.imitate_path(np.transpose(y))
w = self.sensori_dmp.dmp.w.flatten()
s_ag = list(w)
elif self.sensori_traj_type == "samples":
w = y[self.samples, :]
s_ag = list(np.transpose(w).flatten())
else:
raise NotImplementedError
s = s_ag
if self.gui:
self.plot()
return bounds_min_max(s, self.conf.s_mins, self.conf.s_maxs)
def compute_sensori_effect(self, m_traj):
s = self.env.update(m_traj, reset=False, log=False)
self.s_traj = s
y = np.array(s[:self.move_steps])
if self.sensori_traj_type == "DMP":
self.sensori_dmp.dmp.imitate_path(np.transpose(y))
w = self.sensori_dmp.dmp.w.flatten()
s_ag = list(w)
elif self.sensori_traj_type == "samples":
w = y[self.samples,:]
s_ag = list(np.transpose(w).flatten())
elif self.sensori_traj_type == "end_point":
s_ag = y[self.end_point,:].flatten()
else:
raise NotImplementedError
s = s_ag
return bounds_min_max(s, self.conf.s_mins, self.conf.s_maxs)
def compute_sensori_effect(self, m_traj):
s = self.env.update(m_traj, log=False)
y = np.array(s[:self.move_steps])
if self.sensori_traj_type == "DMP":
self.sensori_dmp.dmp.imitate_path(np.transpose(y))
w = self.sensori_dmp.dmp.w.flatten()
s_ag = list(w)
elif self.sensori_traj_type == "samples":
w = y[self.samples,:]
s_ag = list(np.transpose(w).flatten())
else:
raise NotImplementedError
s = s_ag + s[self.move_steps:]
if self.gui:
#if abs(s[11] - (-0.85)) > 0.1: #Tool1
#if s[-2] > 0: # One of the boxes
self.plot()
return bounds_min_max(s, self.conf.s_mins, self.conf.s_maxs)
def compute_sensori_effect(self, m_traj):
s = self.env.update(m_traj, log=False)
y = np.array(s[:self.move_steps])
if self.sensori_traj_type == "DMP":
self.sensori_dmp.dmp.imitate_path(np.transpose(y))
w = self.sensori_dmp.dmp.w.flatten()
s_ag = list(w)
elif self.sensori_traj_type == "samples":
w = y[self.samples, :]
s_ag = list(np.transpose(w).flatten())
else:
raise NotImplementedError
s = s_ag + s[self.move_steps:]
if self.gui:
#if abs(s[11] - (-0.85)) > 0.1: #Tool1
#if s[-2] > 0: # One of the boxes
self.plot()
return bounds_min_max(s, self.conf.s_mins, self.conf.s_maxs)
def compute_motor_command(self, m_ag):
m_ag = bounds_min_max(m_ag, self.conf.m_mins, self.conf.m_maxs)
if self.motor_traj_type == "DMP":
dyn_idx = range(self.n_dynamic_motor_dims *
self.n_motor_traj_points)
#print "m params", m_ag[dyn_idx] * self.max_params
m_dyn = self.motor_dmp.trajectory(m_ag[dyn_idx] * self.max_params)
#print "mov", m_dyn
static_idx = range(
self.n_dynamic_motor_dims * self.n_motor_traj_points,
self.conf.m_ndims)
m_static = m_ag[static_idx]
m = [
list(m_dyn_param) + list(m_static)
for m_dyn_param in list(m_dyn)
]
else:
raise NotImplementedError
return m
def compute_sensori_effect(self, m_traj):
s = self.env.update(m_traj, reset=False, log=False)
self.s_traj = s
y = np.array(s[:self.move_steps])
if self.sensori_traj_type == "DMP":
self.sensori_dmp.dmp.imitate_path(np.transpose(y))
w = self.sensori_dmp.dmp.w.flatten()
s_ag = list(w)
elif self.sensori_traj_type == "samples":
w = y[self.samples,:]
s_ag = list(np.transpose(w).flatten())
else:
raise NotImplementedError
s = s_ag
if self.gui:
#if abs(s[11] - (-0.85)) > 0.1: #Tool1
#if s[-2] > 0: # One of the boxes
#if abs(s[-1] - 0.8) > 0.01:
#if np.random.rand() < 0.001:
#if abs(s[-1] - s[-3]) > 0.01:
self.plot()
#print 'dyn env s', s
return bounds_min_max(s, self.conf.s_mins, self.conf.s_maxs)
