Goal state driven trajectory optimization


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.

This is a preview of subscription content, log in to check access.

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7
Fig. 8
Fig. 9
Fig. 10
Fig. 11
Fig. 12
Fig. 13
Fig. 14
Fig. 15


  1. 1.

    Matlab is a registered trademark of The Mathworks, Inc.


  1. Boyd, S., & Vandenberghe, L. (2004). Convex optimization. Berichte über verteilte messysteme. Cambridge: Cambridge University Press.

    Google Scholar 

  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).

  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)

  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.

    Article  Google Scholar 

  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)

  8. Donald, B., Xavier, P., Canny, J., & Reif, J. (1993). Kinodynamic motion planning. Journal of the ACM, 40(5), 1048–1066.

    MathSciNet  Article  MATH  Google Scholar 

  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)

  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)

  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.

    Article  MATH  Google Scholar 

  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)

  13. Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 846–894.

    Article  MATH  Google Scholar 

  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.

    Article  Google Scholar 

  15. Keller, H. (1976). Numerical solution of two point boundary value problems. Society for industrial and applied mathematics.

  16. Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research, 5(1), 90–98.

    Article  Google Scholar 

  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)

  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)

  20. Lavalle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning. Technical report.

  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)

  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)

  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, 2012

  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.

  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.

    Google Scholar 

  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)

  29. Reif, J. (1979). Complexity of the mover’s problem and generalizations. In 20th annual symposium on foundations of computer science (pp. 421–427)

  30. Rimon, E., & Koditschek, D. (1992). Exact robot navigation using artificial potential functions. IEEE Transactions on Robotics and Automation, 8(5), 501–518.

    Article  Google Scholar 

  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)

  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.

    MathSciNet  Article  MATH  Google Scholar 

  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)

  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

  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

  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)

  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)

  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.

    MathSciNet  Article  MATH  Google Scholar 

  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.

    MathSciNet  Article  MATH  Google Scholar 

  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)

  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.

Download references


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

Author information



Corresponding author

Correspondence to Avishai Sintov.

Electronic supplementary material

Below is the link to the electronic supplementary material.

Supplementary material 1 (mp4 20184 KB)

Rights and permissions

Reprints and Permissions

About this article

Verify currency and authenticity via CrossMark

Cite this article

Sintov, A. Goal state driven trajectory optimization. Auton Robot 43, 631–648 (2019). https://doi.org/10.1007/s10514-018-9728-3

Download citation


  • Motion planning
  • Kinodynamic constraints
  • Trajectory optimization