Added reset of joints. CLeaned some code. Deleted set PID = 0 during send messages. Still problem with ITorqueControl.