def get_path(self, treeA, treeB): nodeA = treeA[-1] pathA = [nodeA.copy()] while nodeA.parent is not None: nodeA = nodeA.parent pathA.append(nodeA.copy()) pathA.reverse() # treeB was built backwards from the goal, so headings # need to be reversed nodeB = treeB[-1] prev_heading = wrap_angle(nodeB.q + pi) if nodeB.parent is None: pathB = [nodeB.copy()] else: pathB = [] while nodeB.parent is not None: nodeB = nodeB.parent (nodeB.q, prev_heading) = (prev_heading, wrap_angle(nodeB.q + pi)) pathB.append(nodeB.copy()) (pathA, pathB) = self.join_paths(pathA, pathB) self.path = pathA + pathB self.smooth_path() target_q = self.target_heading if not isnan(target_q): # Last nodes turn to desired final heading last = self.path[-1] goal = RRTNode(parent=last, x=self.goal.x, y=self.goal.y, q=target_q, radius=0) self.path.append(goal) return (treeA, treeB, self.path)
def interpolate(self, node, target): dx = target.x - node.x dy = target.y - node.y distsq = dx * dx + dy * dy q = atan2(dy, dx) dq = wrap_angle(q - node.q) if abs(dq) > self.max_turn: dq = self.max_turn if dq > 0 else -self.max_turn q = wrap_angle(node.q + dq) if abs(dq) >= self.q_tol: # Must be able to turn to the new heading without colliding turn_dir = +1 if dq >= 0 else -1 q_inc = turn_dir * self.q_tol while abs(q_inc - dq) > self.q_tol: if self.collides(RRTNode(x=node.x, y=node.y, q=node.q + q_inc)): return (self.COLLISION, None) q_inc += turn_dir * self.q_tol if distsq < self.xy_tolsq: return (self.REACHED, RRTNode(parent=node, x=target.x, y=target.y, q=q)) xstep = self.step_size * cos(q) ystep = self.step_size * sin(q) new_node = RRTNode(parent=node, x=node.x + xstep, y=node.y + ystep, q=q) if self.collides(new_node): return (self.COLLISION, None) else: return (self.INTERPOLATE, new_node)
def __init__(self, sdk_obj, id=None, x=0, y=0, z=0, theta=0, rotation=0): # 'theta' is orientation relative to North in particle filter reference frame # 'rotation' is orientation relative to "up" in the camera image if id is None: custom_type = sdk_obj.object_type.name[-2:] id = 'CustomMarkerObj-' + str(custom_type) super().__init__(id, x, y, z) self.theta = wrap_angle(theta) self.sdk_obj = sdk_obj self.marker_number = int(id[-2:]) self.size = self.custom_marker_size if self.sdk_obj: self.orientation, self.rotation, _, _ = \ get_orientation_state(self.sdk_obj.pose.rotation.q0_q1_q2_q3, True) else: self.rotation = wrap_angle(rotation) if abs(self.rotation) < 0.1: self.orientation = geometry.ORIENTATION_UPRIGHT elif abs(self.rotation - pi / 2) < 0.1: self.orientation = geometry.ORIENTATION_LEFT elif abs(self.rotation + pi / 2) < 0.1: self.orientation = geometry.ORIENTATION_RIGHT elif abs(wrap_angle(self.rotation + pi)) < 0.1: self.orientation = geometry.ORIENTATION_INVERTED else: self.orientation = geometry.ORIENTATION_TILTED
def update_coords_from_sdk(self, wmobject, sdk_obj): dx = sdk_obj.pose.position.x - self.robot.pose.position.x dy = sdk_obj.pose.position.y - self.robot.pose.position.y alpha = atan2(dy, dx) - self.robot.pose.rotation.angle_z.radians r = sqrt(dx * dx + dy * dy) (rob_x, rob_y, rob_theta) = self.robot.world.particle_filter.pose wmobject.x = rob_x + r * cos(alpha + rob_theta) wmobject.y = rob_y + r * sin(alpha + rob_theta) wmobject.z = sdk_obj.pose.position.z orient_diff = wrap_angle(rob_theta - self.robot.pose.rotation.angle_z.radians) wmobject.theta = wrap_angle(sdk_obj.pose.rotation.angle_z.radians + orient_diff)
def smooth_path(self): """Smooth a path by picking random subsequences and replacing them with a direct link if there is no collision.""" smoothed_path = self.path for _ in range(0, len(smoothed_path)): L = len(smoothed_path) if L <= 2: break i = random.randrange(0, L - 2) cur_x = smoothed_path[i].x cur_y = smoothed_path[i].y cur_q = smoothed_path[i].q j = random.randrange(i + 2, L) if j < L - 1 and smoothed_path[j + 1].radius != None: continue # j is parent node of an arc segment: don't touch dx = smoothed_path[j].x - cur_x dy = smoothed_path[j].y - cur_y new_q = atan2(dy, dx) dist = sqrt(dx**2 + dy**2) turn_angle = wrap_angle(new_q - cur_q) if abs(turn_angle) <= self.max_turn: result = self.try_linear_smooth(smoothed_path, i, j, cur_x, cur_y, new_q, dist) else: result = self.try_arc_smooth(smoothed_path, i, j, cur_x, cur_y, cur_q) smoothed_path = result or smoothed_path self.path = smoothed_path
def join_paths(self, pathA, pathB): turn_angle = wrap_angle(pathB[0].q - pathA[-1].q) if abs(turn_angle) <= self.max_turn: return (pathA, pathB) print('*** JOIN PATHS EXCEEDED MAX TURN ANGLE: ', turn_angle * 180 / pi) return (pathA, pathB)
def add_obstacle(self, obstacle): obstacle_id = -(1 + len(self.obstacles)) self.obstacles[obstacle_id] = obstacle if isinstance(obstacle, Rectangle): centerX, centerY = obstacle.center[0, 0], obstacle.center[1, 0] width, height = obstacle.dimensions[0], obstacle.dimensions[1] theta = wrap_angle(obstacle.orient) for x in range(floor(centerX - width / 2), ceil(centerX + width / 2), int(self.square_size / 2)): for y in range(floor(centerY - height / 2), ceil(centerY + height / 2), int(self.square_size / 2)): new_x = ((x - centerX) * cos(theta) - (y - centerY) * sin(theta)) + centerX new_y = ((x - centerX) * sin(theta) + (y - centerY) * cos(theta)) + centerY self.set_obstacle_cell(new_x, new_y, obstacle_id) elif isinstance(obstacle, Polygon): raise NotImplemented(obstacle) elif isinstance(obstacle, Circle): raise NotImplemented(obstacle) elif isinstance(obstacle, Compound): raise NotImplemented(obstacle) else: raise Exception("%s has no add_obstacle() method defined for %s." % (self, obstacle))
def update_aruco_landmarks(self): try: seen_marker_objects = self.robot.world.aruco.seen_marker_objects.copy( ) except: return aruco_parent = self.robot.world.aruco for (key, value) in seen_marker_objects.items(): marker_id = value.id_string wmobject = self.objects.get(marker_id, None) if wmobject is None: # TODO: wait to see marker several times before adding. wmobject = ArucoMarkerObj( aruco_parent, key) # coordinates will be filled in below self.objects[marker_id] = wmobject landmark_spec = None else: landmark_spec = self.robot.world.particle_filter.sensor_model.landmarks.get( marker_id, None) wmobject.pose_confidence = +1 if isinstance(landmark_spec, tuple): # Particle filter is tracking this marker wmobject.x = landmark_spec[0][0][0] wmobject.y = landmark_spec[0][1][0] wmobject.theta = landmark_spec[1] elevation = atan2(value.camera_coords[1], value.camera_coords[2]) cam_pos = geometry.point( 0, value.camera_distance * sin(elevation), value.camera_distance * cos(elevation)) base_pos = self.robot.kine.joint_to_base('camera').dot(cam_pos) wmobject.z = base_pos[2, 0] wmobject.elevation = elevation wmobject.cam_pos = cam_pos wmobject.base_pos = base_pos elif isinstance( landmark_spec, Pose): # Hard-coded landmark pose for laboratory exercises wmobject.x = landmark_spec.position.x wmobject.y = landmark_spec.position.y wmobject.theta = landmark_spec.rotation.angle_z.radians wmobject.is_fixed = True else: # Non-landmark: convert aruco sensor values to pf coordinates and update elevation = atan2(value.camera_coords[1], value.camera_coords[2]) cam_pos = geometry.point( 0, value.camera_distance * sin(elevation), value.camera_distance * cos(elevation)) base_pos = self.robot.kine.joint_to_base('camera').dot(cam_pos) wmobject.x = base_pos[0, 0] wmobject.y = base_pos[1, 0] wmobject.z = base_pos[2, 0] wmobject.theta = wrap_angle( self.robot.world.particle_filter.pose[2] + value.euler_rotation[1] * (pi / 180) + pi) wmobject.elevation = elevation wmobject.cam_pos = cam_pos wmobject.base_pos = base_pos
def update_walls(self): for key, value in self.robot.world.particle_filter.sensor_model.landmarks.items( ): if key.startswith('Wall-'): if key in self.objects: wall = self.objects[key] if not wall.is_fixed and not wall.is_foreign: wall.update(self, x=value[0][0][0], y=value[0][1][0], theta=value[1]) else: print('Creating new wall in worldmap:', key) wall_spec = wall_marker_dict[key] wall = WallObj(id=key, x=value[0][0][0], y=value[0][1][0], theta=value[1], length=wall_spec.length, height=wall_spec.height, door_width=wall_spec.door_width, door_height=wall_spec.door_height, marker_specs=wall_spec.marker_specs, doorways=wall_spec.doorways, door_ids=wall_spec.door_ids, is_foreign=False, spec_id=key) self.objects[key] = wall wall.pose_confidence = +1 # Make the doorways wall.make_doorways(self.robot.world.world_map) # Relocate the aruco markers to their predefined positions spec = wall_marker_dict.get(wall.id, None) if spec is None: return for key, value in spec.marker_specs.items(): if key in self.robot.world.world_map.objects: aruco_marker = self.robot.world.world_map.objects[key] dir = value[0] # +1 for front side or -1 for back side s = 0 if dir == +1 else pi aruco_marker.theta = wrap_angle(wall.theta + s) wall_xyz = geometry.point( -dir * (wall.length / 2 - value[1][0]), 0, value[1][1]) rel_xyz = geometry.aboutZ(aruco_marker.theta + pi / 2).dot(wall_xyz) aruco_marker.x = wall.x + rel_xyz[0][0] aruco_marker.y = wall.y + rel_xyz[1][0] aruco_marker.z = rel_xyz[2][0] aruco_marker.is_fixed = wall.is_fixed
def make_arucos(self, world_map): "Called by add_fixed_landmark to make fixed aruco markers." for key, value in self.marker_specs.items(): # Project marker onto the wall; move marker if it already exists marker = world_map.objects.get(key, None) if marker is None: marker_number = int(key[1 + key.rfind('-'):]) marker = ArucoMarkerObj(world_map.robot.world.aruco, marker_number=marker_number) world_map.objects[marker.id] = marker wall_xyz = geometry.point(self.length / 2 - value[1][0], 0, value[1][1]) s = 0 if value[0] == +1 else pi rel_xyz = geometry.aboutZ(self.theta + s).dot(wall_xyz) marker.x = self.x + rel_xyz[1][0] marker.y = self.y + rel_xyz[0][0] marker.z = rel_xyz[2][0] marker.theta = wrap_angle(self.theta + s) marker.is_fixed = self.is_fixed
def calculate_end(self, smoothed_path, parent, new_q, j): # Return False if arc not needed, None if arc not possible, # or pair of new nodes if arc is required. if j == len(smoothed_path) - 1: return False node_j = smoothed_path[j] node_k = smoothed_path[j + 1] next_turn = wrap_angle(node_k.q - new_q) if abs(next_turn) <= self.max_turn: return False dist = sqrt((node_k.x - node_j.x)**2 + (node_k.y - node_j.y)**2) if False and dist < self.arc_radius: return None next_x = node_j.x - self.arc_radius * cos(new_q) next_y = node_j.y - self.arc_radius * sin(new_q) next_node = RRTNode(parent, next_x, next_y, new_q) arc_spec = self.calculate_arc(next_node, node_k) if arc_spec is None: return None (tang_x, tang_y, tang_q, radius) = arc_spec turn_node = RRTNode(next_node, tang_x, tang_y, tang_q, radius=radius) return (next_node, turn_node)
def update_charger(self): charger = self.robot.world.charger if charger is None: return charger_id = 'Charger' wmobject = self.objects.get(charger_id, None) if wmobject is None: wmobject = ChargerObj(charger) self.objects[charger_id] = wmobject wmobject.sdk_obj = charger # In case we created charger before seeing it if self.robot.is_on_charger: wmobject.update_from_sdk = False theta = wrap_angle(self.robot.world.particle_filter.pose[2] + pi) charger_offset = np.array([[-30], [0], [0], [1]]) offset = geometry.aboutZ(theta).dot(charger_offset) wmobject.x = self.robot.world.particle_filter.pose[0] + offset[0, 0] wmobject.y = self.robot.world.particle_filter.pose[1] + offset[1, 0] wmobject.theta = theta wmobject.pose_confidence = +1 elif charger.is_visible: wmobject.update_from_sdk = True wmobject.pose_confidence = +1 elif ((charger.pose is None) or not charger.pose.is_comparable(self.robot.pose)): wmobject.update_from_sdk = False wmobject.pose_confidence = -1 else: # Robot re-localized so charger pose came back pass # skip for now due to SDK bug # wmobject.update_from_sdk = True # wmobject.pose_confidence = max(0, wmobject.pose_confidence) if wmobject.update_from_sdk: # True unless pose isn't comparable self.update_coords_from_sdk(wmobject, charger) wmobject.orientation, _, _, wmobject.theta = get_orientation_state( charger.pose.rotation.q0_q1_q2_q3) return wmobject
def calculate_arc(self, node_i, node_j): # Compute arc node parameters to get us on a heading toward node_j. cur_x = node_i.x cur_y = node_i.y cur_q = node_i.q dest_x = node_j.x dest_y = node_j.y direct_turn_angle = wrap_angle( atan2(dest_y - cur_y, dest_x - cur_x) - cur_q) # find center of arc we'll be moving along dir = +1 if direct_turn_angle >= 0 else -1 cx = cur_x + self.arc_radius * cos(cur_q + dir * pi / 2) cy = cur_y + self.arc_radius * sin(cur_q + dir * pi / 2) dx = cx - dest_x dy = cy - dest_y center_dist = sqrt(dx * dx + dy * dy) if center_dist < self.arc_radius: # turn would be too wide: punt return None # tangent points on arc: outer tangent formula from Wikipedia with r=0 gamma = atan2(dy, dx) beta = asin(self.arc_radius / center_dist) alpha1 = gamma + beta tang_x1 = cx + self.arc_radius * cos(alpha1 + pi / 2) tang_y1 = cy + self.arc_radius * sin(alpha1 + pi / 2) tang_q1 = (atan2(tang_y1 - cy, tang_x1 - cx) + dir * pi / 2) turn1 = tang_q1 - cur_q if dir * turn1 < 0: turn1 += dir * 2 * pi alpha2 = gamma - beta tang_x2 = cx + self.arc_radius * cos(alpha2 - pi / 2) tang_y2 = cy + self.arc_radius * sin(alpha2 - pi / 2) tang_q2 = (atan2(tang_y2 - cy, tang_x2 - cx) + dir * pi / 2) turn2 = tang_q2 - cur_q if dir * turn2 < 0: turn2 += dir * 2 * pi # Correct tangent point has shortest turn. if abs(turn1) < abs(turn2): (tang_x, tang_y, tang_q, turn) = (tang_x1, tang_y1, tang_q1, turn1) else: (tang_x, tang_y, tang_q, turn) = (tang_x2, tang_y2, tang_q2, turn2) # Interpolate along the arc and check for collision. q_traveled = 0 while abs(q_traveled) < abs(turn): cur_x = cx + self.arc_radius * cos(cur_q + q_traveled) cur_y = cy + self.arc_radius * sin(cur_q + q_traveled) if self.collides(RRTNode(None, cur_x, cur_y, cur_q + q_traveled)): return None q_traveled += dir * self.q_tol # Now interpolate from the tangent point to the target. cur_x = tang_x cur_y = tang_y dx = dest_x - cur_x dy = dest_y - cur_y new_q = atan2(dy, dx) dist = sqrt(dx * dx + dy * dy) step_x = self.step_size * cos(new_q) step_y = self.step_size * sin(new_q) traveled = 0 while traveled < dist: traveled += self.step_size cur_x += step_x cur_y += step_y if self.collides(RRTNode(None, cur_x, cur_y, new_q)): return None # No collision, so arc is good. return (tang_x, tang_y, tang_q, dir * self.arc_radius)
def plan_path(self, start, goal, max_turn=pi, arc_radius=40): self.max_turn = max_turn self.arc_radius = arc_radius if self.auto_obstacles: obstacle_inflation = 5 doorway_adjustment = +77 # widen doorways for RRT self.generate_obstacles(obstacle_inflation, doorway_adjustment) self.start = start self.goal = goal self.target_heading = goal.q self.compute_bounding_box() # Check for StartCollides collider = self.collides(start) if collider: raise StartCollides(start, collider, collider.obstacle_id) # Set up treeA with start node treeA = [start.copy()] self.treeA = treeA # Set up treeB with goal node(s) if not isnan(self.target_heading): offset_x = goal.x + center_of_rotation_offset * cos(goal.q) offset_y = goal.y + center_of_rotation_offset * sin(goal.q) offset_goal = RRTNode(x=offset_x, y=offset_y, q=goal.q) collider = self.collides(offset_goal) if collider: raise GoalCollides(goal, collider, collider.obstacle_id) treeB = [offset_goal] self.treeB = treeB else: # target_heading is nan treeB = [goal.copy()] self.treeB = treeB temp_goal = goal.copy() offset_goal = goal.copy() for theta in range(0, 360, 10): q = theta / 180 * pi step = max(self.step_size, abs(center_of_rotation_offset)) temp_goal.x = goal.x + step * cos(q) temp_goal.y = goal.y + step * sin(q) temp_goal.q = wrap_angle(q + pi) collider = self.collides(temp_goal) if collider: continue offset_goal.x = temp_goal.x + center_of_rotation_offset * cos( q) offset_goal.y = temp_goal.y + center_of_rotation_offset * sin( q) offset_goal.q = temp_goal.q collider = self.collides(offset_goal) if not collider: treeB.append( RRTNode(parent=treeB[0], x=temp_goal.x, y=temp_goal.y, q=temp_goal.q)) if len(treeB) == 1: raise GoalCollides(goal, collider, collider.obstacle_id) # Set bounds for search area self.compute_world_bounds(start, goal) # Grow the RRT until trees meet or max_iter exceeded swapped = False for i in range(self.max_iter): r = self.random_node() (status, new_node) = self.extend(treeA, r) if status is not self.COLLISION: (status, new_node) = self.extend(treeB, treeA[-1]) if status is self.REACHED: break (treeA, treeB) = (treeB, treeA) swapped = not swapped # Search terminated. Check for success. if swapped: (treeA, treeB) = (treeB, treeA) if status is self.REACHED: return self.get_path(treeA, treeB) else: raise MaxIterations(self.max_iter)