# Goal state driven trajectory optimization

Article

## Abstract

Many applications demand a dynamical system to reach a goal state under kinematic and dynamic (i.e., kinodynamic) constraints. Moreover, industrial robots perform such motions over and over again and therefore demand efficiency, i.e., optimal motion. In many applications, the initial state may not be constrained and can be taken as an additional variable for optimization. The semi-stochastic kinodynamic planning (SKIP) algorithm presented in this paper is a novel method for trajectory optimization of a fully actuated dynamic system to reach a goal state under kinodynamic constraints. The basic principle of the algorithm is the parameterization of the motion trajectory to a vector in a high-dimensional space. The kinematic and dynamic constraints are formulated in terms of time and the trajectory parameters vector. That is, the constraints define a time-varying domain in the high dimensional parameters space. We propose a semi stochastic technique that finds a feasible set of parameters satisfying the constraints within the time interval dedicated to task completion. The algorithm chooses the optimal solution based on a given cost function. Statistical analysis shows the probability to find a solution if one exists. For simulations, we found a time-optimal trajectory for a 6R manipulator to hit a disk in a desired state.

## Keywords

Motion planning Kinodynamic constraints Trajectory optimization

## Notes

### Acknowledgements

The research was supported by the Helmsley Charitable Trust through the Agricultural, Biological and Cognitive Robotics Center of Ben-Gurion University of the Negev.

## Supplementary material

Supplementary material 1 (mp4 20184 KB)

## References

