Changed _T_controller from 10 to 1. Checked PositionCtrlDirect, PositionMove (using RobotMotorGui) and torque control in COMAN simulation and it works.