def GetDestinationLocationAndOrientation(self, x, y, orientation,
steering_angle, distance):
if geom_util.AngleAlmostEqual(steering_angle, 0.0):
nx = x + math.cos(orientation) * distance
ny = y + math.sin(orientation) * distance
no = orientation
elif geom_util.AngleAlmostEqual(steering_angle, math.pi):
nx = x - math.cos(orientation) * distance
ny = y - math.sin(orientation) * distance
no = orientation
else:
radius = self.vehicle_length / math.sin(abs(steering_angle))
circ_angle = distance / radius
dyc = math.sin(circ_angle) * radius
dy = math.sin(orientation) * dyc
dx = math.cos(orientation) * dyc
dxc = radius - math.cos(circ_angle) * radius
dxc_angle = (orientation + 0.5 * math.pi) % ANGLE_MOD
no = orientation + circ_angle
if steering_angle < 0.0:
dxc_angle = (orientation - 0.5 * math.pi) % ANGLE_MOD
no = orientation - circ_angle
dy += math.sin(dxc_angle) * dxc
dx += math.cos(dxc_angle) * dxc
nx = x + dx
ny = y + dy
return nx, ny, (no % ANGLE_MOD)
def CreatePath(start_config, engine, obstacles, max_walk, ideal=True):
current_path = []
current_config = start_config
walk_limit = max_walk
if ideal:
sample_func = engine.grid.GetDestinationLocationAndOrientation
else:
sample_func = engine.grid.SampleMovement
while (walk_limit and not markov.CloseToGoalConst(
current_config, engine.goal_config, engine.grid)
and not obstacles.contains(
Point(current_config[0][0], current_config[0][1]))
and not markov.OutsideBoundaries(current_config)):
walk_limit -= 1
steering_angle = engine.GetSteeringAngle(current_config)
x, y, o = sample_func(current_config[0][0], current_config[0][1],
current_config[1], steering_angle,
engine.average_path_length)
if geom_util.AngleAlmostEqual(steering_angle % ANGLE_MOD, 0.0):
current_path.append(LineString([current_config[0], (x, y)]))
ls = LineString([current_config[0], (x, y)])
if ls.crosses(obstacles):
break
else:
radius = engine.vehicle_length / math.sin(steering_angle)
antirangle = (math.pi / 2.0 - current_config[1]) % ANGLE_MOD
dx = math.cos(antirangle) * radius
dy = math.sin(antirangle) * radius
circumrad = engine.average_path_length / abs(radius)
if steering_angle > 0.0:
direction = COUNTER_CLOCKWISE
begin_radian = (current_config[1] - math.pi / 2.0) % ANGLE_MOD
end_radian = (begin_radian + circumrad) % ANGLE_MOD
else:
direction = CLOCKWISE
begin_radian = (current_config[1] + math.pi / 2.0) % ANGLE_MOD
end_radian = (begin_radian - circumrad) % ANGLE_MOD
current_path.append(
Arc(
(current_config[0][0] - dx, current_config[0][1] + dy),
abs(radius),
begin_radian,
end_radian,
direction,
))
end_pos = current_path[-1].AngleToPoint(end_radian)
current_path.append(LineString([end_pos, (x, y)]))
current_config = ((x, y), o)
state = SUCCESS
if not markov.CloseToGoalConst(current_config, engine.goal_config,
engine.grid):
state = ICED
if not walk_limit:
state = TLE
return current_path, state
def CanArcGo(self, arc):
assert arc.circle_pk is not None
circle = self.circle_pk_to_circle[arc.circle_pk]
if len(circle.intersection_angles) == 1:
return not circle.intersection_is_obstructed[0]
for angle1, angle2, is_obstructed in zip(
circle.intersection_angles,
circle.intersection_angles[1:] + circle.intersection_angles[:1],
circle.intersection_is_obstructed):
if is_obstructed:
if (geom_util.IsAngleBetween(arc._begin_radian, angle1, angle2) or
geom_util.IsAngleBetween(arc._end_radian, angle1, angle2) or
geom_util.IsAngleBetween(angle1, arc._begin_radian, arc._end_radian) or
geom_util.IsAngleBetween(angle2, arc._begin_radian, arc._end_radian) or
geom_util.AngleAlmostEqual(arc._begin_radian, angle1) or
geom_util.AngleAlmostEqual(arc._end_radian, angle2)
):
return False
return True
def GetSteeringAngle(self, configuration):
goal_angle = math.atan2(
self.goal_config[0][1] - configuration[0][1],
self.goal_config[0][0] - configuration[0][0]) % ANGLE_MOD
if geom_util.AngleAlmostEqual(goal_angle, configuration[1]):
return 0.0
leftdist = goal_angle - configuration[1]
rightdist = configuration[1] - goal_angle
if (leftdist % ANGLE_MOD < rightdist % ANGLE_MOD):
return self.max_steering_angle
else:
return -self.max_steering_angle
def ConfigurationToState(self, configuration):
grid_size = 1.0 / self.axis_resolution
base_r = int(math.floor(configuration[0][0] / grid_size))
base_c = int(math.floor(configuration[0][1] / grid_size))
def Normalize(v):
if v < 0:
v = 0
if v >= self.axis_resolution:
v = self.axis_resolution - 1
return v
r = Normalize(base_r)
c = Normalize(base_c)
for io in range(self.orientation_resolution):
_, _, (o1, o2) = self.StateToCoorRange((0, 0, io))
if geom_util.IsAngleBetween(
configuration[1], o1, o2) or geom_util.AngleAlmostEqual(
configuration[1], o1) or geom_util.AngleAlmostEqual(
configuration[1], o2):
return (r, c, io)
# Already in ice/goal
assert False
def testStateToRangeCoor(self):
(xl, xh), (yl, yh), (ol, oh) = self.grid.StateToCoorRange((0, 1, 2))
self.assertAlmostEquals(xl, 0.0)
self.assertAlmostEquals(xh, 0.2)
self.assertAlmostEquals(yl, 0.2)
self.assertAlmostEquals(yh, 0.4)
self.assertAlmostEquals(ol, math.pi)
self.assertAlmostEquals(oh, math.pi * 3.0 / 2.0)
(xl, xh), (yl, yh), (ol, oh) = self.grid.StateToCoorRange((4, 4, 3))
self.assertAlmostEquals(xl, 0.8)
self.assertAlmostEquals(xh, 1.0)
self.assertAlmostEquals(yl, 0.8)
self.assertAlmostEquals(yh, 1.0)
self.assertAlmostEquals(ol, math.pi * 3.0 / 2.0)
self.assertTrue(geom_util.AngleAlmostEqual(oh, math.pi * 2.0))
def testGetDestinationLocationAndOrientation(self):
x, y, o = self.grid.GetDestinationLocationAndOrientation(
0.0, 1.0, math.pi / 2.0, 0.0, 0.2)
self.assertAlmostEquals(x, 0.0)
self.assertAlmostEquals(y, 1.0 + 0.2)
self.assertAlmostEquals(o, math.pi / 2.0)
x, y, o = self.grid.GetDestinationLocationAndOrientation(
0.0, 1.0, math.pi / 2.0, math.pi, 0.2)
self.assertAlmostEquals(x, 0.0)
self.assertAlmostEquals(y, 1.0 - 0.2)
self.assertAlmostEquals(o, math.pi / 2.0)
g = grid.Grid(axis_resolution=5,
orientation_resolution=4,
average_path_length=math.pi / 2.0,
vehicle_length=1.0,
x_variance=0.05,
y_variance=0.05,
orientation_variance=0.1)
x, y, o = g.GetDestinationLocationAndOrientation(
0.0, 1.0, math.pi / 2.0, math.pi / 2.0, math.pi / 2.0)
self.assertAlmostEquals(x, -1.0)
self.assertAlmostEquals(y, 2.0)
self.assertAlmostEquals(o, math.pi)
g = grid.Grid(axis_resolution=5,
orientation_resolution=4,
average_path_length=math.pi / 2.0,
vehicle_length=1.0,
x_variance=0.05,
y_variance=0.05,
orientation_variance=0.1)
x, y, o = g.GetDestinationLocationAndOrientation(
0.0, 1.0, math.pi / 2.0, -math.pi / 2.0, math.pi / 2.0)
self.assertAlmostEquals(x, 1.0)
self.assertAlmostEquals(y, 2.0)
self.assertTrue(geom_util.AngleAlmostEqual(o, 0.0))