Remove v and vd variables from WholeBodyPlanner

Instead, just formulate the appropriate quadratic costs on Q that are
equivalent to the quadratic costs and linear constraints I had before.
Increase tolerance in F vs m*comddot check