1. Boyd, S., & Vandenberghe, L. (2004). Convex optimization. Berichte über verteilte messysteme. Cambridge: Cambridge University Press.
2. Brooks, R. A., & Lozano-Perez, T. (1983). A subdivision algorithm configuration space for findpath with rotation. In Proceedings of the international joint conferences on artificial intelligence (Vol. 2, pp. 799–806).Google Scholar
3. Byravan, A., Boots, B., Srinivasa, S., & Fox, D. (2014). Space-time functional gradient optimization for motion planning. In IEEE international conference on robotics and automation (ICRA) Google Scholar
4. Canny, J. (1988). The complexity of robot motion planning. ACM doctoral dissertation award. Cambridge: MIT Press.Google Scholar
5. Cauchy, A. (1847). Méthode générale pour la résolution des systémes d’équations simultanées. Comptes rendus de l’Académie des Sciences, 25, 536–538.Google Scholar
6. Clark, C. M. (2005). Probabilistic road map sampling strategies for multi-robot motion planning. Robotics and Autonomous Systems, 53(3–4), 244–264.
7. Denny, J., Shi, K., & Amato, N. (2013). Lazy toggle PRM: A single-query approach to motion planning. In Proceedings of the IEEE international conference on robotics and automation (pp. 2407–2414)Google Scholar
8. Donald, B., Xavier, P., Canny, J., & Reif, J. (1993). Kinodynamic motion planning. Journal of the ACM, 40(5), 1048–1066.
9. Dragan, A., Ratliff, N., & Srinivasa, S. (2011). Manipulation planning with goal sets using constrained trajectory optimization. In IEEE international conference on robotics and automation (ICRA) (pp. 4582–4588)Google Scholar
10. Heo, Y. J., & Chung, W. K. (2013). RRT-based path planning with kinematic constraints of AUV in underwater structured environment. In 10th international conference on ubiquitous robots and ambient intelligence (URAI) (pp. 523–525)Google Scholar
11. Hsu, D., Kindel, R., Latombe, J.-C., & Rock, S. (2002). Randomized kinodynamic motion planning with moving obstacles. The International Journal of Robotics Research, 21(3), 233–255.
12. Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P., Schaal, S. (2011). Stomp: Stochastic trajectory optimization for motion planning. In 2011 IEEE international conference on robotics and automation (ICRA) (pp. 4569–4574)Google Scholar
13. Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 846–894.
14. Kavraki, L., Svestka, P., Latombe, J. C., & Overmars, M. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 12(4), 566–580.
15. Keller, H. (1976). Numerical solution of two point boundary value problems. Society for industrial and applied mathematics.Google Scholar
16. Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research, 5(1), 90–98.
17. Kim, D. H., Lim, S. J., Lee, D. H., Lee, J. Y., & Han, C. S. (2013) A RRT-based motion planning of dual-arm robot for (dis)assembly tasks. In 44th international symposium on robotics (ISR) (pp. 1–6)Google Scholar
18. Kuhn, H. W., & Tucker, A. W. (1950). Nonlinear programming. In J. Neyman (Ed.), Proceedings of the 2nd Berkeley symposium on mathematical statistics and probability (pp. 481–492). Berkeley: University of California Press.Google Scholar
19. LaValle, S., & Kuffner J. J. J. (1999). Randomized kinodynamic planning. In Proceedings of the IEEE international conference on robotics and automation (Vol. 1, pp. 473–479)Google Scholar
20. Lavalle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning. Technical report.Google Scholar
21. Lebesgue, H., & May, K. (1966). Measure and the integral., The Mathesis Series San Francisco: Holden-Day.Google Scholar
22. Luders, B., Karaman, S., Frazzoli, E., & How, J. (2010). Bounds on tracking error using closed-loop rapidly-exploring random trees. In American control conference (ACC) (pp. 5406–5412)Google Scholar
23. Motonaka, K., Watanabe, K., & Maeyama, S. (2013). Motion planning of a UAV using a kinodynamic motion planning method. In 39th annual conference of the IEEE industrial electronics society (IECON) (pp. 6383–6387)Google Scholar
24. Park, C., Pan, J., & Manocha, D. (2012). ITOMP: Incremental trajectory optimization for real-time replanning in dynamic environments. In Proceedings of the 22nd international conference on automated planning and scheduling, ICAPS 2012 (pp. 207–215), Atibaia, São Paulo, June 25–19, 2012Google Scholar
25. Pham, Q.-C., Caron, S., & Nakamura, Y. (2013). Kinodynamic planning in the configuration space via admissible velocity propagation. In Proceedings of robotics: Science and systems.Google Scholar
26. Rao, A. (2014). Trajectory optimization: A survey. In H. Waschl, I. Kolmanovsky, M. Steinbuch, & L. del Re (Eds.), Optimization and optimal control in automotive systems (Vol. 455, pp. 3–21)., Lecture notes in control and information sciences Berlin: Springer.
27. Rao, A. V. (2009). A survey of numerical methods for optimal control. Advances in the Astronautical Sciences, 135(1), 497–528.Google Scholar
28. Ratliff, N., Zucker, M., Bagnell, J. A. D., & Srinivasa, S. (2009). Chomp: Gradient optimization techniques for efficient motion planning. In IEEE international conference on robotics and automation (ICRA) Google Scholar
29. Reif, J. (1979). Complexity of the mover’s problem and generalizations. In 20th annual symposium on foundations of computer science (pp. 421–427)Google Scholar
30. Rimon, E., & Koditschek, D. (1992). Exact robot navigation using artificial potential functions. IEEE Transactions on Robotics and Automation, 8(5), 501–518.
31. Schulman, J., Ho, J., Lee, A., Awwal, I., Bradlow, H., & Abbeel, P. (2013). Finding locally optimal, collision-free trajectories with sequential convex optimization. In Proceedings of the robotics: Science and systems (RSS) Google Scholar
32. Schwartz, J. T., & Sharir, M. (1983). On the ’piano movers’ problem. II. General techniques for computing topological properties of real algebraic manifolds. Advances in Applied Mathematics, 4(3), 298–351.
33. Sintov, A., & Shapiro, A. (2014) Time-based RRT algorithm for rendezvous planning of two dynamic systems. In Proceedings of the IEEE international conference on robotics and automation (ICRA) (pp. 6745–6750)Google Scholar
34. Sintov, A., & Shapiro, A. (2015). A stochastic dynamic motion planning algorithm for object-throwing. In Proceedings of the IEEE international conference on robotics and automation Google Scholar
35. Sintov, A., & Shapiro, A. (2017). Dynamic regrasping by in-hand orienting of grasped objects using non-dexterous robotic grippers. In Robotics and computer-integrated manufacturing Google Scholar
36. Song, G., & Amato, N. (2001). Randomized motion planning for car-like robots with c-prm. In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (Vol. 1, pp. 37–42)Google Scholar
37. Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2006). Robot modeling and control (Vol. 3). New York: Wiley.Google Scholar
38. Van der Stappen, A., Halperin, D., & Overmars, M. (1993). Efficient algorithms for exact motion planning amidst fat obstacles. In Proceedings of the IEEE international conference on robotics and automation (Vol. 1, pp. 297–304)Google Scholar
39. Van der Stappen, A. F., Overmars, M. H., de Berg, M., & Vleugels, J. (1998). Motion planning in environments with low obstacle density. Discrete & Computational Geometry, 20(4), 561–587.
40. Verscheure, D., Demeulenaere, B., Swevers, J., De Schutter, J., & Diehl, M. (2009). Time-optimal path tracking for robots: A convex optimization approach. IEEE Transactions on Automatic Control, 54(10), 2318–2327.
41. Webb, D., & van den Berg, J. (2013). Kinodynamic rrt*: Asymptotically optimal motion planning for robots with linear dynamics. In: IEEE international conference on robotics and automation (ICRA) (pp. 5054–5061)Google Scholar
42. Zucker, M., Ratliff, N., Dragan, A. D., Pivtoraiko, M., Klingensmith, M., Dellin, C. M., et al. (2013). CHOMP: Covariant Hamiltonian optimization for motion planning. The International Journal of Robotics Research, 32(9–10), 1164–1193.Google Scholar
{"extraction_info": {"found_math": false, "script_math_tex": 0, "script_math_asciimath": 0, "math_annotations": 0, "math_alttext": 0, "mathml": 0, "mathjax_tag": 0, "mathjax_inline_tex": 0, "mathjax_display_tex": 0, "mathjax_asciimath": 0, "img_math": 0, "codecogs_latex": 0, "wp_latex": 0, "mimetex.cgi": 0, "/images/math/codecogs": 0, "mathtex.cgi": 0, "katex": 0, "math-container": 0, "wp-katex-eq": 0, "align": 0, "equation": 0, "x-ck12": 0, "texerror": 0, "math_score": 0.8651583194732666, "perplexity": 5565.974853357854}, "config": {"markdown_headings": true, "markdown_code": true, "boilerplate_config": {"ratio_threshold": 0.18, "absolute_threshold": 10, "end_threshold": 15, "enable": true}, "remove_buttons": true, "remove_image_figures": true, "remove_link_clusters": true, "table_config": {"min_rows": 2, "min_cols": 3, "format": "plain"}, "remove_chinese": true, "remove_edit_buttons": true, "extract_latex": true}, "warc_path": "s3://commoncrawl/crawl-data/CC-MAIN-2018-17/segments/1524125947759.42/warc/CC-MAIN-20180425080837-20180425100837-00044.warc.gz"}