diff --git a/aos/util/trapezoid_profile.py b/aos/util/trapezoid_profile.py
index af85339..76c9758 100644
--- a/aos/util/trapezoid_profile.py
+++ b/aos/util/trapezoid_profile.py
@@ -2,8 +2,9 @@
 
 import numpy
 
+
 class TrapezoidProfile(object):
-  """Computes a trapezoidal motion profile
+    """Computes a trapezoidal motion profile
 
   Attributes:
     _acceleration_time: the amount of time the robot will travel at the
@@ -21,136 +22,140 @@
     _timestep: time between calls to Update (delta_time)
     _output: output array containing distance to goal and velocity
   """
-  def __init__(self, delta_time):
-    """Constructs a TrapezoidProfile.
+
+    def __init__(self, delta_time):
+        """Constructs a TrapezoidProfile.
 
     Args:
       delta_time: time between calls to Update (seconds)
     """
-    self._acceleration_time = 0
-    self._acceleration = 0
-    self._constant_time = 0
-    self._deceleration_time = 0
-    self._deceleration = 0
+        self._acceleration_time = 0
+        self._acceleration = 0
+        self._constant_time = 0
+        self._deceleration_time = 0
+        self._deceleration = 0
 
-    self._maximum_acceleration = 0
-    self._maximum_velocity = 0
-    self._timestep = delta_time
+        self._maximum_acceleration = 0
+        self._maximum_velocity = 0
+        self._timestep = delta_time
 
-    self._output = numpy.array(numpy.zeros((2,1)))
+        self._output = numpy.array(numpy.zeros((2, 1)))
 
-  # Updates the state
-  def Update(self, goal_position, goal_velocity):
-    self._CalculateTimes(goal_position - self._output[0], goal_velocity)
+    # Updates the state
+    def Update(self, goal_position, goal_velocity):
+        self._CalculateTimes(goal_position - self._output[0], goal_velocity)
 
-    next_timestep = self._timestep
+        next_timestep = self._timestep
 
-    # We now have the amount of time we need to accelerate to follow the
-    # profile, the amount of time we need to move at constant velocity
-    # to follow the profile, and the amount of time we need to decelerate to
-    # follow the profile.  Do as much of that as we have time left in dt.
-    if self._acceleration_time > next_timestep:
-      self._UpdateVals(self._acceleration, next_timestep)
-    else:
-      self._UpdateVals(self._acceleration, self._acceleration_time)
-
-      next_timestep -= self._acceleration_time
-      if self._constant_time > next_timestep:
-        self._UpdateVals(0, next_timestep)
-      else:
-        self._UpdateVals(0, self._constant_time)
-        next_timestep -= self._constant_time;
-        if self._deceleration_time > next_timestep:
-          self._UpdateVals(self._deceleration, next_timestep)
+        # We now have the amount of time we need to accelerate to follow the
+        # profile, the amount of time we need to move at constant velocity
+        # to follow the profile, and the amount of time we need to decelerate to
+        # follow the profile.  Do as much of that as we have time left in dt.
+        if self._acceleration_time > next_timestep:
+            self._UpdateVals(self._acceleration, next_timestep)
         else:
-          self._UpdateVals(self._deceleration, self._deceleration_time)
-          next_timestep -= self._deceleration_time
-          self._UpdateVals(0, next_timestep)
+            self._UpdateVals(self._acceleration, self._acceleration_time)
 
-    return self._output
+            next_timestep -= self._acceleration_time
+            if self._constant_time > next_timestep:
+                self._UpdateVals(0, next_timestep)
+            else:
+                self._UpdateVals(0, self._constant_time)
+                next_timestep -= self._constant_time
+                if self._deceleration_time > next_timestep:
+                    self._UpdateVals(self._deceleration, next_timestep)
+                else:
+                    self._UpdateVals(self._deceleration,
+                                     self._deceleration_time)
+                    next_timestep -= self._deceleration_time
+                    self._UpdateVals(0, next_timestep)
 
-  # Useful for preventing windup etc.
-  def MoveCurrentState(self, current):
-    self._output = current
+        return self._output
 
-  # Useful for preventing windup etc.
-  def MoveGoal(self, dx):
-    self._output[0] += dx
+    # Useful for preventing windup etc.
+    def MoveCurrentState(self, current):
+        self._output = current
 
-  def SetGoal(self, x):
-    self._output[0] = x
+    # Useful for preventing windup etc.
+    def MoveGoal(self, dx):
+        self._output[0] += dx
 
-  def set_maximum_acceleration(self, maximum_acceleration):
-    self._maximum_acceleration = maximum_acceleration
+    def SetGoal(self, x):
+        self._output[0] = x
 
-  def set_maximum_velocity(self, maximum_velocity):
-    self._maximum_velocity = maximum_velocity
+    def set_maximum_acceleration(self, maximum_acceleration):
+        self._maximum_acceleration = maximum_acceleration
 
-  def _UpdateVals(self, acceleration, delta_time):
-    self._output[0, 0] += (self._output[1, 0] * delta_time
-        + 0.5 * acceleration * delta_time * delta_time)
-    self._output[1, 0] += acceleration * delta_time
+    def set_maximum_velocity(self, maximum_velocity):
+        self._maximum_velocity = maximum_velocity
 
