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 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 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 get_SE2transform(x: PlatformDynamics) -> SE2Transform:
check_isinstance(x, PlatformDynamics)
q, v = x.TSE2_from_state()
return SE2Transform.from_SE2(q)