Cartesian Constrained Stochastic Trajectory Optimization for Motion Planning

Round 1

Reviewer 1 Report

In this paper the authors have presented some modifications of the Stochastic Optimization Motion Planning algorithm, which considers path cartesian constraints. The approach suggested seems novel which is supported by results. However, some changes are necessary in the paper. The authors are suggested to make the necessary changes/updates to their paper as per the following comments:

1. In Section 4 – the procedure for trajectory generation and calculation of multiple trajectories is not clear.
2. What is the significance behind the selection of 15 trajectory rollouts?
3. Details on data preprocessing, filtering, and outlier detection and elimination on the dataset is missing. These details should be included in Section 4.1 or before Section 4.1.
4. The dataset for the second experiment consists of 4 combinations of methods. Please elaborate the relevance of selecting these 4 methods specifically.
5. Discussion of results is missing. This is not the first time a research work has been proposed to modify the STOMP algorithm. It is suggested that the authors compare (both qualitatively and quantitatively) the modifications proposed in this paper with the modifications proposed in prior works to outline the relevance of their approach.
6. Very old references: The papers cited by the authors to discuss potential applications of their work are very old. For instance, to discuss the application of this approach in human environments, the authors have cited a paper (reference 10) that was published 14 years ago. It is suggested that instead/in addition to reference 10 – the authors cite this recent paper - https://www.mdpi.com/2078-2489/12/2/81 and similarly they cite other recent papers to discuss the applications of their work in different domains.

Author Response

Dear Madam/Sir

Thank you for your review and your opinion help us to improve the paper.

1. In Section 4 – the procedure for trajectory generation and calculation of multiple trajectories is not clear.

You have right, the section 4 was not absolutely clearly described. The term “generated trajectory” misleads to confusions. This term described rollout, which was generated by Noise Generator. On other hand, this term was used also for resulting trajectory, which represent an output from algorithm. The dataset of multiple trajectories contains only resulting trajectories. Therefore, the introduction of section 4 has been rewritten.

2. What is the significance behind the selection of 15 trajectory rollouts?

Thank you for note, it was not explained in the text. It’s based on our experiences. A value smaller than what was used can cause that the solution to be never found, and on the other hand a higher value can slow down the iteration. Now it’s described also in the text

3. Details on data preprocessing, filtering, and outlier detection and elimination on the dataset is missing. These details should be included in Section 4.1 or before Section 4.1.

We are glad about your interest in details. We think that you mean outliers in set of generated rollouts, which are filtered by cost function. But this is not a scope of this experiment. The experiment is focused on resulting trajectories. Desired tolerances have been satisfied for each resulting trajectory and experiment is consider as successful. The dataset of resulting trajectories does not contain outliers needed for filtering. This misunderstanding could be caused by weakly described introduction of section 4, which is rewritten now. (The similar misunderstanding as in point 1.)

4. The dataset for the second experiment consists of 4 combinations of methods. Please elaborate the relevance of selecting these 4 methods specifically.

Thank you for your suggestion, now each combination is described, and the selection is specified.

5. Discussion of results is missing. This is not the first time a research work has been proposed to modify the STOMP algorithm. It is suggested that the authors compare (both qualitatively and quantitatively) the modifications proposed in this paper with the modifications proposed in prior works to outline the relevance of their approach.

Thank you for your note, we wrote only conclusions and discussion was missing. The discussion has been inserted to manuscript. Our goal was to extend STOMP algorithm and our results show that the implementation of plugins satisfies requirements. We consider this as a relatively difficult and appropriate for publication and our goal is to write a simple letter, which describes our implemented plugins. The comparison with different algorithms we would like to provide in our future. This idea is involved in discussion.

6. Very old references: The papers cited by the authors to discuss potential applications of their work are very old. For instance, to discuss the application of this approach in human environments, the authors have cited a paper (reference 10) that was published 14 years ago. It is suggested that instead/in addition to reference 10 – the authors cite this recent paper - https://www.mdpi.com/2078-2489/12/2/81 and similarly they cite other recent papers to discuss the applications of their work in different domains.

We appreciate your review of old references. Your suggested reference and other similar references have been inserted to manuscript.

Author Response File: Author Response.pdf

Reviewer 2 Report

This paper presents a Cartesian constrained stochastic trajectory optimization for robot arm motion planning in environments with obstacles. This is a modification of existing algorithm STOMP to particularly address the Cartesian constraints problem. In evaluating of the quality and novelty of this work, I do not recommend the acceptance of this work. The specific comments are as follows.

