Changes to test the changing of a trajectory and then sending a new one
before the next control cycle to prepare for the change.
Removed mutex operations
Removed ModifiedTrajectory structure
4 Bugs fixed:
-Moved checking if the best trajectory from the population class to the planner class
-In Utility::findAngleFromAtoB, if A and B are the same, it will always
return 0 rather than positions.at(2) because positions.at(2) may not
exist or be accurate. Checks for this are added in the code that use
this method.
-Changed floats to doubles in the Utility class. Rounding off for
floats was changing ths orientation between two points too drastically
as the robot moved along a straight line.
-When getting a rotation trajectory from Reflexxes, the global theta
values are not used because Reflexxes does not consider the [-pi, pi]
range. Instead, the starting theta is 0 and the goal is the amount to
turn.
-In Reflexxes, the DOFs that are not selected will have their initial
values used for every point rather than 0

Testing messages were left in the code because I will use those
whenever I get around to using some legitimate logging software.