Fix a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint.

If certain inputs were passed to trajectory planner, an expression inside the trajectory planner which is an argument to sqrtf() could become negative due to finite floating point accuracy.
This led to Vr_ == NaN and then Tf_ == 0, causing the trajectory to jump to the final setpoint instantaneously. To the user, this manifested as a sudden increase in velocity (limited by controller.config.vel_limit) and/or an overcurrent fault.

This bug was likely to show up when constantly sending trajectory setpoints while moving in the negative direction.

See also: https://discourse.odriverobotics.com/t/move-to-pos-not-works-well/4626

CHANGELOG.md

@@ -38,6 +38,7 @@ Please add a note of your changes below this heading if you make a Pull Request.
 * Fix IPython `RuntimeWarning` that would occur every time `odrivetool` was started.
 * Reboot on `erase_configuration()`. This avoids unexpected behavior of a subsequent `save_configuration()` call, since the configuration is only erased from NVM, not from RAM.
 * Change `motor.get_inverter_temp()` to use a property which was already being sampled at `motor.inverter_temp`
+* Fixed a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint
 
 # Releases
 ## [0.4.11] - 2019-07-25

Firmware/MotorControl/trapTraj.cpp

@@ -44,7 +44,7 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
     // Are we displacing enough to reach cruising speed?
     if (s*dX < s*dXmin) {
         // Short move (triangle profile)
-        Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
+        Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
         Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
         Td_ = std::max(0.0f, -Vr_ / Dr_);
         Tv_ = 0.0f;