Modifiy MotorModel in TorqueController 

Updated motor model in TorqueController. I expect this modification have no influence on other rtcs.
- Change motor model in TorqueController from P Model to PD Model (TwoDofControllerPDModel).
- TorqueController continuously support old P Model controller. Therse are descriminated by number of initial arguments of MotorTorqueController.
- Modify calculating algorithm of integration (simple summation -> trapezonal rule)
- Easy parameter tuning based on measured torque and joint angles.