Ejemplo n.º 1
0
    def _load(self):
        history = self.load_history()

        self.meta = history.core.meta
        self.m_channels = history.core.meta['m_channels']
        self.s_channels = history.core.meta['s_channels']

        self.data = {'ticks': [], 'explorations': [], 'observations': [],
                     's_signals': [], 's_vectors': [], 's_goals': [],
                     'm_signals': [], 'm_vectors': [], 'metadata': [],
                     's_channels': self.s_channels, 'm_channels': self.m_channels,
                     'meta': self.meta}

        for t, entry in enumerate(history.core.entries):
            if entry is not None:
                exploration = entry['data']['exploration']
                feedback    = entry['data']['feedback']
                m_signal = exploration['m_signal']
                s_signal = feedback['s_signal']
                self.data['ticks'].append(t)
                self.data['explorations'].append((exploration, feedback))
                self.data['observations'].append((m_signal, s_signal))
                self.data['m_signals'].append(m_signal)
                self.data['s_signals'].append(s_signal)
                self.data['m_vectors'].append(tools.to_vector(m_signal, self.m_channels))
                self.data['s_vectors'].append(tools.to_vector(s_signal, self.s_channels))
                try:
                    self.data['metadata'].append(entry['data']['meta'])
                except KeyError:
                    self.data['metadata'].append({})
Ejemplo n.º 2
0
    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)
Ejemplo n.º 3
0
    def process_motor_signal(self, m_signal):
        assert len(m_signal) == self.n_basis*(2*self.size+1)

        traj = []
        m_vector = tools.to_vector(m_signal, self.m_channels)

        widths = m_vector[-self.n_basis:]
        centers = tuple(np.linspace(0.0, 1.0, num=self.n_basis+2)[1:-1])

        for i, d in enumerate(self.dmps):
            slopes, offsets = [], []
            for j in range(self.n_basis):
                cursor = 2 * (self.n_basis * i + j)
                slope, offset = m_vector[cursor:cursor + 2]
                slopes.append(slope)
                offsets.append(offset)

            d.lwr_model_params(centers, widths, slopes, offsets)
            ts, ys, yds = d.trajectory()
            ts = self.cfg.mprims.target_end*self.cfg.mprims.dt*np.array(ts)
            ys = self.dmp2angle_deg(i, np.array(ys))

            traj_i = Trajectory(ts, ys, self.cfg.mprims.max_speed)
            traj.append(traj_i)

        return traj
Ejemplo n.º 4
0
def posture_extrema(env, explorations, thetas=tuple(i*math.pi/4 for i in range(8)), **kwargs):
    s_vectors = [env_tools.to_vector(e[1]['s_signal'], env.s_channels) for e in explorations]
    if 'theta' in [c.name for c in env.s_channels]:
        s_vectors, s_channels = depolarize(s_vectors, env.s_channels)

    idxs = factored.spread_extrema(s_vectors, dirs=thetas)
    m_signals = [explorations[idx][0]['m_signal'] for idx in idxs]
    return posture_signals(env, m_signals, **kwargs)
Ejemplo n.º 5
0
def choose_m_vectors(m_channels, explorations, n=5):
    """FIXME: no replacements"""
    m_vectors = []
    for _ in range(n):
        explo = random.choice(explorations)
        m_vector = env_tools.to_vector(explo[0]['m_signal'], m_channels)
        m_vectors.append(m_vector)

    return m_vectors
Ejemplo n.º 6
0
def run_exploration(env, ex, N, mesh=None, verbose=False, prefix=""):
    explorations, s_vectors, s_goals = [], [], []

    for i in range(N):
        if verbose and i % 100 == 0:
            gfx.print_progress(i, N, prefix=prefix)
        exploration = ex.explore()
        feedback = env.execute(exploration["m_signal"])
        ex.receive(exploration, feedback)
        s_vectors.append(tools.to_vector(feedback["s_signal"], env.s_channels))
        if "s_goal" in exploration:
            s_goals.append(tools.to_vector(exploration["s_goal"], env.s_channels))
        if mesh is not None:
            mesh.add(feedback["s_signal"], m_signal=exploration["m_signal"])
        explorations.append((exploration, feedback))
    if verbose:
        gfx.print_progress(N, N, prefix=prefix)

    return explorations, s_vectors, s_goals
