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)