Made the 2 link snake work. It was drifting b/c the angular velocity (u1) was not zero, so just pick q1(0) such that u1(0) = 0 and it osciallates back and forth. Still need to check how well it is doing that, but the movie looks promising.