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))