def test_textured_raytracer_box():
world = Box(10, 10)
sensor = TexturedRaytracer(directions=np.linspace(-np.pi / 2, +np.pi /
2, 181))
sensor.set_world_primitives(world.get_primitives())
pose = SE2_from_translation_angle([0, 0], 0)
sensor.raytracing(pose)
def generate_votes(self, segment, delta):
"""
yields xytheta, weight
"""
p1 = np.array([segment.points[0].x, segment.points[0].y, segment.points[0].z])
p2 = np.array([segment.points[1].x, segment.points[1].y, segment.points[1].z])
p_hat = 0.5 * p1 + 0.5 * p2
d = np.linalg.norm(p1 - p2)
weight = d
n_hat = get_normal_outward_for_segment(p2, p1)
# SegmentsMap
sm = self._localization_template.get_map()
num = 0
for map_segment in sm.segments:
if map_segment.color == segment.color:
for p, n in iterate_segment_sections(sm, map_segment, delta):
xy, theta = get_estimate(p, n, p_hat, n_hat)
pose = SE2_from_translation_angle(xy, theta)
yield pose, weight
num += 1
if num == 0:
msg = f"No segment found for {segment.color}"
dtu.logger.debug(msg)
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)
def add_sign(k: str, rel, theta_deg):
nonlocal i
obname = f"{i}-{k}"
i += 1
q = SE2_from_translation_angle(rel, np.deg2rad(theta_deg))
logger.info(tile.transform_sequence)
m1 = tile.transform_sequence.asmatrix2d().m
pose = m1 @ q
st = dw.SE2Transform.from_SE2(pose)
objects[obname] = {"kind": k, "pose": st.as_json_dict(), "height": 0.18}
def instance_vehicle_spec(entry):
from ..simulation import Vehicle
from ..interfaces import VehicleSensor, Dynamics
from vehicles.configuration.master import get_conftools_dynamics, \
get_conftools_sensors
check_valid_vehicle_config(entry)
try:
master = get_conftools_dynamics()
if 'id_dynamics' in entry:
id_dynamics = entry['id_dynamics']
dynamics = master.instance(id_dynamics)
else:
id_dynamics = entry['dynamics']['id']
dynamics = master.instance_spec(entry['dynamics'])
assert isinstance(dynamics, Dynamics)
sensors = entry['sensors']
radius = entry['radius']
extra = entry.get('extra', {})
vehicle = Vehicle(radius=radius, extra=extra)
vehicle.add_dynamics(id_dynamics, dynamics)
master = get_conftools_sensors()
for sensor in sensors:
if 'id_sensor' in sensor:
id_sensor = sensor['id_sensor']
sensor_instance = master.instance(id_sensor)
else:
id_sensor = sensor['sensor']['id']
sensor_instance = master.instance_spec(sensor['sensor'])
assert isinstance(sensor_instance, VehicleSensor)
# TODO: document this
x, y, theta_deg = sensor.get('pose', [0, 0, 0])
theta = np.radians(theta_deg)
pose = SE2_from_translation_angle([x, y], theta)
pose = SE3_from_SE2(pose)
joint = sensor.get('joint', 0)
extra = sensor.get('extra', {})
vehicle.add_sensor(id_sensor=id_sensor,
sensor=sensor_instance,
pose=pose, joint=joint,
extra=extra)
return vehicle
except:
logger.error('Error while trying to instantiate vehicle. Entry:\n%s'
% (pformat(entry)))
raise
def pose2state(self, pose):
'''
Returns the state that best approximates
the given pose (in SE3).
'''
# random_angle = SO2.convert_to(SE3, SO2.sample_uniform()) # XXX
# random_angle = SE3_from_SE2(SE2_from_SO2(SO2.sample_uniform()))
# angle = BaseTopDynamics.last_turret_angle
angle = np.random.rand() * np.pi * 2
turret_pose = SE3_from_SE2(SE2_from_translation_angle([0, 0], angle))
# print('Starting at %s deg ' % np.rad2deg(angle))
return self.compose_state(base=self.base.pose2state(pose),
top=self.top.pose2state(turret_pose))
def test_raytracer_box():
world = Box(10, 10)
sensor = MyRaytracer(directions=np.linspace(0, np.pi * 2, 181))
sensor.set_world_primitives(world.get_primitives())
pose = SE2_from_translation_angle([0, 0], 0)
observations = sensor.raytracing(pose)
readings = np.array(observations['readings'])
#coords = np.array(observations['curvilinear_coordinate'])
# print observations
assert np.array(observations['valid']).all()
assert (readings <= 10 * np.sqrt(2) + 0.0001).all()
assert (readings >= 10).all()
hit, _ = sensor.query_circle([0, 0], 1)
assert not hit
hit, _ = sensor.query_circle([0, 0], 11)
assert hit
def joint_state(self, state, joint=0):
car_state = self.car_state_from_big_state(state)
steering = self.steering_from_big_state(state)
car_position = self.car.joint_state(car_state, joint=0)
if joint == 0:
return car_position
elif joint == 1:
p = [self.car.axis_dist, self.car.L]
elif joint == 2:
p = [self.car.axis_dist, -self.car.L]
else:
raise ValueError('Invalid joint ID %r' % joint)
pose, vel = car_position
rel_pose = SE3_from_SE2(SE2_from_translation_angle(p, steering))
wpose = SE3.multiply(pose, rel_pose)
return wpose, vel # XXX: vel
def sample_trees(po: dw.PlacedObject, tree_density: float,
tree_min_dist: float) -> List[SE2value]:
if tree_density == 0:
return []
def choose_grass(x: dw.PlacedObject) -> bool:
if isinstance(x, dw.Tile) and x.kind == "floor":
return True
else:
return False
def the_others(x: dw.PlacedObject) -> bool:
if isinstance(x, dw.Tile) and x.kind != "floor":
return True
else:
return False
choices = list(iterate_by_test(po, choose_grass))
non_grass = list(iterate_by_test(po, the_others))
# logger.info(choices=choices)
if not choices:
msg = "Cannot put trees in grass"
tiles = list(_.object.kind for _ in iterate_by_class(po, dw.Tile))
raise ZValueError(msg, tiles=tiles)
ntrees = int(len(choices) * tree_density)
results: List[SE2value] = []
def far_enough(pose_):
for p in results:
if distance_poses(p, pose_) < tree_min_dist:
return False
return True
options = list(range(len(choices)))
attitude = [None] * len(choices)
a: int
for a, c in enumerate(choices):
p1 = c.transform_sequence.asmatrix2d().m
min_distance = 100.0
sum_distances = 0.0
for k, s in enumerate(non_grass):
p2 = s.transform_sequence.asmatrix2d().m
d = distance_poses(p1, p2)
sum_distances += d
# if d < 0.4:
# logger.info(a=a, k=k, d=d)
# raise ZValueError()
min_distance = min(d, min_distance)
# noinspection PyTypeChecker
attitude[a] = 1.0 / sum_distances
# logger.info(attitude=attitude)
nplaced = [0] * len(choices)
for i in range(ntrees):
while True:
weights = 1.0 / (np.array(nplaced, dtype=float) * 3 + 1)
weights2 = weights * np.array(attitude)
weights3 = weights2 / np.sum(weights2)
chosen = choice(options, p=weights3)
itr = choices[chosen]
m1 = itr.transform_sequence.asmatrix2d().m
t = [random.uniform(-0.4, 0.4), random.uniform(-0.4, 0.4)]
pos_in_tile = SE2_from_translation_angle(t, 0.0)
pose = m1 @ pos_in_tile
if far_enough(pose):
results.append(pose)
nplaced[chosen] += 1
break
return results
assert_allclose(pose2, pose, atol=1e-8)
def comparison_test_2():
''' Compares between se2_from_SE2 and se2_from_SE2_slow. '''
for pose in SE2.interesting_points():
se2a = se2_from_SE2(pose)
se2b = se2_from_SE2_slow(pose)
#printm('pose', pose, 'se2a', se2a, 'se2b', se2b)
assert_allclose(se2a, se2b, atol=1e-8)
# Known pairs of pose, algebra
known_pairs = [
(
SE2_from_translation_angle([0, 0], np.pi),
np.array([
[0, +np.pi, 0], # Note: should be - if normalize_pi is changed
[-np.pi, 0, 0],
[0, 0, 0]
])),
(np.array([[-1, 0, 0], [0, -1, 0],
[0, 0, 1]]), np.array([[0, np.pi, 0], [-np.pi, 0, 0], [0, 0,
0]])),
(SE2_from_translation_angle([0, 0], 0), np.zeros((3, 3))),
]
def check_pi_test():
for g, w in known_pairs:
w2 = se2_from_SE2(g)
def pose_from_friendly(p: FriendlyPose) -> SE2value:
return SE2_from_translation_angle([p.x, p.y], np.deg2rad(p.theta_deg))
