Example #1
0
def show_sensor_data(pylab, vehicle, robot_pose=None, col='r'):
    if robot_pose is None:
        robot_pose = SE3.from_yaml(vehicle['pose'])
    for attached in vehicle['sensors']:
        sensor_pose = from_yaml(attached['current_pose'])
        sensor_t, sensor_theta = \
            translation_angle_from_SE2(SE2_from_SE3(sensor_pose))
        print('robot: %s' % SE2.friendly(SE2_from_SE3(robot_pose)))
        print(' sens: %s' % SE2.friendly(SE2_from_SE3(sensor_pose)))
#        sensor_theta = -sensor_theta
        sensor = attached['sensor']
        if sensor['type'] == 'Rangefinder':
            directions = np.array(sensor['directions'])
            observations = attached['current_observations']
            readings = np.array(observations['readings'])
            valid = np.array(observations['valid'])
#            directions = directions[valid]
#            readings = readings[valid]
            x = []
            y = []
            rho_min = 0.05
            for theta_i, rho_i in zip(directions, readings):
                print('theta_i: %s' % theta_i)
                x.append(sensor_t[0] + 
                         np.cos(sensor_theta + theta_i) * rho_min)
                y.append(sensor_t[1] + 
                         np.sin(sensor_theta + theta_i) * rho_min)
                x.append(sensor_t[0] + 
                         np.cos(sensor_theta + theta_i) * rho_i)
                y.append(sensor_t[1] + 
                         np.sin(sensor_theta + theta_i) * rho_i)
                x.append(None)
                y.append(None)
            pylab.plot(x, y, color=col, markersize=0.5, zorder=2000)
        elif sensor['type'] == 'Photoreceptors':
            directions = np.array(sensor['directions'])
            observations = attached['current_observations']
            readings = np.array(observations['readings'])
            luminance = np.array(observations['luminance'])
            valid = np.array(observations['valid'])
            readings[np.logical_not(valid)] = 0.6
            rho_min = 0.5
            for theta_i, rho_i, lum in zip(directions, readings, luminance):
                x = []
                y = []
                x.append(sensor_t[0] + 
                         np.cos(sensor_theta + theta_i) * rho_min)
                y.append(sensor_t[1] + 
                         np.sin(sensor_theta + theta_i) * rho_min)
                x.append(sensor_t[0] + 
                         np.cos(sensor_theta + theta_i) * rho_i)
                y.append(sensor_t[1] + 
                         np.sin(sensor_theta + theta_i) * rho_i)
                pylab.plot(x, y, color=(lum, lum, lum),
                           markersize=0.5, zorder=2000)
        else:
            print('Unknown sensor type %r' % sensor['type'])
Example #2
0
    def test_poses_SE2(self):

        dynamics = SE2Dynamics(max_linear_velocity=[1, 1],
                               max_angular_velocity=1)

        dt = 0.1

        M = 1  # max
        Z = 0  # zero 
        m = -1 # min

        tests = [
            # format   (  (start_xy, start_theta), commands, 
            # (final_xy, final_theta))
            (([0, 0], 0), [Z, Z, Z], ([0, 0], 0)),
            (([1, 2], 3), [Z, Z, Z], ([1, 2], 3)),

            (([0, 0], 0), [M, Z, Z], ([dt, 0], 0)),

            (([0, 0], 0), [m, Z, Z], ([-dt, 0], 0)),
            (([0, 0], 0), [Z, M, Z], ([0, dt], 0)),
            (([0, 0], 0), [Z, m, Z], ([0, -dt], 0)),
            (([0, 0], 0), [Z, Z, M], ([0, 0], dt)),
            (([0, 0], 0), [Z, Z, m], ([0, 0], -dt)),

            (([0, 0], np.radians(90)), [M, Z, Z], ([0, dt], np.radians(90))),
            (([0, 0], np.radians(90)), [Z, M, Z], ([-dt, 0], np.radians(90)))

            # TODO: add some more tests with non-zero initial rotation
        ]

        for initial, commands, final in tests:
            start_pose = SE2_from_translation_angle(*initial)
            final_pose = SE2_from_translation_angle(*final)

            start_state = dynamics.pose2state(SE3_from_SE2(start_pose))
            final_state = dynamics.pose2state(SE3_from_SE2(final_pose))
            commands = np.array(commands)

            print('%s -> [%s] -> %s' %
                  (SE2.friendly(start_pose), commands,
                   SE2.friendly(final_pose)))

            actual, dummy = dynamics.integrate(start_state, +commands, dt)
            SE2.assert_close(actual, final_pose)

            start2, dummy = dynamics.integrate(final_state, -commands, dt)
            SE2.assert_close(start_pose, start2)
