Angular velocity improperly calculated for TwoWheelRobot for proprioception dictionary

[sujaygarlanka]

The TwoWheelRobot seems to be incorrectly calculating the base angular velocity that is returned in the proprioception dictionary.

$\omega$ = angular velocity $\upsilon$ = linear velocity $V_r$ = right wheel velocity $V_l$ = left wheel velocity $R$ = wheel radius $l$ = wheel axle length

The incorrect formula can be found here and is

\omega=\frac{V_r-V_l}{l}

The equations to get wheel velocities from linear and angular velocities that are applied to a DD controller are here. These equations seem to be the source of truth that the proprioception calculation should match. The equations are the following:

V_l = \frac{\upsilon - \omega \cdot l/2}{R}

V_r = \frac{\upsilon + \omega \cdot l/2}{R}

Solving for $\omega$ and $\upsilon$ results in the following equations:

\omega = \frac{(V_r - V_l) \cdot R }{l}

\upsilon = \frac{(V_l + V_r) \cdot R}{2}

Ultimately, I think the incorrect angular velocity formula above needs to be updated to this $\omega = \frac{(V_r - V_l) \cdot R }{l}$

[cgokmen]

We'll review and include your PR in our next release coming out soon. Thanks!