def process_raw_sensors(self, sensors_data):
# print('sensors_data_keys: {}'.format(sensors_data.keys()), file=sys.stderr)
try:
ori_array = sensors_data[self.object_name + '.ori']
except KeyError:
assert sensors_data['flag'] == 'no_collision'
return tools.to_signal((0.0, 0.0), self.s_channels)
u = np.matrix([[0.0], [0.0], [1.0]])
angle = 0.0
last_u = None
for ori in ori_array:
ori_rot = matrices.quat2rot(ori)
x_rot = ori_rot*u
if last_u is None:
last_u = x_rot
dangle = angle_between((last_u[0,0], last_u[1,0], last_u[2,0]),
(x_rot[0,0], x_rot[1,0], x_rot[2,0]))
#print('spin\n{}\n{}\n{}\n{}'.format(last_u.T, x_rot.T, (last_u-x_rot).T, dangle))
angle += dangle
last_u = x_rot
return tools.to_signal((angle, 0.0 if angle < 1e-2 else 1000.0), self.s_channels)
def process_raw_sensors(self, sensors_data):
if self.cfg.sprims.max_force > 0:
for contact in sensors_data.get('contacts', []):
if contact.force_norm > self.cfg.sprims.max_force:
return tools.to_signal(self.null_effect, self._s_channels)
if self.object_name + '.pos' not in sensors_data:
return tools.to_signal(self.null_effect, self._s_channels) # does this hide bugs ? when is it necessary ?
pos_array = sensors_data[self.object_name + '.pos']
pos_a = pos_array[0]
pos_b = pos_array[-1]
collision = 0.0 if dist(pos_a[:2], pos_b[:2]) < 1.0 else 1000.0
return tools.to_signal((pos_b[0], pos_b[1]) + (collision,), self._s_channels)
def process_raw_sensors(self, sensors_data):
if self.cfg.sprims.max_force > 0:
for contact in sensors_data.get('contacts', []):
if contact.force_norm > self.cfg.sprims.max_force:
return tools.to_signal(self.null_effect, self._s_channels)
if self.object_name + '.pos' not in sensors_data:
return tools.to_signal(
self.null_effect, self._s_channels
) # does this hide bugs ? when is it necessary ?
pos_array = sensors_data[self.object_name + '.pos']
pos_a = pos_array[0]
pos_b = pos_array[-1]
collision = 0.0 if dist(pos_a[:2], pos_b[:2]) < 1.0 else 1000.0
return tools.to_signal((pos_b[0], pos_b[1]) + (collision, ),
self._s_channels)
def process_raw_sensors(self, sensors_data):
effect_spin = tools.to_vector(self.spin.process_raw_sensors(sensors_data), self.spin.s_channels)
effect_roll = tools.to_vector(self.roll.process_raw_sensors(sensors_data), self.roll.s_channels)
if effect_spin[-1] == effect_roll[-1] == 0.0:
effect = effect_spin[:-1] + effect_roll[:-1] + (0.0,)
else:
effect = effect_spin[:-1] + effect_roll[:-1] + (1000.0,)
return tools.to_signal(effect, self.s_channels)
def vrep_capture(poses):
cfg = desc._deepcopy()
cfg.execute.is_simulation = True
cfg.execute.prefilter = False
cfg.execute.check_self_collisions = False
cfg.execute.scene.name = 'vanilla'
cfg.execute.scene.objects.ball45 = objdesc._deepcopy()
cfg.execute.scene.objects.ball45.pos = (None, None, None)
cfg.execute.scene.objects.ball45.mass = -1
cfg.execute.scene.objects.ball45.tracked = True
cfg.execute.simu.ppf = 200
cfg.execute.simu.mac_folder = '/Applications/V-REP/v_rep/bin'
cfg.execute.simu.load = True
cfg.execute.simu.headless = True
cfg.execute.simu.calibrdir = '~/.dovecot/tttcal/'
cfg.sprims.names = ['push']
cfg.sprims.uniformize = False
cfg.sprims.tip = True
cfg.mprims.dt = 0.01
cfg.mprims.name = 'dmp_sharedwidth'
cfg.mprims.target_end = 500
cfg.mprims.traj_end = 1500
cfg.mprims.sim_end = 1500
cfg.mprims.uniformize = False
cfg.mprims.n_basis = 2
cfg.mprims.max_speed = 30.0
cfg.mprims.init_states = [-30.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cfg.mprims.target_states = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cfg.mprims.angle_ranges = ((110.0, 110.0), (99.0, 99.0), (99.0, 99.0),
(120.0, 120.0), (99.0, 99.0), (99.0, 99.0))
sb = sim_env.SimulationEnvironment(cfg)
vrep_res = []
for tg in poses:
cfg.mprims.target_states = tg
print(tg)
sb.m_prim = prims.create_mprim(cfg.mprims.name, cfg)
m_signal = tools.to_signal(
(0.0, 0.0) * cfg.mprims.n_basis * 6 + (0.20, 0.20), sb.m_channels)
meta = {}
sb.execute(m_signal, meta=meta)
vrep_res.append(
np.mean([meta['log']['raw_sensors']['tip_pos'][-50:]], axis=1))
sb.close()
return vrep_res
def vrep_capture(poses):
cfg = desc._deepcopy()
cfg.execute.is_simulation = True
cfg.execute.prefilter = False
cfg.execute.check_self_collisions = False
cfg.execute.scene.name = 'vanilla'
cfg.execute.scene.objects.ball45 = objdesc._deepcopy()
cfg.execute.scene.objects.ball45.pos = (None, None, None)
cfg.execute.scene.objects.ball45.mass = -1
cfg.execute.scene.objects.ball45.tracked = True
cfg.execute.simu.ppf = 200
cfg.execute.simu.mac_folder = '/Applications/V-REP/v_rep/bin'
cfg.execute.simu.load = True
cfg.execute.simu.headless = True
cfg.execute.simu.calibrdir = '~/.dovecot/tttcal/'
cfg.sprims.names = ['push']
cfg.sprims.uniformize = False
cfg.sprims.tip = True
cfg.mprims.dt = 0.01
cfg.mprims.name = 'dmp_sharedwidth'
cfg.mprims.target_end = 500
cfg.mprims.traj_end = 1500
cfg.mprims.sim_end = 1500
cfg.mprims.uniformize = False
cfg.mprims.n_basis = 2
cfg.mprims.max_speed = 30.0
cfg.mprims.init_states = [-30.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cfg.mprims.target_states = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cfg.mprims.angle_ranges = ((110.0, 110.0), (99.0, 99.0), (99.0, 99.0), (120.0, 120.0), (99.0, 99.0), (99.0, 99.0))
sb = sim_env.SimulationEnvironment(cfg)
vrep_res = []
for tg in poses:
cfg.mprims.target_states = tg
print(tg)
sb.m_prim = prims.create_mprim(cfg.mprims.name, cfg)
m_signal = tools.to_signal((0.0, 0.0)*cfg.mprims.n_basis*6 + (0.20, 0.20),
sb.m_channels)
meta = {}
sb.execute(m_signal, meta=meta)
vrep_res.append(np.mean([meta['log']['raw_sensors']['tip_pos'][-50:]], axis=1))
sb.close()
return vrep_res
def process_raw_sensors(self, sensors_data):
try:
ori_array = sensors_data[self.object_name + '.ori']
except KeyError:
assert sensors_data['flag'] == 'no_collision'
return tools.to_signal((0.0, 0.0), self.s_channels)
u = np.matrix([[1.0], [0.0], [0.0]])
angle = 0.0
last_u = None
for ori in ori_array:
ori_rot = matrices.quat2rot(ori)
x_rot = ori_rot*u
if last_u is None:
last_u = x_rot
dangle = angle_between((last_u[0,0], last_u[1,0], last_u[2,0]),
(x_rot[0,0], x_rot[1,0], x_rot[2,0]))
angle += dangle
last_u = x_rot
return tools.to_signal((angle, 0.0 if angle < 1e-2 else 1000.0), self.s_channels)
def posture(kin_env,
m_vector,
fig=None,
swap_xy=True,
x_T=0.0,
y_T=0.0,
color='#666666',
alpha=0.5,
radius_factor=1.0,
line_factor=1.0,
**kwargs):
assert fig is not None
kwargs.update({
'line_color': color,
'line_alpha': alpha,
'fill_color': color,
'fill_alpha': alpha,
})
s_signal = kin_env.execute(
env_tools.to_signal(m_vector, kin_env.m_channels))
xs, ys = zip(*kin_env.posture)
if swap_xy:
ys, xs = xs, ys
xs, ys = np.array(xs), np.array(ys)
fig.line(xs + x_T,
ys + y_T,
line_width=line_factor * radius_factor,
line_color=color,
line_alpha=alpha)
fig.circle(xs[:1] + x_T,
ys[:1] + y_T,
radius=radius_factor * 0.015,
**kwargs)
fig.circle(xs[1:-1] + x_T,
ys[1:-1] + y_T,
radius=radius_factor * 0.008,
**kwargs)
fig.circle(xs[-1:] + x_T,
ys[-1:] + y_T,
radius=radius_factor * 0.01,
color='red',
alpha=alpha)
def posture(kin_env, m_vector, fig=None, swap_xy=True, x_T=0.0, y_T=0.0,
color='#666666', alpha=0.5, radius_factor=1.0, line_factor=1.0, **kwargs):
assert fig is not None
kwargs.update({'line_color' : color,
'line_alpha' : alpha,
'fill_color' : color,
'fill_alpha' : alpha,
})
s_signal = kin_env.execute(env_tools.to_signal(m_vector, kin_env.m_channels))
xs, ys = zip(*kin_env.posture)
if swap_xy:
ys, xs = xs, ys
xs, ys = np.array(xs), np.array(ys)
fig.line(xs+x_T, ys+y_T, line_width=line_factor*radius_factor, line_color=color, line_alpha=alpha)
fig.circle(xs[ : 1]+x_T, ys[ : 1]+y_T, radius=radius_factor*0.015, **kwargs)
fig.circle(xs[ 1:-1]+x_T, ys[ 1:-1]+y_T, radius=radius_factor*0.008, **kwargs)
fig.circle(xs[-1: ]+x_T, ys[-1: ]+y_T, radius=radius_factor*0.01, color='red', alpha=alpha)
def posture_vectors(env, m_vectors, **kwargs):
m_signals = [env_tools.to_signal(m_vector, env.m_channels) for m_vector in m_vectors]
return posture_signals(env, m_signals, **kwargs)
def posture_vectors(env, m_vectors, **kwargs):
m_signals = [
env_tools.to_signal(m_vector, env.m_channels) for m_vector in m_vectors
]
return posture_signals(env, m_signals, **kwargs)
