fix debug-simulation function, simulate motion trnasition with torque input