Pd controller combined with joint energy based controller