# Cartesian Constrained Stochastic Trajectory Optimization for Motion Planning
## 6 Conclusions  
The STOMP algorithm is an optimization-based path planner that uses random sampling of noised trajectories. The algorithm works in robot joint space and, therefore, it cannot consider any cartesian path constraints. The cartesian constraints are often required in many industrial applications. Our research was focused on extension of the STOMP algorithm, which will be able to do path planning under cartesian constraints. We designed and implemented the new Cartesian Normal Distribution Sampling Generator and the Cartesian Distance Cost Function, which work in cartesian space of a robot TCP to satisfy cartesian constraints. This implementation is available in supplementary materials [[https://github.com/photoneo/stomp_ros/tree/MichalD/cartesian_constraints_sampling](https://github.com/photoneo/stomp_ros/tree/MichalD/cartesian_constraints_sampling)]([https://github.com/photoneo/stomp_ros/tree/MichalD/cartesian_constraints_sampling](https://github.com/photoneo/stomp_ros/tree/MichalD/cartesian_constraints_sampling)) (accessed on 5 November 2021) or online at [26]. Original noise generators in STOMP generate trajectory rollouts in robot joint space. Our Cartesian Normal Distribution Sampling Generator generates tool-point positions in cartesian space. The trajectory rollout, which is generated under cartesian constraints, could be a better candidate as a generated rollout in joint space without consideration of any constraints. Due to STOMP being an optimization-based path planner, we defined the new Cartesian Distance Cost Function which is able to validate each trajectory rollout. This cost function tries to choose rollouts that are the most similar to a linear trajectory of tool-point in cartesian space. In our simple experiments, STOMP tried to find collision-free trajectory under cartesian constraints between the same start state and the same goal state. The first experiment proved that the cartesian constrained STOMP satisfied cartesian path constraints in each resulting trajectory. The second experiment compared configurations of original and newly created plugins of STOMP with respect to the computation time, accumulated sum of tool-point orientation changes and cartesian distances between waypoints. The experiment showed that the accumulated sum of tool-point orientation was minimized using the Cartesian Normal Distribution Sampling Generator and the Cartesian Distance Cost Function. On the other hand, the computation time was increased using Cartesian Distance Cost Function. However, this increase was expected because the planning problem under cartesian constraints is more difficult than without constraints. Our future work will focus on reducing computation time. It is possible to improve Cartesian Normal Distribution Sampling, which will generate better candidates and a STOMP algorithm capable of finding trajectory after less iterations. The other goal will be to evaluate STOMP plugin extensions on a real robot or a real industrial application. The following are available online at [[https://github.com/photoneo/stomp_ros/tree/[PERSON]/cartesian_constraints_sampling](https://github.com/photoneo/stomp_ros/tree/[PERSON]/cartesian_constraints_sampling)]([https://github.com/photoneo/stomp_ros/tree/[PERSON]/cartesian_constraints_sampling](https://github.com/photoneo/stomp_ros/tree/[PERSON]/cartesian_constraints_sampling)) (accessed on 5 November 2021). Conceptualization, P.B. and M.D. ([PERSON]); methodology, P.B. and M.D. ([PERSON]); software, M.D. ([PERSON]); validation, M.D. ([PERSON]) and A.S.; formal analysis, A.S.; investigation, M.D. ([PERSON]); resources, P.B. ; data curation, M.D. ([PERSON]); software, M.  
bijs); writing--original draft preparation, M.D. ([PERSON]); writing--review and editing, M.D. ([PERSON]) and A.S.; visualization, M.D. ([PERSON]); supervision, P.B. ; project administration, P.B. ; funding acquisition, F.D. All authors have read and agreed to the published version of the manuscript.