fixed error in qdotdot calculation, added all 7 arm links properly, changed timestep for integration