In a geometric mechanics framework, the configuration space is decomposed into a shape space and a position space. The internal motion of the system is prescribed by a closed loop in the shape space, which causes net motion in the position space. If the shape space is a simply connected domain in an Euclidean space, then with an optimal choice of the body frame, the displacement in the position space is reasonably approximated by the surface integral of the height function, a functional relationship between the internal shape and position space variables. Our recent work has extended the scope of geometric methods from limbless undulatory system to those with legs; interestingly, the shape space for such systems has a torus structure. However, to the best of our knowledge, the optimal choice of the body frame on the torus shape space was not explored. In this paper, we develop a method to optimally choose the body frame on the torus which results in good approximation of displacement by the integral of the height function. We apply our methods to the centipede locomotion system and observe quantitative agreement of our prediction and experimental results.