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)
