Corrected errors in controller. Gravity compensation is computed with dq = 0 and ddq = 0. CHanged sign of damping term.