-  def _CalculateTimes(self, distance_to_target, goal_velocity):
-    if distance_to_target == 0:
-      self._acceleration_time = 0
-      self._acceleration = 0
-      self._constant_time = 0
-      self._deceleration_time = 0
-      self._deceleration = 0
-      return
-    elif distance_to_target < 0:
-      # Recurse with everything inverted.
-      self._output[1] *= -1
-      self._CalculateTimes(-distance_to_target, -goal_velocity)
-      self._output[1] *= -1
-      self._acceleration *= -1
-      self._deceleration *= -1
-      return
+    def _UpdateVals(self, acceleration, delta_time):
+        self._output[0, 0] += (self._output[1, 0] * delta_time +
+                               0.5 * acceleration * delta_time * delta_time)
+        self._output[1, 0] += acceleration * delta_time
 
-    self._constant_time = 0
-    self._acceleration = self._maximum_acceleration
-    maximum_acceleration_velocity = (
-        distance_to_target * 2 * numpy.abs(self._acceleration)
-        + self._output[1] * self._output[1])
-    if maximum_acceleration_velocity > 0:
-      maximum_acceleration_velocity = numpy.sqrt(maximum_acceleration_velocity)
-    else:
-      maximum_acceleration_velocity = -numpy.sqrt(-maximum_acceleration_velocity)
+    def _CalculateTimes(self, distance_to_target, goal_velocity):
+        if distance_to_target == 0:
+            self._acceleration_time = 0
+            self._acceleration = 0
+            self._constant_time = 0
+            self._deceleration_time = 0
+            self._deceleration = 0
+            return
+        elif distance_to_target < 0:
+            # Recurse with everything inverted.
+            self._output[1] *= -1
+            self._CalculateTimes(-distance_to_target, -goal_velocity)
+            self._output[1] *= -1
+            self._acceleration *= -1
+            self._deceleration *= -1
+            return
 
-    # Since we know what we'd have to do if we kept after it to decelerate, we
-    # know the sign of the acceleration.
-    if maximum_acceleration_velocity > goal_velocity:
-      self._deceleration = -self._maximum_acceleration
-    else:
-      self._deceleration = self._maximum_acceleration
+        self._constant_time = 0
+        self._acceleration = self._maximum_acceleration
+        maximum_acceleration_velocity = (
+            distance_to_target * 2 * numpy.abs(self._acceleration) +
+            self._output[1] * self._output[1])
+        if maximum_acceleration_velocity > 0:
+            maximum_acceleration_velocity = numpy.sqrt(
+                maximum_acceleration_velocity)
+        else:
+            maximum_acceleration_velocity = -numpy.sqrt(
+                -maximum_acceleration_velocity)
 
-    # We now know the top velocity we can get to.
-    top_velocity = numpy.sqrt((distance_to_target +
-                                (self._output[1] * self._output[1]) /
-                                (2.0 * self._acceleration) +
-                                (goal_velocity * goal_velocity) /
-                                (2.0 * self._deceleration)) /
-                               (-1.0 / (2.0 * self._deceleration) +
-                                1.0 / (2.0 * self._acceleration)))
+        # Since we know what we'd have to do if we kept after it to decelerate, we
+        # know the sign of the acceleration.
+        if maximum_acceleration_velocity > goal_velocity:
+            self._deceleration = -self._maximum_acceleration
+        else:
+            self._deceleration = self._maximum_acceleration
 
-    # If it can go too fast, we now know how long we get to accelerate for and
-    # how long to go at constant velocity.
-    if top_velocity > self._maximum_velocity:
-      self._acceleration_time = ((self._maximum_velocity - self._output[1]) /
-                                 self._maximum_acceleration)
-      self._constant_time = (distance_to_target +
-                        (goal_velocity * goal_velocity -
-                         self._maximum_velocity * self._maximum_velocity) /
-                        (2.0 * self._maximum_acceleration)) / self._maximum_velocity
-    else:
-      self._acceleration_time = (
-          (top_velocity - self._output[1]) / self._acceleration)
+        # We now know the top velocity we can get to.
+        top_velocity = numpy.sqrt(
+            (distance_to_target + (self._output[1] * self._output[1]) /
+             (2.0 * self._acceleration) + (goal_velocity * goal_velocity) /
+             (2.0 * self._deceleration)) / (-1.0 /
+                                            (2.0 * self._deceleration) + 1.0 /
+                                            (2.0 * self._acceleration)))
 
-    if self._output[1] > self._maximum_velocity:
-      self._constant_time = 0
-      self._acceleration_time = 0
+        # If it can go too fast, we now know how long we get to accelerate for and
+        # how long to go at constant velocity.
+        if top_velocity > self._maximum_velocity:
+            self._acceleration_time = (
+                (self._maximum_velocity - self._output[1]) /
+                self._maximum_acceleration)
+            self._constant_time = (
+                distance_to_target +
+                (goal_velocity * goal_velocity -
+                 self._maximum_velocity * self._maximum_velocity) /
+                (2.0 * self._maximum_acceleration)) / self._maximum_velocity
+        else:
+            self._acceleration_time = ((top_velocity - self._output[1]) /
+                                       self._acceleration)
 
-    self._deceleration_time = (
-        (goal_velocity - top_velocity) / self._deceleration)
+        if self._output[1] > self._maximum_velocity:
+            self._constant_time = 0
+            self._acceleration_time = 0
 
+        self._deceleration_time = ((goal_velocity - top_velocity) /
+                                   self._deceleration)