Hey @Geronimo,
We have had a discussion some time ago about adding feedforward inputs to the simplefoc.
I think it would be a good idea, it would enable you to use the external planning algorihtm without the need to rewrite the whole control part of the library.
Maybe we will attack this problem as well and make a SimpleFOC motion planner. I am not sure at the moment. Probably trapezoidal velocity profile based approach.
I’ve seqrched quicky and it seems that there is an arduino library that might do the trick:
https://github.com/joshua-8/Derivs_Limiter
If you wanna test it and if you get some good results, please do let me know. If it works well, we might do a collaboration with them maybe. 
Cheers,
Antun