Correction needed for the calculation in SimpleTrajectoryPlanner.set_trajectory() function

[marvin-oh]

It seems like there is a missing term for gravity equation while calculating the y-axis position of the parabolic equation.

Currently, the code calculates the y-axis position as follows:

# work out initial velocities and coefficients of the parabola
self._ux = self._velocity * cos(self._theta)
self._uy = self._velocity * sin(self._theta)
self._a = -0.5 / (self._ux * self._ux)
self._b = self._uy / self._ux

# work out points of the trajectory
for x in range(0, self.X_MAX):
    xn = x / self._scale
    y = self._ref.Y - (int)((self._a * xn * xn + self._b * xn) * self._scale)
    self._trajectory.append(Point2D(x + self._ref.X, y))

The correct equation is following:

$$ y_t = y_0 - \frac{1}{2} \times g \times t^2 + v_0 \times \sin(\theta) \times t $$

Therefore, the code should be modified as follows:

# gravity
g = 0.48 * 9.81 / self.scale_factor

# coefficients of the parabola
self._a = -0.5 / (self._ux * self._ux) * g