Ejemplo n.º 7
0
def posture_signals(kin_env, m_signals, fig=None, plot_height=PLOT_SIZE, plot_width=PLOT_SIZE,
                    swap_xy=True, x_T=0.0, y_T=0.0, tools=TOOLS,
                    title='posture graphs', grid=False,
                    color='#666666', alpha=1.0, radius_factor=1.0, line_factor=1.0,
                    x_range=[-1.0, 1.0], y_range=[-1.0, 1.0], **kwargs):

    fig = prepare_fig(fig, x_range=x_range, y_range=y_range, tools=tools,
                      plot_width=plot_width, plot_height=plot_height, **kwargs)

    for m_signal in m_signals:
        m_vector = env_tools.to_vector(m_signal, kin_env.m_channels)
        posture(kin_env, m_vector, fig=fig, swap_xy=swap_xy, x_T=x_T, y_T=y_T,
                color=color, alpha=alpha, radius_factor=radius_factor, line_factor=line_factor)

    return fig
Ejemplo n.º 8
0
    def _load(self):
        history = self.load_history(name='sensoryfile')

        self.m_channels = history.core.meta['m_channels']
        self.s_channels = history.core.meta['s_channels']

        self.data = {'ticks': [], 's_signals': [], 's_vectors': [],
                     's_channels': self.s_channels, 'm_channels': self.m_channels}

        for t, entry in enumerate(history.core.entries):
            if entry is not None:
                s_signal = entry['data']['s_signal']
                self.data['ticks'].append(t)
                self.data['s_signals'].append(s_signal)
                self.data['s_vectors'].append(tools.to_vector(s_signal, self.s_channels))
Ejemplo n.º 9
0
def posture_quadrans(env, explorations, **kwargs):
    s_vectors = [env_tools.to_vector(e[1]['s_signal'], env.s_channels) for e in explorations]
    pp, pn, nn, np = [], [], [], []
    for idx, (x, y) in enumerate(s_vectors):
        if x < 0:
            if y < 0:
                nn.append(idx)
            else:
                np.append(idx)
        else:
            if y < 0:
                pn.append(idx)
            else:
                pp.append(idx)
    idxs = [random.choice(pp), random.choice(np), random.choice(pn), random.choice(nn)]
    m_signals = [explorations[idx][0]['m_signal'] for idx in idxs]
    posture_signals(env, m_signals, **kwargs)
Ejemplo n.º 10
0
def bokeh_kin(kin_env, m_signal, color='#DF4949', alpha=1.0, **kwargs):

    m_vector = tools.to_vector(m_signal, kin_env.m_channels)
    s_signal = kin_env._multiarm.forward_kin(m_vector)

    xs, ys = [0.0], [0.0]
    for i in range(kin_env.cfg.dim):
        xs.append(s_signal['x{}'.format(i+1)])
        ys.append(s_signal['y{}'.format(i+1)])
    xs, ys = ys, xs # we swap x and y for a more symmetrical look

    if isinstance(kin_env.cfg.lengths, numbers.Real):
        total_length = kin_env.cfg.lengths*kin_env.cfg.dim
    else:
        total_length = sum(kin_env.cfg.lengths)
    total_length += 0.0

    kwargs ={'plot_height' : int(350*1.60),
             'plot_width'  : int(350*1.60),
             'x_range'     : [-1.0, 1.0],
             'y_range'     : [-1.0, 1.0],
             'line_color'  : color,
             'line_alpha'  : alpha,
             'fill_color'  : color,
             'fill_alpha'  : alpha,
             'title':''
            }

    plotting.hold()
    plotting.line(xs, ys, **kwargs)
    plotting.grid().grid_line_color = None
    plotting.xaxis().major_tick_in   = 0
    plotting.ygrid().grid_line_color = None
    plotting.yaxis().major_tick_in   = 0

    plotting.circle(xs[  : 1], ys[  : 1], radius=0.015, **kwargs)
    plotting.circle(xs[ 1:-1], ys[ 1:-1], radius=0.008, **kwargs)
    plotting.circle(xs[-1:  ], ys[-1:  ], radius=0.01, color='red')
    plotting.hold(False)
