# Cartesian Constrained Stochastic Trajectory Optimization for Motion Planning
## 4 Results
### Comparison of Noise Generators and Cost Functions  
The goal of the second experiment was to compare configurations of original and cartesian constrained STOMP. The experiment was be executed in four different configurations, where Cartesian and Joint Normal Distribution Sampling Generator were switched. Likewise, Cartesian Distance Cost Function was activated in two configurations and deactivated  
Figure 7: Histograms of differences between waypoints of cartesian linear trajectory and waypoints of resulting trajectories: (**a**) Translation distance; (**b**) Rotation angle. in two configurations. Each configuration had a similar dataset as the first experiment, but there were 1000 computed resulting trajectories. The configurations were as follows:  
1. Joint Normal Distribution Sampling Generator without Cartesian Distance Cost Function (green bars in Figure 8)--this represented the original STOMP and it was not expected that cartesian constraints would be satisfied. 2. Joint Normal Distribution Sampling Generator with Cartesian Distance Cost Function (red bars in Figure 8)--this represented the original STOMP, which was extended with Cartesian Distance Cost Function. It was expected that cartesian constraints would be satisfied. 3. Cartesian Normal Distribution Sampling Generator without Cartesian Distance Cost Function (blue bars in Figure 8)--this replaced the original noise generator in joint space with the noise generator in the cartesian space, but there was still a risk that cartesian constraints would not be satisfied. 4. Cartesian Normal Distribution Sampling Generator with Cartesian Distance Cost Function (orange bars in Figure 8)--the combinations of both newly implemented plugins. It used the noise generator in the cartesian space with Cartesian Distance Cost Function and cartesian constraints that should be satisfied. For the cartesian noise generator, the amplitudes of noise were (0.2, 0.2, 0.5, 0.05, 0.05, 0.05), while for the Normal Distribution Sampling Generator, the amplitudes were (0.04, 0.08, 0.1, 0.07, 0.1, 0.10). Cartesian distance cost function used a rotation tolerance of 0.4 rad and translation tolerance of 2.5 m. The cost weight was the same as the weight of the CollisionCheck cost function. This configuration of noise and tolerances was different to the first experiment, 4.1 Cartesian Constraints Verification, where the configuration was  
Figure 8: Histograms of scores: (**a**) Scores by (**4**); (**b**) Computation time; (**c**) Scores by Equation (5) where the full range of scores is shown; (**d**) Detail of the previous histogram, where it used a range of 0–0.8. relatively extreme. There was a risk that generated rollouts were often out of tolerances, which increased computation time unnecessarily. The goal of the second experiment was to test functionality in situations that were close to real robotic applications; therefore, noise amplitudes were lower and tolerances were increased. The comparison focused on four criteria, which consisted of computation time, the accumulated sum of cartesian distances of tool-points between waypoints (4) and the accumulated differences of tool-points orientations between waypoints (5) [25]. \[x=\sum_{i=2}^{n}\|p_{i}-p_{i-1}\| \tag{4}\]  
where \(p_{i}\) is robot tool-point position, \(i\) is the waypoint id of \(n\) waypoints and the score \(x\) is assigned to each trajectory. \[x=\sum_{i=2}^{n}\cos^{-1}\big{(}q_{xi}*~{}q_{xi-1}+q_{yi}*~{}q_{yi-1}+q_{zi}*~ {}q_{zi-1}+q_{wi}*~{}q_{wi-1}\big{)} \tag{5}\]  
where \(q_{x}\), \(q_{y}\), \(q_{z}\) and \(q_{w}\) are values of tool-point quaternion of \(i\) waypoint and the score \(x\) is assigned to each trajectory. Table 2 summarizes the averages and variances of evaluation criteria scores, with the histograms in Figure 8 providing a results comparison. The evaluation of tool-point orientation shows that the Normal Distribution Sampling Generator, which works only in robot joint space, cannot satisfy the orientation constraints as was expected.