Cartesian Constrained Stochastic Trajectory Optimization for Motion Planning

This paper presents novel extensions of the Stochastic Optimization Motion Planning (STOMP), which considers cartesian path constraints. It potentially has high usage in many autonomous applications with robotic arms, where preservation or minimization of tool-point rotation is required. The original STOMP algorithm is unable to use the cartesian path constraints in a trajectory generation because it works only in robot joint space. Therefore, the designed solution, described in this paper, extends the most important parts of the algorithm to take into account cartesian constraints. The new sampling noise generator generates trajectory samples in cartesian space, while the new cost function evaluates them and minimizes traversed distance and rotation change of the tool-point in the resulting trajectory. These improvements are verified with simple experiments and the solution is compared with the original STOMP. Results of the experiments show that the implementation satisfies the cartesian constraints requirements.