DMP online execution fix

Problem description

The existing implementation of the online execution was unrolling the DMP (i.e. calling the dmp.step function) at every step of the control loop, without checking if the intermediate goal has been reached. For this reason, the robot was very quickly just starting to move directly towards the goal, thus not following the trajectory at all.

The issue was first noticed during the development for https://github.com/b-it-bots/mas_domestic_robotics/pull/246, but directly reported by @sthoduka, who noticed that the robot was just moving in a straight line towards the goal rather than according to the primitive.

Summary of changes

In the fixed version, a DMP step is only taken if the currently pursued goal has been reached, which means that the trajectory is actually followed.

Tests to verify the changes

[x] The changes have been tested on the HSR (I hope to do that this afternoon)

cc @AhmedFaisal95

b-it-bots / mas_domestic_robotics