TODO :

• add support for L shaped servo arm.
• optimize.
• use different plotting lib
• translate matlab code to python, to use Y-up axis def
• add odrive support