# Cartesian Constrained Stochastic Trajectory Optimization for Motion Planning
## 1 Introduction  
Currently, there are many path planners available that can solve the problem of path planning in a complicated configuration space of an industrial robotic arm. Commonly, the effective path planners that can fast-search any complicated configuration space are sampling-based, e.g., PRM [1], RRT [2] or their modifications (e.g., [3]), which even consider robot dynamics [4]. These algorithms are effective in searching for feasible paths, but they do not meet requirements of optimization principles. On the other hand, there are optimization-based algorithms (e.g., Covariant Hamiltonian Optimization for Motion Planning (CHOMP) [5] and Energy-Optimal Collision-Free Motion Planning [6]) that combine energy consumption and collision avoidance constraints in an optimization task. These algorithms do not use randomized sampling and are not guaranteed to find a path for each case because there is a risk the planner will be trapped in a local minimum. An interesting solution is introduced by the Stochastic Optimization Motion Planning (STOMP) [7], which combines optimization-based methods and randomized generation of noised trajectories. The best candidate is chosen from these noised trajectories by optimization of the cost function. The disadvantage of STOMP is that it works in robot joint space but some specific industrial applications need to find optimal solutions in cartesian space of robot TCP (tool center point). Imagine an industrial application in which a manipulated object is grasped by a vacuum or magnetic gripper. The grasp could have been weak, such as when using a two-finger gripper, and the object could have been dropped during the manipulation if the external forces are greater than the grasp force. This can occur with a specific tool-point orientation or position [8; 9; 10; 11]. Likewise, many other applicationsexist where a minimization of tool-point rotation is required, e.g., applications in human environment [12; 13; 14; 15], liquid handling [16], in milling and in the study of the cutter path [17] and others [18; 19]. The restrictions of specific tool-point orientations and positions on a robot trajectory are called cartesian path constraints. Therefore, a new requirement for cartesian path constraints is necessary in the path planner. Authors of the GSTOMP algorithm [20] tried to solve a similar problem. Their solution was based on a task-informed initialization of the STOMP algorithm by generation of initial trajectories using the Learning from Demonstration method [21], with their custom cost function aiming to minimize the distance between collision-free trajectory and initial trajectory. STOMP works in robot joint space, though the cartesian path constraints are not considered in the current version. However, STOMP is extendable because the implementation is based on plugins, which can be simply switched. Therefore, the idea of our designed solution is to use cartesian space instead of joint space in parts of the original algorithm, where it is required. The most potentially beneficial plugins could be the noise generator and cost function. In the cartesian space it is much easier to generate good trajectory candidates that satisfy cartesian constraints of the robot TCP. Likewise, the important part is the cost function, which can evaluate these generated trajectory candidates in respect of cartesian constraints. The STOMP version that uses these plugins is called cartesian constrained STOMP. In contrast to GSTOMP, our research provides a solution that does not require the task-informed initialization and is usable without any third-party algorithms. The main contribution of our research are extensions of STOMP that allow the usage of cartesian constraints. The new plugins, being the sampling generator and cost function, are briefly described in the Section 2. The Section 3 focuses on the description of our proposed implementation and solution for path planning under cartesian constraints. Our research is verified with experiments in the Section 4.