Add potential_energy function to util.

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