Example #3
0
    def test_poses_SE2(self):
        from vehicles_dynamics import SE2Dynamics

        dynamics = SE2Dynamics(max_linear_velocity=[1, 1],
                               max_angular_velocity=1)

        dt = 0.1

        M = 1  # max
        Z = 0  # zero
        m = -1  # min

        tests = [
            # format   (  (start_xy, start_theta), commands,
            # (final_xy, final_theta))
            (([0, 0], 0), [Z, Z, Z], ([0, 0], 0)),
            (([1, 2], 3), [Z, Z, Z], ([1, 2], 3)),
            (([0, 0], 0), [M, Z, Z], ([dt, 0], 0)),
            (([0, 0], 0), [m, Z, Z], ([-dt, 0], 0)),
            (([0, 0], 0), [Z, M, Z], ([0, dt], 0)),
            (([0, 0], 0), [Z, m, Z], ([0, -dt], 0)),
            (([0, 0], 0), [Z, Z, M], ([0, 0], dt)),
            (([0, 0], 0), [Z, Z, m], ([0, 0], -dt)),
            (([0, 0], np.radians(90)), [M, Z, Z], ([0, dt], np.radians(90))),
            (([0, 0], np.radians(90)), [Z, M, Z], ([-dt, 0], np.radians(90)))

            # TODO: add some more tests with non-zero initial rotation
        ]

        for initial, commands, final in tests:
            start_pose = SE2_from_translation_angle(*initial)
            final_pose = SE2_from_translation_angle(*final)

            start_state = dynamics.pose2state(SE3_from_SE2(start_pose))
            final_state = dynamics.pose2state(SE3_from_SE2(final_pose))
            commands = np.array(commands)

            print(
                '%s -> [%s] -> %s' %
                (SE2.friendly(start_pose), commands, SE2.friendly(final_pose)))

            actual, dummy = dynamics.integrate(start_state, +commands, dt)
            SE2.assert_close(actual, final_pose)

            start2, dummy = dynamics.integrate(final_state, -commands, dt)
            SE2.assert_close(start_pose, start2)
Example #4
0
def test_coords1():
    vl = np.array([
        0,
        1,
        0,
    ])
    va = np.array([np.deg2rad(20), 0, 0])

    vel = {
        'F': vl,
        'FL': vl + va,
        'FR': vl - va,
        'B': (-vl),
        'BL': (-vl + va),
        'BR': (-vl - va),
    }

    def make_motion(v):
        A = se2.algebra_from_vector(v)
        Q = SE2.group_from_algebra(A)
        return Q

    motions = dictmap(make_motion, vel)

    print motions
    for k, v in motions.items():

        print(' - %s:  %s -> %s' % (k, vel[k], SE2.friendly(v)))

    names = sorted(vel.keys())

    def commuting(a, b):
        q1 = motions[a]
        q2 = motions[b]
        return SE2.distance(SE2.multiply(q1, q2), SE2.multiply(q2, q1))

    def same(a, b):
        q1 = motions[a]
        q2 = motions[b]
        return SE2.distance(q1, q2)

    def anti(a, b):
        q1 = motions[a]
        q2 = motions[b]
        return SE2.distance(q1, SE2.inverse(q2))

    cD = construct_matrix_iterators((names, names), commuting)
    aD = construct_matrix_iterators((names, names), anti)
    D = construct_matrix_iterators((names, names), same)

    r = Report('test_coords1')
    r.table('D', data=D, cols=names, rows=names, fmt='%f')
    r.table('aD', data=aD, cols=names, rows=names, fmt='%f')
    r.table('cD', data=cD, cols=names, rows=names, fmt='%f')
    r.to_html('out/test_coords1/test_coords1.html')
