def center_point1(): outdir = get_comptests_output_dir() templates = load_tile_types() for k, v in templates.items(): if isinstance(v, LaneSegment): area = RectangularArea((-2, -2), (3, 3)) dest = os.path.join(outdir, k) N = len(v.control_points) betas = list(np.linspace(-2, N + 1, 20)) betas.extend(range(N)) betas = sorted(betas) transforms = [] for timestamp in betas: beta = timestamp p = v.center_point(beta) # print('%s: %s' % (beta, geo.SE2.friendly(p))) transform = SE2Transform.from_SE2(p) transforms.append(transform) c = SampledSequence[SE2Transform](betas, transforms) v.set_object("a", PlacedObject(), ground_truth=c) draw_static(v, dest, area=area)
def m1(): outdir = get_comptests_output_dir() gm = load_map("udem1") # dw = DuckietownWorld() # for map_name, tm in gym_maps.items(): # DW.root.set_object(map_name, tm) root = PlacedObject() world = PlacedObject() root.set_object("world", world) origin = SE2Transform([1, 10], np.deg2rad(10)) world.set_object("tile_map", gm, ground_truth=Constant[SE2Transform](origin)) # d = dw.as_json_dict() # print(json.dumps(d, indent=4)) # print(yaml.safe_dump(d, default_flow_style=False)) # G = get_meausurements_graph(root) fn = os.path.join(outdir, "out1.pdf") plot_measurement_graph(root, G, fn)
def svg2(): outdir = get_comptests_output_dir() templates = load_tile_types() for k, v in templates.items(): area = RectangularArea((-1, -1), (1, 1)) dest = os.path.join(outdir, k) draw_static(v, dest, area=area)
def lane_pose_test1(): outdir = get_comptests_output_dir() # load one of the maps (see the list using dt-world-draw-maps) dw = load_map("udem1") v = 5 # define a SampledSequence with timestamp, command commands_sequence = SampledSequence.from_iterator( [ # we set these velocities at 1.0 (1.0, WheelVelocityCommands(0.1 * v, 0.1 * v)), # at 2.0 we switch and so on (2.0, WheelVelocityCommands(0.1 * v, 0.4 * v)), (4.0, WheelVelocityCommands(0.1 * v, 0.4 * v)), (5.0, WheelVelocityCommands(0.1 * v, 0.2 * v)), (6.0, WheelVelocityCommands(0.1 * v, 0.1 * v)), ] ) # we upsample the sequence by 5 commands_sequence = commands_sequence.upsample(5) ## Simulate the dynamics of the vehicle # start from q0 q0 = geo.SE2_from_translation_angle([1.8, 0.7], 0) # instantiate the class that represents the dynamics dynamics = reasonable_duckiebot() # this function integrates the dynamics poses_sequence = get_robot_trajectory(dynamics, q0, commands_sequence) ################# # Visualization and rule evaluation # Creates an object 'duckiebot' ego_name = "duckiebot" db = DB18() # class that gives the appearance # convert from SE2 to SE2Transform representation transforms_sequence = poses_sequence.transform_values(SE2Transform.from_SE2) # puts the object in the world with a certain "ground_truth" constraint dw.set_object(ego_name, db, ground_truth=transforms_sequence) # Rule evaluation (do not touch) interval = SampledSequence.from_iterator(enumerate(commands_sequence.timestamps)) evaluated = evaluate_rules( poses_sequence=transforms_sequence, interval=interval, world=dw, ego_name=ego_name, ) timeseries = make_timeseries(evaluated) # Drawing area = RectangularArea((0, 0), (3, 3)) draw_static(dw, outdir, area=area, timeseries=timeseries)
def maps(): outdir = get_comptests_output_dir() map_names = list_maps() print(map_names) for map_name in map_names: duckietown_map = load_map(map_name) out = os.path.join(outdir, map_name) draw_map(out, duckietown_map)
def test_pose_sampling_1(): m = dw.load_map("4way") along_lane = 0.2 only_straight = True for i in range(45): q = sample_good_starting_pose(m, only_straight=only_straight, along_lane=along_lane) ground_truth = dw.SE2Transform.from_SE2(q) m.set_object(f"sampled-{i}", dw.DB18(), ground_truth=ground_truth) outdir = get_comptests_output_dir() draw_static(m, outdir)
def svg1(): outdir = get_comptests_output_dir() control_points = [ SE2Transform.from_SE2(geo.SE2_from_translation_angle([0, 0], 0)), SE2Transform.from_SE2(geo.SE2_from_translation_angle([1, 0], 0)), SE2Transform.from_SE2(geo.SE2_from_translation_angle([2, -1], np.deg2rad(-90))), ] width = 0.3 ls = LaneSegment(width, control_points) area = RectangularArea((-1, -2), (3, 2)) draw_static(ls, outdir, area=area)
def lane_pose_test1(): outdir = get_comptests_output_dir() dm = load_map('udem1') print(get_object_tree(dm, attributes=True)) res = get_skeleton_graph(dm) # area = RectangularArea((0, 0), (3, 3)) draw_static(res.root2, outdir + '/root2') # draw_static(dm, outdir + '/orig') print(get_object_tree(res.root2, attributes=True))
def lane_pose_segment1(): outdir = get_comptests_output_dir() dm = load_map("udem1") _ = get_object_tree(dm, attributes=True) res = get_skeleton_graph(dm) # area = RectangularArea((0, 0), (3, 3)) draw_static(res.root2, outdir + "/root2") # draw_static(dm, outdir + '/orig') _ = get_object_tree(res.root2, attributes=True)
def assert_seq(s: Union[str, Language], seq: List[Event], expect: Sequence[type], final: type): if isinstance(s, str): s = s.replace('\n', ' ').strip() while ' ' in s: s = s.replace(' ', ' ') l = parse_language(s) else: l = s s2 = language_to_str(l) print(s) print(s2) l2 = parse_language(s2) assert l == l2, (s, s2) pc = LanguageChecker(l) logger.info(f'Active start: {pc.get_active_states_names()}') dn = get_comptests_output_dir() fn = os.path.join(dn, 'language.dot') make_sure_dir_exists(fn) write_dot(pc.g, fn) logger.info(f'Written to {fn}') # all except last for i, (e, r) in enumerate(zip(seq, expect)): logger.info(f'Active before: {pc.get_active_states_names()}') logger.info(f'Event {e}') res = pc.push(e) logger.info(f'Active after: {pc.get_active_states_names()}') if not isinstance(res, r): msg = f'Input {i} ({e}) response was {type(res).__name__} instead of {r.__name__}' msg += f'\n entire sequence: {seq}' msg += f'\n language: {l}' msg += f'\n language string: {s2}' raise Exception(msg) res = pc.finish() if not isinstance(res, final): msg = f'finish response was {type(res).__name__} instead of {final.__name__}' msg += f'\n entire sequence: {seq}' msg += f'\n language: {l}' msg += f'\n language string: {s2}' raise Exception(msg)
def wb1(): outdir = get_comptests_output_dir() root = PlacedObject() tile_map = create_map(H=3, W=3) world = PlacedObject() root.set_object('world', world) placement = Constant[SE2Transform](SE2Transform.identity()) world.set_object('map1', tile_map, ground_truth=placement) ego = PlacedObject() world_coordinates = Constant[SE2Transform](SE2Transform([0, 0], 0)) world.set_object('ego', ego, ground_truth=world_coordinates) d = root.as_json_dict() # print(json.dumps(DW.root.as_json_dict(), indent=4)) # print(yaml.safe_dump(d, default_flow_style=False)) # print('------') r1 = Serializable.from_json_dict(d) # print('read: %s' % r1) d1 = r1.as_json_dict()
def tag_positions(): map_yaml_data = yaml.load(map_yaml, Loader=yaml.Loader) m = construct_map(map_yaml_data) print(get_object_tree(m, attributes=True)) outdir = get_comptests_output_dir() draw_static(m, outdir)
def test_pwm1(): parameters = get_DB18_nominal(delay=0) # initial configuration init_pose = np.array([0, 0.8]) init_vel = np.array([0, 0]) q0 = geo.SE2_from_R2(init_pose) v0 = geo.se2_from_linear_angular(init_vel, 0) tries = { "straight_50": (PWMCommands(+0.5, 0.5)), "straight_max": (PWMCommands(+1.0, +1.0)), "straight_over_max": (PWMCommands(+1.5, +1.5)), "pure_left": (PWMCommands(motor_left=-0.5, motor_right=+0.5)), "pure_right": (PWMCommands(motor_left=+0.5, motor_right=-0.5)), "slight-forward-left": (PWMCommands(motor_left=0, motor_right=0.25)), "faster-forward-right": (PWMCommands(motor_left=0.5, motor_right=0)), # 'slight-right': (PWMCommands(-0.1, 0.1)), } dt = 0.03 t_max = 10 map_data_yaml = """ tiles: - [floor/W,floor/W, floor/W, floor/W, floor/W] - [straight/W , straight/W , straight/W, straight/W, straight/W] - [floor/W,floor/W, floor/W, floor/W, floor/W] tile_size: 0.61 """ map_data = yaml.load(map_data_yaml) root = construct_map(map_data) timeseries = {} for id_try, commands in tries.items(): seq = integrate_dynamics(parameters, q0, v0, dt, t_max, commands) ground_truth = seq.transform_values( lambda t: SE2Transform.from_SE2(t[0])) poses = seq.transform_values(lambda t: t[0]) velocities = get_velocities_from_sequence(poses) linear = velocities.transform_values(linear_from_se2) angular = velocities.transform_values(angular_from_se2) # print(linear.values) # print(angular.values) root.set_object(id_try, DB18(), ground_truth=ground_truth) sequences = {} sequences["motor_left"] = seq.transform_values( lambda _: commands.motor_left) sequences["motor_right"] = seq.transform_values( lambda _: commands.motor_right) plots = TimeseriesPlot(f"{id_try} - PWM commands", "pwm_commands", sequences) timeseries[f"{id_try} - commands"] = plots sequences = {} sequences["linear_velocity"] = linear sequences["angular_velocity"] = angular plots = TimeseriesPlot(f"{id_try} - Velocities", "velocities", sequences) timeseries[f"{id_try} - velocities"] = plots outdir = os.path.join(get_comptests_output_dir(), "together") draw_static(root, outdir, timeseries=timeseries)
def check_car_dynamics_correct(klass, CarCommands, CarParams): """ :param klass: the implementation :return: """ # load one of the maps (see the list using dt-world-draw-maps) outdir = get_comptests_output_dir() dw = load_map('udem1') v = 5 # define a SampledSequence with timestamp, command commands_sequence = SampledSequence.from_iterator([ # we set these velocities at 1.0 (1.0, CarCommands(0.1 * v, 0.1 * v)), # at 2.0 we switch and so on (2.0, CarCommands(0.1 * v, 0.4 * v)), (4.0, CarCommands(0.1 * v, 0.4 * v)), (5.0, CarCommands(0.1 * v, 0.2 * v)), (6.0, CarCommands(0.1 * v, 0.1 * v)), ]) # we upsample the sequence by 5 commands_sequence = commands_sequence.upsample(5) ## Simulate the dynamics of the vehicle # start from q0 and v0 q0 = geo.SE2_from_translation_angle([1.8, 0.7], 0) v0 = geo.se2.zero() c0 = q0, v0 # instantiate the class that represents the dynamics dynamics = CarParams(10.0) # this function integrates the dynamics poses_sequence = get_robot_trajectory(dynamics, q0, commands_sequence) ################# # Visualization and rule evaluation # Creates an object 'duckiebot' ego_name = 'duckiebot' db = DB18() # class that gives the appearance # convert from SE2 to SE2Transform representation transforms_sequence = poses_sequence.transform_values( SE2Transform.from_SE2) # puts the object in the world with a certain "ground_truth" constraint dw.set_object(ego_name, db, ground_truth=transforms_sequence) # Rule evaluation (do not touch) interval = SampledSequence.from_iterator( enumerate(commands_sequence.timestamps)) evaluated = evaluate_rules(poses_sequence=transforms_sequence, interval=interval, world=dw, ego_name=ego_name) timeseries = make_timeseries(evaluated) # Drawing area = RectangularArea((0, 0), (3, 3)) draw_static(dw, outdir, area=area, timeseries=timeseries) expected = { 1.0: geo.SE2_from_translation_angle([1.8, 0.7], 0.0), 1.6: geo.SE2_from_translation_angle([2.09998657, 0.70245831], 0.016389074695313716), 2.0: geo.SE2_from_translation_angle([2.29993783, 0.70682836], 0.027315124492189525), 2.4: geo.SE2_from_translation_angle([2.49991893, 0.70792121], -0.016385672773040847), 2.8: geo.SE2_from_translation_angle([2.69975684, 0.70027647], -0.060086470038271216), 3.2: geo.SE2_from_translation_angle([2.89906999, 0.68390873], -0.10378726730350164), 3.6: geo.SE2_from_translation_angle([3.09747779, 0.65884924], -0.14748806456873198), 4.0: geo.SE2_from_translation_angle([3.2946014, 0.62514586], -0.19118886183396236), 4.6: geo.SE2_from_translation_angle([3.58705642, 0.55852875], -0.25674005773180786), 5.0: geo.SE2_from_translation_angle([3.77932992, 0.50353298], -0.3004408549970382), 6.0: geo.SE2_from_translation_angle([4.2622088, 0.37429798], -0.22257046876429318), } for t, expected_pose in expected.items(): assert np.allclose(poses_sequence.at(t=t), expected_pose), t print('All tests passed successfully!')