1. As mentioned by the authors that the motivation to conduct this research is to address the Cartesian constraint problem, which cannot be solved by existing STOMP. Actually, this claim is incorrect. The STOMP model provides a framework for motion planning in high dimensional space with two penalty terms in the objective function, i.e., the obstacle avoidance penalty and the trajectory penalty. Usually, the Cartesian constraint, e.g., the end-effector trajectory length in Cartesian space, the smoothness of the end effector trajectory in Cartesian space, can be directly considered in the objective function. Also, the authors only explained the advantages compared with GSTOMP, but did not explain the contribution of this work. Therefore, the authors are suggested to clarify the contribution and novelty of this work.
2. The authors are suggested to take a comprehensive comparison with existing stochastic motion planning solutions, e.g., RPM (Random probabilistic map), RRT (Rapidly-explored random tree), CHOMP (Covariant Hamiltonian optimization for motion planning) and their variants. The authors are suggested to summarize the experimental comparison in a table, including results on time complexity (mean, and variance), planned trajectory length (mean and variance), distance from nearest obstacles (mean and variance) and failure rate. The authors may also evaluate the performance in extreme experimental setups. With the increase of obstacles, it is becoming gradually difficult to find an optimal motion trajectory timely. The authors may therefore compare the performance in this situation. 
3. In this paper, the authors only considered simple situations with super sparse obstacles. I understand that in most real situations there are not so many obstacles. However, as a mature algorithm, its completeness is also important. Could you please therefore consider complex environments by introducing more obstacles?
4. The authors are suggested to provide the pseudo-code of the proposed algorithm in the paper so that practitioners can directly use this research result without the necessity to understand the technical details if it is effective.
5. Some references, e.g., [2][3], are relatively old. The following paper seems to be related and should be discussed: " A Mode-Switching Motion Control System for Reactive Interaction and Surface Following Using Industrial Robots," "Energy-Optimal Collision-Free Motion Planning for Multiaxis Motion Systems: An Alternating Quadratic Programming Approach,” "An Efficient RRT-Based Framework for Planning Short and Smooth Wheeled Robot Motion Under Kinodynamic Constraints" and “Monocular Visual-Inertial and Robotic-Arm Calibration in a Unifying Framework”.
6. The results cannot be seen in Fig. 6, and the comparison of specific data should be given.
7. In “4.1. Cartesian constraints verification”, the translation tolerance is set to 1.0 m and rotation tolerance to 0.2 rad. In “4.2. Comparison of noise generators and cost functions”, the translation tolerance is set to 2.5 m and rotation tolerance to 0.4 rad. How are these determined? STOMP calculation speed is fast, but the effect is slightly worse. It may also be acceptable.
8. There is no comparison in “4.1. Cartesian constraints verification”, which should be synchronized with “4.2. Comparison of noise generators and cost functions”.
9. There are many problems with the format of figures. Figs. 1 and 2 are not very clear, Figs. 3, 4 and 5 are not centered, and Figs. 6, 7 and 8 exceed the boundary.
10. There are some presentation mistakes:

Many symbols do not have italics, such as x, y, z in Page 4 line 145; n in Page 6 line 183; i and x in Page 8 line 226 and 228.

Page 8 line 246, there is an extra period.

The data in Table.2 is not centered.

Page 7 line 225, the reference number should precede the period.

The authors should check the grammatical errors and typos of the paper carefully.

Author Response

Dear Madam/Sir

Thank you for your review and your opinion help us to improve the paper.

1. As mentioned by the authors that the motivation to conduct this research is to address the Cartesian constraint problem, which cannot be solved by existing STOMP. Actually, this claim is incorrect. The STOMP model provides a framework for motion planning in high dimensional space with two penalty terms in the objective function, i.e., the obstacle avoidance penalty and the trajectory penalty. Usually, the Cartesian constraint, e.g., the end-effector trajectory length in Cartesian space, the smoothness of the end effector trajectory in Cartesian space, can be directly considered in the objective function. Also, the authors only explained the advantages compared with GSTOMP, but did not explain the contribution of this work. Therefore, the authors are suggested to clarify the contribution and novelty of this work.

We appreciate your opinion and experiences about STOMP algorithm. You have the right that the STOMP model provides objective function. This function is called as “cost function” in our paper and in STOMP documentation. The current version of STOMP provides Collision Check, Obstacle Distance Gradient and Tool Goal Pose cost functions, but these cost functions does not consider cartesian path constraints. Your note “Usually, the Cartesian constraint, e.g., the end-effector trajectory length in Cartesian space, the smoothness of the end effector trajectory in Cartesian space, can be directly considered in the objective function” is correct and we are implemented Cartesian Distance as the cost function plugin. Therefore, we can not write about modification of STOMP, but about its extensions. The introduction and other parts of article has been updated in depend on your opinion. Also, the contribution of this work could be highlighted and explained in introduction.

 