Example #5
0
def test_coords1():
    vl = np.array([0, 1, 0, ])
    va = np.array([np.deg2rad(20), 0, 0])


    vel = {
       'F': vl,
       'FL': vl + va,
       'FR': vl - va,
       'B': (-vl),
       'BL': (-vl + va),
       'BR': (-vl - va),
    }

    def make_motion(v):
        A = se2.algebra_from_vector(v)
        Q = SE2.group_from_algebra(A)
        return Q
    
    motions = dictmap(make_motion, vel)
    
    print motions
    for k, v in motions.items():
        
        print(' - %s:  %s -> %s' % (k, vel[k], SE2.friendly(v)))

    names = sorted(vel.keys())
    
    def commuting(a, b):
        q1 = motions[a]
        q2 = motions[b]
        return SE2.distance(SE2.multiply(q1, q2),
                            SE2.multiply(q2, q1))
    
    def same(a, b):
        q1 = motions[a]
        q2 = motions[b]
        return SE2.distance(q1, q2)
    
    def anti(a, b):
        q1 = motions[a]
        q2 = motions[b]
        return SE2.distance(q1, SE2.inverse(q2))     
        
    cD = construct_matrix_iterators((names, names), commuting)
    aD = construct_matrix_iterators((names, names), anti)
    D = construct_matrix_iterators((names, names), same)
    
    r = Report('test_coords1')
    r.table('D', data=D, cols=names, rows=names, fmt='%f')
    r.table('aD', data=aD, cols=names, rows=names, fmt='%f')
    r.table('cD', data=cD, cols=names, rows=names, fmt='%f')
    r.to_html('out/test_coords1/test_coords1.html')
Example #6
0
def scenario_display(scenario, S, sim):
    y0 = scenario['y0']
    q0 = scenario['pose0']
    y1 = scenario['y1']
    q1 = scenario['pose1']
    delta = scenario['delta']
    print(SE2.friendly(SE2_from_SE3(q0)))
    print(SE2.friendly(SE2_from_SE3(q1)))
    print('increment: %s' % SE2.friendly(SE2_from_SE3(delta)))

    with S.plot('data') as pylab:
        pylab.plot(y0, label='y0')
        pylab.plot(y1, label='y1')
        pylab.axis((0, 180, -0.04, 1.04))

    with S.plot('world') as pylab:
        show_sensor_data(pylab, scenario['sim0']['vehicle'], col='r')
        show_sensor_data(pylab, scenario['sim1']['vehicle'], col='g')
        pylab.axis('equal')

#    for pose in scenario['poses']:
#        delta = pose_diff(q0, pose)
#        print('increment: %s' % SE2.friendly(SE2_from_SE3(delta)))
#        
    S.array_as_image('M0', scenario['M0'])
    S.array_as_image('M1', scenario['M1'])

    pdfparams = dict(figsize=(4, 4), mime=MIME_PDF)

    def display_all(S, name, sensels, mapp):
        x = scenario['sim0']['vehicle']['sensors'][0]['sensor']
        theta = x['directions']
        theta = np.array(theta)
        if len(theta) > len(sensels):  # XXX: hack
            theta = theta[::2]
        theta_rad = theta
        theta = np.rad2deg(theta)
        sensels = sensels.copy()
        sensels = sensels / sensels.max()
        sec = S.section(name)
        with sec.plot('readings', **pdfparams) as pylab:
            pylab.plot(theta, sensels, 'b.')
            pylab.plot([theta[0], theta[-1]], [0, 0], 'k--')
            pylab.plot([theta[0], theta[-1]], [1, 1], 'k--')
            pylab.axis((theta[0], theta[-1], -0.05, 1.01))

        with sec.plot('minimap', **pdfparams) as pylab:
            xs = np.cos(theta_rad) * sensels
            ys = np.sin(theta_rad) * sensels
            pylab.plot(xs, ys, 'k.')

            L = 0.2
            parms = dict(linewidth=2)
            pylab.plot([0, L], [0, 0], 'r-', **parms)
            pylab.plot([0, 0], [0, L], 'g-', **parms)

            pylab.axis('equal')
            R = 1.05
            pylab.axis((-R, R, -R, R))

        if mapp is not None:
            sec.array_as_image('field', mapp, 'scale')

    display_all(S, 'y0', y0, None)
    display_all(S, 'm0y', map2sensels(scenario['M0']), scenario['M0'])
    display_all(S, 'm1y', map2sensels(scenario['M1']), scenario['M1'])

    with S.plot('poses') as pylab:
#        for pose in scenario['poses']:
# #            print pose
#            draw_axes(pylab, SE2_from_SE3(pose))
#        
        for pose in scenario['poses']:
            draw_axes(pylab, SE2_from_SE3(pose), 'k', 'k', size=0.5)
        for pose in scenario['poses_important']:
            draw_axes(pylab, SE2_from_SE3(pose), 'k', 'k', size=1)
        draw_axes(pylab, SE2_from_SE3(q0), [0.3, 0, 0], [0, 0.3, 0], size=5)
        draw_axes(pylab, SE2_from_SE3(q1), 'r', 'g', size=5)

#        plot_sensor(pylab, sim.vehicle, q0, y0, 'g')
#        plot_sensor(pylab, sim.vehicle, q1, y1, 'b')
        pylab.axis('equal')

#    print(scenario['commands'])

    Se = S.section('exploration')
    for name, M1est in scenario['exploration'].items():
        Si = Se.section(name)
#        Si.array_as_image('M1est', M1est)
        Si.array_as_image('diff', M1est - scenario['M1'])

        display_all(Si, 'M1est', map2sensels(M1est), M1est)
Example #7
0
def run_simulation(task, vehicle, agent, log, dt, maxT):
        
    world = task.get_world()
    simulation = VehicleSimulation(world=world, vehicle=vehicle)
    directions = simulation.vehicle.sensors[0].sensor.directions
    vehicle_spec = VehicleSpec(directions=directions)
    agent.init(vehicle_spec)
    
    simulation.new_episode() 

    tmp_log = log + '.active'
    ldir = os.path.dirname(log)
    #last = os.path.join(ldir, 'last.yaml')
    logger.info('Writing on log %r.' % log)
    #logger.info(' (also accessible as %r)' % last)
    
    if not os.path.exists(ldir):
        os.makedirs(ldir)
    #if os.path.exists(last):
    #    os.unlink(last)
    
    logfile = open(tmp_log, 'w')
    
    #assert not os.path.exists(last)
    assert os.path.exists(tmp_log)
    #logger.info('Link %s, %s' % (tmp_log, last))
    #os.symlink(tmp_log, last)
    
    
    
    logger.info('Simulation dt=%.3f max T: %.3f' % (dt, maxT))
    while True:
        simulation.compute_observations()
        # TODO: perhaps this needs to be gerealized
        luminance = simulation.vehicle.sensors[0].current_observations['luminance']
        observations = VehicleObservations(time=simulation.timestamp,
                            dt=dt,
                            luminance=luminance)
        agent.process_observations(observations)
        commands = agent.choose_commands()
        # TODO: check format
        if logfile is not None:
            y = simulation.to_yaml()
            y['commands'] = commands.tolist()
            logfile.write('---\n')
            yaml.dump(y, logfile, Dumper=Dumper)
            
        logger.info('t=%.3f  pose: %s' % 
                    (simulation.timestamp,
                    SE2.friendly(SE2_from_SE3(simulation.vehicle.get_pose()))))
        
        if task.end_condition(simulation):
            break
        
        if simulation.timestamp > maxT:
            # TODO: add why we finished
            break
        
        simulation.simulate(commands, dt)

    logfile.close()

    if os.path.exists(log):
        os.unlink(log)
    assert not os.path.exists(log)
    os.rename(tmp_log, log)
    assert not os.path.exists(tmp_log)
    assert os.path.exists(log)