Motion Planning HW5
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.
I expand convex obstacles by placing the obstacle at the
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.
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.
The visibility graph constructor uses the naive
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
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.
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
The start and goal are in the same connected space, and so their visibility graphs are also connected.
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.
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.
The start and goal are in different connected spaces, and so their visibility graphs are also disconnected.
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.
Archive of files
The matlab files I have included are
- 1-indexed mod function
- 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.
- Find the Euclidean distance between two points.
- Draw an object.
- Draw all objects in an array.
- Given an obstacle and a robot, expand the obstacle.
- Node expansion for
- Get an algebraic expression for each line segment of the object.
- Get the maximum value and subscripts of that value for a two dimensional matrix.
- Find the intersection, if any, between two sets of line segments.
- Main program for the failure case. Run with argument (1,1,1,1,1,1) to execute the whole code block.
- Main program for the success case. Run with argument (1,1,1,1,1,1) to execute the whole code block.
- Merge the boundaries and sense boundaries of two obstacles.
- Test whether a vector is inside or outside a pair of vectors.
n_best for the
- Update the geometry of an object to reflect its current local geometry and reference state.
- Generate the visibility graph for a set of objects.