2. The authors are suggested to take a comprehensive comparison with existing stochastic motion planning solutions, e.g., RPM (Random probabilistic map), RRT (Rapidly-explored random tree), CHOMP (Covariant Hamiltonian optimization for motion planning) and their variants. The authors are suggested to summarize the experimental comparison in a table, including results on time complexity (mean, and variance), planned trajectory length (mean and variance), distance from nearest obstacles (mean and variance) and failure rate. The authors may also evaluate the performance in extreme experimental setups. With the increase of obstacles, it is becoming gradually difficult to find an optimal motion trajectory timely. The authors may therefore compare the performance in this situation.

Thank you for your suggestion and we would like to provide comprehensive comparison with other planning solutions. But our goal was to improve STOMP and provides possibility of planning under cartesian path constraints. We consider these extensions of STOMP as a relatively difficult and appropriate for publication and therefore we prepared a simple letter, which describes our implemented plugins. The comparison with different algorithms we would like to provide in our future work. This idea is involved in discussion. Also we have prepared a framework for complex comparison of resulting trajectory, which we would like to use for this comparison, but we are waiting for review of publication of this tool.

3. In this paper, the authors only considered simple situations with super sparse obstacles. I understand that in most real situations there are not so many obstacles. However, as a mature algorithm, its completeness is also important. Could you please therefore consider complex environments by introducing more obstacles?

We are glad about your interest in complex environment testing. Extensions of STOMP were tested in a environment that is comparable to common industrial applications such as pick and place, or binpicking. The results were compared with the standard STOMP algorithm. Both experiments show that these modifications lead to desirable results.  But it should be noted, that in complex environments where the STOMP algorithm doesn't find a suitable trajectory with 100% success rate, our extensions lower the success rate even further. These notes are involved in discussion section.

4. The authors are suggested to provide the pseudo-code of the proposed algorithm in the paper so that practitioners can directly use this research result without the necessity to understand the technical details if it is effective.

Thank you for your interest in pseudo-code. This is not a problem to add it, but a link to github is available in supplementary materials. The whole solution is an open source and after acceptation of this publication we would like to create a pull request to main repository of STOMP. Practitioners do not need to read pseudo-code or understand the technical details, they could directly use our source code.

5. Some references, e.g., [2][3], are relatively old. The following paper seems to be related and should be discussed: " A Mode-Switching Motion Control System for Reactive Interaction and Surface Following Using Industrial Robots," "Energy-Optimal Collision-Free Motion Planning for Multiaxis Motion Systems: An Alternating Quadratic Programming Approach,” "An Efficient RRT-Based Framework for Planning Short and Smooth Wheeled Robot Motion Under Kinodynamic Constraints" and “Monocular Visual-Inertial and Robotic-Arm Calibration in a Unifying Framework”.

We appreciate your effort to check references. Some references describe the basic version of mentioned algorithms, therefore it relatively old. But we are glad about your suggestion of reference and following references has been inserted in the paper: "Energy-Optimal Collision-Free Motion Planning for Multiaxis Motion Systems: An Alternating Quadratic Programming Approach,” and "An Efficient RRT-Based Framework for Planning Short and Smooth Wheeled Robot Motion Under Kinodynamic Constraints".

6. The results cannot be seen in Fig. 6, and the comparison of specific data should be given.

We are glad about your advice, but the fig. 6 is just illustration, where it is illustrated example one trajectory from original STOMP and one trajectory from cartesian constrained STOMP. The of comparison of specific data is provided in section “4.1 Cartesian Constraints Verifications” and “4.2 Comparison of Noise Generators and Cost Functions”.

7. In “4.1. Cartesian constraints verification”, the translation tolerance is set to 1.0 m and rotation tolerance to 0.2 rad. In “4.2. Comparison of noise generators and cost functions”, the translation tolerance is set to 2.5 m and rotation tolerance to 0.4 rad. How are these determined? STOMP calculation speed is fast, but the effect is slightly worse. It may also be acceptable.

Thank you for your question about these constants, which let us to improve this section, which was weakly described. The reason was that the configuration in section “4.1. Cartesian Constraints Verification” has rather disproportional sampling amplitude compared to the tolerance as it is able to generate a configuration outside the desired tolerance in single step. This was selected to ensure that the constrains work even under such harsh conditions. On other hand, the goal of the second experiment “4.2 Comparison of Noise Generators and Cost Functions” is to test functionality in situations, that are close to real robotic applications, therefore noise amplitudes are lower and tolerances are increased. Now it is better described in the paper.

8. There is no comparison in “4.1. Cartesian constraints verification”, which should be synchronized with “4.2. Comparison of noise generators and cost functions”.

Thank you for your note. Comparison in these sections could not be synchronized. This misunderstanding was caused by weakly described our experiments. This is the same problem as in your 7. Now it is updated.

