x0 = -1.5; xdot0 = 0
q10 = 0; q1dot0 = 0 #0=vertical, pi=inverted
q20 = 0; q2dot0 = 0 #0=vertical, pi=inverted
xf = 1.5; xdotf = 0