Ejemplo n.º 11
0
    # if i == 100 or i == total-1:
    #     import pdb; pdb.set_trace()
    m_signal = tools.random_signal(vrepb.m_channels)
    meta = {}
    feedback = vrepb.execute(m_signal, meta=meta)
    if 'raw_sensors' in meta['log']:
        max_force = -1.0
        salient_contacts = meta['log']['raw_sensors']['salient_contacts']
        if salient_contacts['max'] is not None:
            max_force = salient_contacts['max'].force_norm
        #max_force = max([-1.0] + [c.force_norm for c in meta['log']['raw_sensors']['contacts']])
        print('max_force_sq: {} N'.format(max_force))
        if max_force > 10000.0:
            hard_col.append(m_signal)
            print(tools.to_vector(m_signal, vrepb.m_channels))
            print('{}{}{}'.format(gfx.green, tools.to_vector(feedback['s_signal'], vrepb.s_channels), gfx.end))
    s_vector = tools.to_vector(feedback['s_signal'], vrepb.s_channels)
    if s_vector[2] != 0.0:
        cols += 1
        col_orders.append(m_signal)
        print()
#    print('{}({})/{}'.format(i+1, cols, total), end='\r')
    sys.stdout.flush()
    #print(' '.join('{:5.2f}'.format(e) for e in effect))
    dur = time.time()-start_order
    print('{:04d}: {:.2f} MiB'.format(i, memory_usage()))
    print('      {:4.2f}s for this order)'.format(dur))


dur = time.time()-start
Ejemplo n.º 12
0
def posture_extrema(env, explorations, thetas=tuple(i * math.pi / 4 for i in range(8)), **kwargs):
    s_vectors = [tools.to_vector(e[1]["s_signal"], env.s_channels) for e in explorations]
    idxs = factored.spread_extrema(s_vectors, dirs=thetas)
    m_signals = [explorations[idx][0]["m_signal"] for idx in idxs]
    posture_signals(env, m_signals, **kwargs)
Ejemplo n.º 13
0
from __future__ import print_function

from environments import tools

import dotdot
from dovecot.vrepsim import sim_env
from cfg import cfg0


cfg0.execute.simu.ppf = 1
cfg0.execute.simu.headless = False
cfg0.execute.prefilter     = False
cfg0.execute.check_self_collisions = True
cfg0.execute.scene.name = 'enormouscube0'

vrepb = sim_env.SimulationEnvironment(cfg0)

m_signals = [
             {'offset1.1': 18.21943132281342, 'offset1.0': -338.9672241670093, 'offset4.0': -397.8297109683216, 'offset4.1': 163.8139828390838, 'offset5.1': -130.2311110970922, 'offset5.0': 23.02622364874412, 'slope4.1': 39.10153799620025, 'slope4.0': -207.56986905246865, 'width0': 0.23436020919298378, 'width1': 0.30399572637750855, 'slope2.1': 396.2688102771226, 'offset3.1': -384.05478771928864, 'slope2.0': -126.1717466704136, 'slope3.0': 270.48508698026603, 'slope3.1': -317.7002945471223, 'offset2.0': -236.69870254908432, 'offset2.1': 320.6583707364009, 'slope0.1': -379.62428223776675, 'slope0.0': 199.06472087226064, 'slope1.0': 228.98425026951793, 'slope1.1': 264.0245367159629, 'offset0.0': -372.3667661134722, 'offset0.1': 289.1960053713341, 'offset3.0': 258.2975166795768, 'slope5.0': 377.35078460201737, 'slope5.1': 106.4133304061964},
            ]



for m_signal in m_signals*10:
    feedback = vrepb.execute(m_signal, meta={})
    s_vector = tools.to_vector(feedback['s_signal'], vrepb.s_channels)
    print(s_vector)
Ejemplo n.º 14
0
def testset():
    global _testset
    if _testset is None:
        ts = datafile.load_file(paths.testset_filepath)
        _testset = tuple(tools.to_vector(s_signal, ts["s_channels"]) for s_signal in ts["s_signals"])
    return _testset
