На первых порах, думаю не стоит так сильно усложнять задачу, достаточно сделать хотя бы прямые зависимости от углов, а не от перегрузки. Ну а по существу вопроса, скажите насколько реально такое организовать?
Думаю даже сначала имеет смысл отработать управление электроприводом, а потом уже сооружать уже эту страшенную кинематику осей/приводов...