Motion Planning HW5

Ross Hatton

``Inside" and ``Outside" vectors

Each line segment in my functions is inherently oriented, in that it is defined by an ordered pair of points. I consider the line segment as a vector departing from the first point and arriving at the second.

The boundaries on all my objects (robot and obstacles) are positively oriented (when moving around the object in the direction of its component line segments, the interior of the object is always on the left). I use this property extensively to determine whether a given line segment is inside or outside of any given object.

At the intersection of two line segments, one line segment will have come from ``inside" the other if its vector has a positive dot product with the outward pointing normal vector of the other segment. This rule can be expanded to handle special cases, such as segments which meet at endpoints, though the rules are more complex.

Obstacle expansion

I expand convex obstacles by placing the obstacle at the $ SE(2)$ identity (such that it's world geometry is equal to its local, or body-coordinate, geometry) and then placing each each vertex of the robot at each vertex of the obstacle. A placement is deemed to be ``valid" if it results in the robot being entirely outside the obstacle. The (local coordinates) vertices of the expanded obstacle boundary, referred to as its ``sense" are the positions of the robot's reference point at the valid placements.

The primary test for valid placements is to intersect the robot's line segments with those of the obstacle. If there are any intersections not at the collocated vertices, then the robot is definitely overlapping the obstacle, and the placement being tested is not valid. If there are no intersections, then the robot is either entirely inside or entirely outside of the obstacle. To check this, I find whether the line segment on the robot which ends at the vertex is inside or outside the obstacle. If it is outside, then the robot is outside, and the placement is valid.

Obstacle merging

While non-convex obstacles can be effectively created by placing convex obstacles near each other, I added an obstacle-merge function, which joins concave, expanded obstacles into single objects. For the boundary and sense vertex sets, the merge algorithm follows the boundary of the first obstacle's vertex set, adding each vertex to the set for the merged obstacle, until it comes to an intersection. If the intersection was approached from outside the second obstacle (and thus was part of the true outer boundary), then the algorithm determines which line segment leaving the intersection is outermost, and continues following boundary, switching primary obstacles if necessary.

I have included support for disjoint merged objects, which is useful for merging objects which have overlapping sense fields, but whose base structures do not intersect, and for building complex objects from multiple objects without having to ensure a fixed order for adding objects.

If the algorithm determines that the intersection was approached from the inside, then it starts over from that intersection.

The merge algorithm terminates when it reaches its most recent starting point.

The special cases for this algorithm were tricky to define, and partially responsible for the excessive amounts of time I put into getting this homework finished.

Figure 1:Construction of a convex obstacle. The 12, 9, and 6 o'clock arms have already been merged, and the 3 o'clock arm has been added but not yet merged. The solid lines are the actual obstacle, and the dotted lines are the expanded obstacle. Video of the merge process.

Visibility graph generation and search

The visibility graph constructor uses the naive $ O(n^3)$ algorithm. Two vertices of expanded obstacles are visible to each other if the line between them intersects obstacles only at those vertices and is outside the expanded obstacles at both ends.

The graph search uses a fairly standard $ A^\star$ search. The only feature of the search algorithm that may be of special interest is that I allow for multi-dimensional node indexing, such that the nodes can be referred to with (obstacle, subobstacle, vertex) triples, and the visibility graph can be constructed without precalculating the number of nodes that it will contain.

Successful search

Figure 2:Geography for the success case. The robot starts at the blue dot at the lower right corner of the space and the goal is the red dot at the upper right corner.
Image success_geography

Figure 3: The start and goal are in the same connected space, and so their visibility graphs are also connected.
Image success_visibility

Figure 4: The visibility graph edges searched by the robot in finding the shortest path to the goal. Video of the search progression and the robot following the path found.

Failed search

Figure 5: Geography for the failure case. The robot starts at the blue dot at the lower right corner of the space and the goal is the red dot at the upper right corner.
Image failure_geography

Figure 6: The start and goal are in different connected spaces, and so their visibility graphs are also disconnected.
Image failure_visibility

Figure 7: The visibility graph edges searched by the robot in its unsuccessful attempt at finding the shortest path to the goal. Note that all vertices were visited, and thus each vertex has at least one edge connected to it. Vertices with multiple edges connected to them were either the source of several explored branches or were reached by a better route after being previously explored, Video of the search progression.

Matlab files

Archive of files

The matlab files I have included are

cyclic.m
1-indexed mod function
detect_collisions.m
find intersections between the boundaries of two objects. For this exercise, I also included finding collisions between the expanded boundaries of two objects, for the merge function.
distance.m
Find the Euclidean distance between two points.
draw_object.m
Draw an object.
draw_set.m
Draw all objects in an array.
expand_obstacle.
Given an obstacle and a robot, expand the obstacle.
expand.m
Node expansion for $ A^\star$ search.
line_eq.m
Get an algebraic expression for each line segment of the object.
max2d.m
Get the maximum value and subscripts of that value for a two dimensional matrix.
meetpoint.m
Find the intersection, if any, between two sets of line segments.
motion_planning_hw5_failure.m
Main program for the failure case. Run with argument (1,1,1,1,1,1) to execute the whole code block.
motion_planning_hw5_success.m
Main program for the success case. Run with argument (1,1,1,1,1,1) to execute the whole code block.
obstacle_merge.m
Merge the boundaries and sense boundaries of two obstacles.
outboard_test.m
Test whether a vector is inside or outside a pair of vectors.
pop_n_best.m
Get n_best for the $ A^\star$ search.
update_vertices.m
Update the geometry of an object to reflect its current local geometry and reference state.
visibility_graph.m
Generate the visibility graph for a set of objects.