Add potential_energy function to util.
Add potential_energy function to util.

This function follows the algorithm in Section 5.1 of Kane 1985.

jcrist/pydy,jcrist/pydy,jcrist/pydy,Shekharrajak/pydy,skidzo/pydy,jcrist/pydy,skidzo/pydy,skidzo/pydy,skidzo/pydy,Shekharrajak/pydy,Shekharrajak/pydy,oliverlee/pydy,oliverlee/pydy,jcrist/pydy,jcrist/pydy,Shekharrajak/pydy,jcrist/pydy,oliverlee/pydy