Improved screw theory using second order terms

The local displacement of an object is very useful for deciding grasp stability, generating trajectories, recognizing assembly tasks, and so on. To calculate this displacement, the screw theory is employed. It is equivalent to the first order Taylor expansion of the displacement. The screw theory is very convenient, because the displacement is formulated as simultaneous linear inequalities, and a powerful tool to calculate such inequalities, the theory of the polyhedral convex cones, has already been established. However, truncation errors introduced by first order approximations sometimes cause mistaken results. In this paper, we improve the screw theory by using 2nd order terms and verify the validity of the result.