Sorry @juju I still haven’t find time to check your problem in details.
Yet,
Yes! This a very important point! You can not force the run loop (or any other loops BTW) to really run at 50Hz except if you are running a real time OS.
Thus, if you want to do something time dependant, you should follow the examples we have used for the sinus primitive. The idea is not to suppose your update loop will be called every 20ms but to directly get the current time from the system. BTW, this is also the only way you will be able to have the same code running on the simulator (where times is not the same than in reality) and on the real robot:
pos = self._amp * numpy.sin(self._freq * 2.0 * numpy.pi * self.elapsed_time + self._phase * numpy.pi / 180.0) + self._offset
Here self.elapsed_time will always get you the current time on your system (real or simulated). The precision of the time will depend on your system.