removed the PID derivative part and added a damping in the joint