Ejemplo n.º 15
0
m_signal = {'slope0.1': 359.80220393397553, 'slope0.0': -227.81079906822575, 'offset4.0': -85.7103889189296, 'offset4.1': -25.735420066496204, 'offset5.1': -186.64161337701435, 'offset5.0': 157.0433582394495, 'slope4.1': -244.44695404573906, 'slope4.0': -191.67123103619278, 'width0': 0.19268932673260142, 'width1': 0.27615834483064006, 'offset3.1': -300.869431663208, 'slope2.1': -109.65384632030873, 'slope2.0': -259.3317003038468, 'slope3.0': -187.35635728906493, 'slope3.1': 61.61756391254147, 'offset2.0': 325.40075250529753, 'offset2.1': -32.73063038339836, 'offset1.1': -122.72243322237546, 'offset1.0': -71.7020756097541, 'slope1.0': -331.5353644241496, 'slope1.1': -324.04964527689754, 'offset0.0': -164.97949641297245, 'offset0.1': 78.94129803114015, 'offset3.0': -12.092508496067978, 'slope5.0': 13.867818190007483, 'slope5.1': 133.3742478782948}
m_signal = {'slope0.1': 286.28651126262582, 'slope0.0': 361.86675289573463, 'offset4.0': -292.89216625306716, 'offset4.1': -386.61263043899686, 'offset5.1': -97.859191311508482, 'offset5.0': 5.3179298744379366, 'slope4.1': -203.9711769515535, 'slope4.0': 219.49426622387966, 'width0': 0.16775078084659101, 'width1': 0.09448622082861588, 'offset3.1': -118.33301790237419, 'slope2.1': 19.243491637709212, 'slope2.0': 268.2515721669846, 'slope3.0': 334.79037411595436, 'slope3.1': -275.72374782794952, 'offset2.0': -70.963051108955653, 'offset2.1': 365.61510612198799, 'offset1.1': -277.95881848193881, 'offset1.0': -297.41210343219825, 'slope1.0': 112.50366790639589, 'slope1.1': -38.221724344275458, 'offset0.0': -307.34408432197557, 'offset0.1': 182.71390994161243, 'offset3.0': 201.52578922191412, 'slope5.0': -315.39699964305294, 'slope5.1': 126.57810917846086}
#m_signal = {'slope0.1': 298.09827156001427, 'slope0.0': 79.652956473864151, 'offset4.0': -162.41210293517642, 'offset4.1': 109.49900798972533, 'offset5.1': -175.42498845615094, 'offset5.0': -160.26596298026982, 'slope4.1': 171.79141657363357, 'slope4.0': -274.29517238030542, 'width0': 0.1435977092967371, 'width1': 0.17356864122584356, 'offset3.1': -186.15766986357559, 'slope2.1': -276.32846375583802, 'slope2.0': -296.63872325766863, 'slope3.0': 329.00446615901274, 'slope3.1': -169.19864925722985, 'offset2.0': 250.843202060718, 'offset2.1': -78.758125413260132, 'offset1.1': -138.77216183846519, 'offset1.0': -351.07111653953416, 'slope1.0': 133.59253889704155, 'slope1.1': -137.77400774139073, 'offset0.0': -322.94341122802507, 'offset0.1': -193.6504864237107, 'offset3.0': 283.42401369233596, 'slope5.0': -340.65603029711804, 'slope5.1': 109.23277816754353}
m_signal = {'slope0.1': 211.31776065122483, 'slope0.0': -48.089030853817349, 'offset4.0': -305.57185871029378, 'offset4.1': 107.71054739766146, 'offset5.1': 295.64969876482257, 'offset5.0': 246.51358421412488, 'slope4.1': -319.31496020716361, 'slope4.0': 89.584125360959092, 'width0': 0.092062389208790757, 'width1': 0.06485087878036716, 'offset3.1': 26.727411279564592, 'slope2.1': -282.05791218907177, 'slope2.0': -131.33893431806734, 'slope3.0': 34.293624163320601, 'slope3.1': -73.788273632193466, 'offset2.0': -231.23990247180441, 'offset2.1': -299.81144381972115, 'offset1.1': -283.35223261010412, 'offset1.0': -399.96166737535157, 'slope1.0': 31.855970828767965, 'slope1.1': -230.23596958294024, 'offset0.0': -178.7995271682712, 'offset0.1': -52.349600749468891, 'offset3.0': -337.39937436407786, 'slope5.0': 75.635437724675626, 'slope5.1': 210.36668763083208}
m_signal = {'slope0.1': 190.50007020934845, 'slope0.0': 199.50958228875788, 'offset4.0': -7.804807100399728, 'offset4.1': -240.07526874538954, 'offset5.1': -163.42168617939717, 'offset5.0': -177.0814778611401, 'slope4.1': 158.9595062415451, 'slope4.0': -141.64499199408039, 'width0': 0.17498410992161906, 'width1': 0.18482871816713167, 'offset3.1': 57.322726165151835, 'slope2.1': -220.46675276703613, 'slope2.0': -225.09555831454574, 'slope3.0': 163.82172821538666, 'slope3.1': -359.39232410734985, 'offset2.0': -83.60386879657204, 'offset2.1': -45.05175463122163, 'offset1.1': 75.49928548590276, 'offset1.0': -317.19341947693215, 'slope1.0': 42.505948966624715, 'slope1.1': -136.72390201007295, 'offset0.0': -204.6329174553664, 'offset0.1': -238.92817744079906, 'offset3.0': 154.5993404190857, 'slope5.0': -186.83767790077522, 'slope5.1': 171.8761882702512}