9. There are many problems with the format of figures. Figs. 1 and 2 are not very clear, Figs. 3, 4 and 5 are not centered, and Figs. 6, 7 and 8 exceed the boundary.

We are glad about your effort to check figures. Figs. 1 and 2 are replaced by figures in higher resolution. But other figures we can not update, because we was trying to keep the template for journal, which is available in https://www.mdpi.com/files/word-templates/applsci-template.dot and in the template figures are not centered and groups of pictures could exceed the boundary.

10. There are some presentation mistakes:

Many symbols do not have italics, such as x, y, z in Page 4 line 145; n in Page 6 line 183; i and x in Page 8 line 226 and 228.

Page 8 line 246, there is an extra period.

The data in Table.2 is not centered.

Page 7 line 225, the reference number should precede the period.

The authors should check the grammatical errors and typos of the paper carefully.

Thank you for your check of presentation mistakes. These mistakes have been removed and we make a check of other grammatical errors and typos.

Author Response File: Author Response.pdf

Reviewer 3 Report

This paper focused on the issue of novel modifications of the stochastic optimization motion planning. It proposed a novel solution,  which modifies the most important parts of the algorithm to take into account cartesian constraints. It is compared with the original STOMP. This research work is interesting for the path planning research, and the experimental results are discussed suitably. However, this paper has several limitations and the standard is not enough, and address the following items would result in a good paper,

1. The literature review is not thorough about the application and the control contributions. To highlight the contributions, it suggests reorganizing the section of the related work. At least, for each contribution, it should be novel and meaningful according to a thorough literature review. In the literature analysis, it is recommended to read the following works and consider their similar applications in the introduction and future works. For example, Improved human-robot collaborative control of redundant robot for teleoperated minimally invasive surgery; Multi-sensor Guided Hand Gestures Recognition for Teleoperated Robot using Recurrent Neural Network; A Smartphone-Based Adaptive Recognition and Real-Time Monitoring System for Human Activities; it could make the proposed method much more meaningful.
2. To illustrate the advanced performance of the proposed method, what is the main task to be solved in this paper? It is recommended to present in the first section so that it can highlight the specific scope of this article.
3. It is suggested to add more details to make fig. 5 clear. In particular, there could be some kinematic details to make it reasonable.
4. There should be a further discussion about the limitation of the current works, in particular, what could be the challenge for its related applications.
5. To let readers better understand future work, please give specific research directions.

Author Response

Dear Madam/Sir

Thank you for your review and your opinion help us to improve the paper.

1. The literature review is not thorough about the application and the control contributions. To highlight the contributions, it suggests reorganizing the section of the related work. At least, for each contribution, it should be novel and meaningful according to a thorough literature review. In the literature analysis, it is recommended to read the following works and consider their similar applications in the introduction and future works. For example, Improved human-robot collaborative control of redundant robot for teleoperated minimally invasive surgery; Multi-sensor Guided Hand Gestures Recognition for Teleoperated Robot using Recurrent Neural Network; A Smartphone-Based Adaptive Recognition and Real-Time Monitoring System for Human Activities; it could make the proposed method much more meaningful.

Thank you for your suggestions and recommended literature. The introduction has been rewritten and some your suggested references has been inserted.

2. To illustrate the advanced performance of the proposed method, what is the main task to be solved in this paper? It is recommended to present in the first section so that it can highlight the specific scope of this article.

We appreciate your review of introduction and the main task idea. The goal of the paper is to describe implementation of extension of STOMP (new noise generator and cost function, which is implemented as plugin) and the STOMP could be able to compute trajectory, which satisfy cartesian constraints. The introduction has been rewritten and the main task and contribution could be more highlighted.

3. It is suggested to add more details to make fig. 5 clear. In particular, there could be some kinematic details to make it reasonable.

We are glad for your suggestion about addition of more details to fig. 5, which illustrated only minimization of translation. We added coordinate systems to each point of rollouts (green) and ideal linear trajectory (yellow).

4. There should be a further discussion about the limitation of the current works, in particular, what could be the challenge for its related applications.

We appreciate your note, we wrote only conclusions and discussion was missing. The discussion has been inserted to manuscript, where limitation of the current works is discussed.

5. To let readers better understand future work, please give specific research directions.

Thank you for your advice. Your advice is included in discussion, where specific research directions of future work are described.

Author Response File: Author Response.pdf

Round 2

Reviewer 1 Report

The authors have made significant updates to their paper as per all my comments. I do not have any additional comments at this point, I recommend the publication of this paper in its current form. 

Reviewer 3 Report

The authors have addressed all of my concerns. No more revision is required from me. The current version can be accepted now.