def update_shape(self, pose: PoseR2S2):
# adjust the tractor vertices based on articulation angel
o = PoseR2S2(pose.x, pose.y, pose.theta)
p0 = o.compose_point(PointR2(0, -self.tractor_w / 2.))
p1 = o.compose_point(PointR2(self.tractor_l, -self.tractor_w / 2.))
p2 = o.compose_point(PointR2(self.tractor_l, self.tractor_w / 2.))
p3 = o.compose_point(PointR2(0, self.tractor_w / 2.))
self.shape = (p0, p1, p2, p3)
def update_shape(self, phi: float):
# adjust the trailer vertices based on articulation angel
self.last_hitch_pose.theta = -phi
p6 = self.last_hitch_pose.compose_point(
PointR2(0, -self.trailer_w / 2.))
p7 = self.last_hitch_pose.compose_point(PointR2(
0, self.trailer_w / 2.))
p8 = self.last_hitch_pose.compose_point(
PointR2(-self.trailer_l, self.trailer_w / 2.))
p9 = self.last_hitch_pose.compose_point(
PointR2(-self.trailer_l, -self.trailer_w / 2.))
self.shape = (*self.shape[:6], p6, p7, p8, p9)
def transform_toTP_obstacles(ptg: PTG, obstacles_ws: List[PointR2], k: int,
max_dist: float) -> List[float]:
# obs_TP = [] # type: List[float]
# for k in range(len(ptg.cpoints)):
# obs_TP.append(ptg.distance_ref)
# If you just turned 180deg, end there
# if len(ptg.cpoints[k]) == 0:
# obs_TP[k] = 0. # Invalid configuration
# continue
# phi = ptg.cpoints[k][-1].theta
# if abs(phi) >= np.pi * 0.95:
# obs_TP[k] = ptg.cpoints[k][-1].d
obs_TP = [ptg.distance_ref] * len(ptg.cpoints) # type: List[float]
for i in range(np.shape(obstacles_ws)[1]):
if abs(obstacles_ws[0][i]) > max_dist or abs(
obstacles_ws[1][i]) > max_dist:
continue
obstacle = PointR2(obstacles_ws[0][i], obstacles_ws[1][i])
collision_cell = ptg.obstacle_grid.cell_by_pos(obstacle)
if collision_cell is None:
continue
# assert collision_cell is not None, 'collision cell is empty!'
# get min_dist for the current k
for kd_pair in collision_cell:
if kd_pair.k == k and kd_pair.d < obs_TP[kd_pair.k]:
obs_TP[kd_pair.k] = kd_pair.d
break
return obs_TP
def get_tractor_vertices_at_pose(self, pose: PoseR2S2) -> List[PointR2]:
result = []
for i in range(4):
vertex = self.shape[i]
point = PointR2()
point.x = pose.x + cos(pose.theta) * vertex.x - sin(
pose.theta) * vertex.y
point.y = pose.y + sin(pose.theta) * vertex.x + cos(
pose.theta) * vertex.y
result.append(point)
return result
def update_shape(self, pose: PoseR2S2):
# adjust the tractor vertices based on articulation angel
theta2 = (pose.theta - pose.phi)
o = PoseR2S2(pose.x, pose.y, theta2, 0.)
p0 = o.compose_point(PointR2(0, self.trailer_w / 2.))
p1 = o.compose_point(PointR2(0, -self.trailer_w / 2.))
p2 = o.compose_point(PointR2(self.trailer_l, -self.trailer_w / 2.))
p3 = o.compose_point(PointR2(self.trailer_l, self.trailer_w / 2.))
p4 = o.compose_point(PointR2(self.trailer_l, 0.))
p5 = o.compose_point(PointR2(self.trailer_l + self.link_l, 0.))
hitch_pose = PoseR2S2(p5.x, p5.y, pose.theta)
p6 = hitch_pose.compose_point(PointR2(0, -self.tractor_w / 2.))
p9 = hitch_pose.compose_point(PointR2(0, self.tractor_w / 2.))
p8 = hitch_pose.compose_point(
PointR2(self.tractor_l, self.tractor_w / 2.))
p7 = hitch_pose.compose_point(
PointR2(self.tractor_l, -self.tractor_w / 2.))
self.shape = (p0, p1, p2, p3, p4, p5, p6, p7, p8, p9)
self.hitch_pose = hitch_pose
self.last_phi = pose.phi
def get_vertices_at_pose(self, pose: PoseR2S2) -> List[PointR2]:
if pose.phi != self.last_phi:
self.update_shape(pose.phi)
result = []
for vertex in self.shape:
point = PointR2()
point.x = pose.x + cos(pose.theta) * vertex.x - sin(
pose.theta) * vertex.y
point.y = pose.y + sin(pose.theta) * vertex.x + cos(
pose.theta) * vertex.y
result.append(point)
return result
def __init__(self, config: dict):
self.v_max = config['v_max'] # type: float
self.w_max = rad(config['w_max']) # type: float
self.phi_max = 0.
self.tractor_w = config['tractor_w'] # type: float
self.tractor_l = config['tractor_l'] # type: float
# vertices numbered as shown below
# Origin is the center of the tractor back axle
#
# 3-------2
# | |
# o | Trk_W
# | |
# 0-------1
# Trk_L
o = PoseR2S2(0., 0., 0., 0.)
p0 = o.compose_point(PointR2(0, -self.tractor_w / 2.))
p1 = o.compose_point(PointR2(self.tractor_l, -self.tractor_w / 2.))
p2 = o.compose_point(PointR2(self.tractor_l, self.tractor_w / 2.))
p3 = o.compose_point(PointR2(0, self.tractor_w / 2.))
self.shape = (p0, p1, p2, p3)
def __init__(self, config: dict):
# check the provided config file for details on the variables initialized
self.v_max = config['v_max'] # type: float
self.w_max = rad(config['w_max']) # type: float
self.phi_max = rad(config['phi_max']) # type: float
self.tractor_w = config['tractor_w'] # type: float
self.tractor_l = config['tractor_l'] # type: float
self.link_l = config['link_l'] # type: float
self.trailer_w = config['trailer_w'] # type: float
self.trailer_l = config['trailer_l'] # type: float
self.last_phi = 0. # keeps track of changes to phi in order to update vehicle shape
# vertices numbered as shown below
#
# 8----------------7 2-------1
# | | | |
# Trl_W | 5-------4 o | Trk_W
# | | L_L | |
# 9----------------6 3-------0
# Trl_L Trk_L
o = PoseR2S2(0., 0., 0., 0.)
p0 = o.compose_point(PointR2(self.tractor_l / 2.,
-self.tractor_w / 2.))
p1 = o.compose_point(PointR2(self.tractor_l / 2., self.tractor_w / 2.))
p2 = o.compose_point(PointR2(-self.tractor_l / 2.,
self.tractor_w / 2.))
p3 = o.compose_point(
PointR2(-self.tractor_l / 2., -self.tractor_w / 2.))
p4 = o.compose_point(PointR2(-self.tractor_l / 2., 0.))
p5 = o.compose_point(PointR2(-self.tractor_l / 2. - self.link_l, 0.))
hitch_pose = PoseR2S2(p5.x, p5.y, self.last_phi)
p6 = hitch_pose.compose_point(PointR2(0, -self.trailer_w / 2.))
p7 = hitch_pose.compose_point(PointR2(0, self.trailer_w / 2.))
p8 = hitch_pose.compose_point(
PointR2(-self.trailer_l, self.trailer_w / 2.))
p9 = hitch_pose.compose_point(
PointR2(-self.trailer_l, -self.trailer_w / 2.))
self.shape = (p0, p1, p2, p3, p4, p5, p6, p7, p8, p9)
self.last_hitch_pose = hitch_pose
def build_obstacle_grid(self):
"""
For each cpoint in cpoints at a given alpha:
1- place the vehicle at cpoint.pose
2- See which cells it collides with
3- Update the cells with alpha and d values
"""
from prrt.primitive import get_bounding_box, polygon_contains_point as polygon_contains_point
assert len(self.cpoints) > 0, 'cpoints don\'t exist!'
for k in range(len(self.idx_to_alpha)):
cpoints_at_k = self.cpoints[k]
for cpoint in cpoints_at_k:
shape_tractor = self.vehicle.get_tractor_vertices_at_pose(
cpoint.pose)
shape_trailer = self.vehicle.get_trailer_vertices_at_pose(
cpoint.pose)
shapes = [shape_tractor, shape_trailer]
for shape in shapes:
shape_bb = get_bounding_box(shape)
x_idx_min = max(0,
self.obstacle_grid.x_to_ix(shape_bb[0].x))
y_idx_min = max(0,
self.obstacle_grid.y_to_iy(shape_bb[0].y))
x_idx_max = min(self.obstacle_grid.cell_count_x - 1,
self.obstacle_grid.x_to_ix(shape_bb[1].x))
y_idx_max = min(self.obstacle_grid.cell_count_y - 1,
self.obstacle_grid.y_to_iy(shape_bb[1].y))
for x_idx in range(x_idx_min - 1, x_idx_max + 1):
cell = PointR2()
cell.x = self.obstacle_grid.idx_to_x(x_idx)
for y_idx in range(y_idx_min - 1, y_idx_max + 1):
cell.y = self.obstacle_grid.idx_to_y(y_idx)
if polygon_contains_point(shape, cell, shape_bb):
self.obstacle_grid.update_cell(
x_idx, y_idx, k, cpoint.d)
self.obstacle_grid.update_cell(
x_idx - 1, y_idx, k, cpoint.d)
self.obstacle_grid.update_cell(
x_idx, y_idx - 1, k, cpoint.d)
self.obstacle_grid.update_cell(
x_idx - 1, y_idx - 1, k, cpoint.d)
print('Completed building obstacle grid for {0}'.format(self.name))