# {'slope0.1': 315.971868333958, 'slope0.0': 356.8184838976531, 'offset4.0': -147.00441901822137, 'offset4.1': -175.00618864226584, 'offset5.1': -151.6421529672196, 'offset5.0': -392.5183115862228, 'slope4.1': 340.2022482469458, 'slope4.0': -225.21258048261953, 'width0': 0.4780400291212285, 'width1': 0.31724360361737886, 'offset3.1': -176.159312934924, 'slope2.1': 243.6458962649599, 'slope2.0': 307.12538181667185, 'slope3.0': 31.285973746327045, 'slope3.1': -51.35630177069919, 'offset2.0': -196.16313176734232, 'offset2.1': 85.93167722899165, 'offset1.1': 20.115107982782376, 'offset1.0': -277.2003967011555, 'slope1.0': -160.2523857430744, 'slope1.1': -275.94757738049145, 'offset0.0': -165.62057389086763, 'offset0.1': -301.89714524561106, 'offset3.0': 328.5699713907246, 'slope5.0': -44.42212498017949, 'slope5.1': 334.36474321990875}
# 0086 {'y': -10.90446412563324, 'x': 10.734552145004272, 'push_saliency': 1000.0} [recorded]
# 500
#      {'y': -7.6036453247070312, 'x': 15.024757385253906, 'push_saliency': 1000.0}
# {'slope0.1': -83.02041898345698, 'slope0.0': 85.47918303830073, 'offset4.0': -111.32730487047871, 'offset4.1': -366.6740769762057, 'offset5.1': 299.67744082397326, 'offset5.0': -261.8987181474696, 'slope4.1': 299.9968055632719, 'slope4.0': 203.09597877643034, 'width0': 0.02673876219898871, 'width1': 0.06841799977515968, 'offset3.1': 243.69212366675447, 'slope2.1': 396.04267129498135, 'slope2.0': 145.56783970841173, 'slope3.0': -330.82158291506846, 'slope3.1': -92.00051687715154, 'offset2.0': -375.00947018524516, 'offset2.1': -307.31137185112516, 'offset1.1': -92.07759592052838, 'offset1.0': -31.832020356072576, 'slope1.0': -305.9008610505911, 'slope1.1': 179.9270036723509, 'offset0.0': -238.47898044796042, 'offset0.1': -133.24833546419035, 'offset3.0': 61.21103688143455, 'slope5.0': -280.45893964776997, 'slope5.1': -321.90771638544004}


#print(tools.to_vector(m_signal, vrep_env.m_channels))
feedback = vrep_env.execute(m_signal, meta={})
print('{}{}{}'.format(gfx.green, tools.to_vector(feedback['s_signal'], vrep_env.s_channels), gfx.end))


# No Object Environment
noobject_cfg = vrep_cfg._deepcopy()
noobject_cfg.execute.scene.objects.cube45.pos = (250.0, 250.0, None)
noobject_cfg.execute.simu.headless            = True
noobject_cfg.execute.simu.ppf                 = 200

noobject_env = sim_env.SimulationEnvironment(noobject_cfg)
meta = {}
feedback = noobject_env.execute(m_signal, meta=meta)
marker_traj = meta['log']['raw_sensors']['marker_sensors']
assert len(marker_traj) % 3 == 0
marker_traj = [(i*vrep_cfg.mprims.dt, marker_traj[3*i:3*(i+1)]) for i in range(int(len(marker_traj)/3))]