Move trapezoid_profile.cc to Python
Change-Id: Ifac28aaabb24a668b04371edbeb58717c49c8925
diff --git a/aos/common/util/trapezoid_profile.py b/aos/common/util/trapezoid_profile.py
new file mode 100644
index 0000000..f956bf0
--- /dev/null
+++ b/aos/common/util/trapezoid_profile.py
@@ -0,0 +1,156 @@
+#!/usr/bin/python
+
+import numpy
+
+class TrapezoidProfile(object):
+ """Computes a trapezoidal motion profile
+
+ Attributes:
+ _acceleration_time: the amount of time the robot will travel at the
+ specified acceleration (s)
+ _acceleration: the acceleration the robot will use to get to the target
+ (unit/s^2)
+ _constant_time: amount of time to travel at a constant velocity to reach
+ target (s)
+ _deceleration_time: amount of time to decelerate (at specified
+ deceleration) to target (s)
+ _deceleration: decceleration the robot needs to get to goal velocity
+ (units/s^2)
+ _maximum_acceleration: the maximum acceleration (units/s^2)
+ _maximum_velocity: the maximum velocity (unit/s)
+ _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.
+
+ 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._maximum_acceleration = 0
+ self._maximum_velocity = 0
+ self._timestep = delta_time
+
+ 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)
+
+ 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)
+ else:
+ self._UpdateVals(self._deceleration, self._deceleration_time)
+ next_timestep -= self._deceleration_time
+ self._UpdateVals(0, next_timestep)
+
+ return self._output
+
+ # Useful for preventing windup etc.
+ def MoveCurrentState(self, current):
+ self._output = current
+
+ # Useful for preventing windup etc.
+ def MoveGoal(self, dx):
+ self._output[0] += dx
+
+ def SetGoal(self, x):
+ self._output[0] = x
+
+ def set_maximum_acceleration(self, maximum_acceleration):
+ self._maximum_acceleration = maximum_acceleration
+
+ def set_maximum_velocity(self, maximum_velocity):
+ self._maximum_velocity = maximum_velocity
+
+ 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 _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
+
+ 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)
+
+ # 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
+
+ # 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 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)
+
+ if self._output[1] > self._maximum_velocity:
+ self._constant_time = 0
+ self._acceleration_time = 0
+
+ self._deceleration_time = (
+ (goal_velocity - top_velocity) / self._deceleration)
+