Robust trajectory optimization under frictional contact with iterative learning


Optimization is often difficult to apply to robots due to the presence of errors in model parameters, which can cause constraints to be violated during execution on the robot. This paper presents a method to optimize trajectories with large modeling errors using a combination of robust optimization and parameter learning. In particular it considers the context of contact modeling, which is highly susceptible to errors due to uncertain friction estimates, contact point estimates, and sensitivity to noise in actuator effort. A robust time-scaling method is presented that computes a dynamically-feasible, minimum-cost trajectory along a fixed path under frictional contact. The robust optimization model accepts confidence intervals on uncertain parameters, and uses a convex parameterization that computes dynamically-feasible motions in seconds. Optimization is combined with an iterative learning method that uses feedback from execution to learn confidence bounds on modeling parameters. It is applicable to general problems with multiple uncertain parameters that satisfy a monotonicity condition that requires parameters to have conservative and optimistic settings. The method is applied to manipulator performing a “waiter” task, on which an object is moved on a carried tray as quickly as possible, and to a simulated humanoid locomotion task. Experiments demonstrate this method can compensate for large modeling errors within a handful of iterations.

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


  1. Argall, B. D., Chernova, S., Veloso, M., & Browning, B. (2009). A survey of robot learning from demonstration. Robotics and Autonomous Systems, 57(5), 469–483.

    Article  Google Scholar 

  2. Bertsimas, D., & Thiele, A. (2006). Robust and data-driven optimization: Modern decision-making under uncertainty. In INFORMS tutorials in operations research: Models, methods, and applications for innovative decision making (pp. 1–39).

  3. Betts, J. T. (1998). Survey of numerical methods for trajectory optimization. Journal of Guidance, Control, and Dynamics, 21(2), 193–207.

    Article  MATH  Google Scholar 

  4. Bobrow, J. E., Dubowsky, S., & Gibson, J. S. (1985). Time-optimal control of robotic manipulators along specified paths. The International Journal of Robotics Research, 4(3), 3–17.

    Article  Google Scholar 

  5. Bristow, D. A., Tharayil, M., & Alleyne, A. G. (2006). A survey of iterative learning control. IEEE Control Systems, 26(3), 96–114.

    Article  Google Scholar 

  6. Cobb, G. W., Witmer, J. A., & Cryer, J. D. (1997). An electronic companion to statistics. New York: Cogito Learning Media.

    Google Scholar 

  7. Constantinescu, D., & Croft, E. A. (2000). Smooth and time-optimal trajectory planning for industrial manipulators along specified paths. Journal of Robotic Systems, 17, 223–249.

    Article  MATH  Google Scholar 

  8. Dahl, O., & Nielsen, L. (1989). Torque limited path following by on-line trajectory time scaling. In IEEE international conference on robotics and automation (ICRA) (Vol. 2, pp. 1122–1128). doi:10.1109/ROBOT.1989.100131.

  9. Escande, A., Kheddar, A., Miossec, S., & Garsault, S. (2009) Planning support contact-points for acyclic motions and experiments on HRP-2. In: O. Khatib, V. Kumar, G. J. Pappas (Eds.), Experimental Robotics. Springer Tracts in Advanced Robotics, Vol. 54. Springer, Berlin, Heidelberg.

  10. Gill, P. E., Murray, W., & Saunders, M. A. (1997). An SQP algorithm for large-scale constrained optimization: Snopt.

  11. GNU. (2015). Gnu linear programming kit (glpk). Accessed 16 April 2015.

  12. Harada, K., Hauser, K., Bretl, T., & Latombe, J.-C. (2006). Natural motion generation for humanoid robots. In IEEE/RSJ international conference on intelligent robots and systems (IROS).

  13. Hargraves, C. R., & Paris, S. W. (1987). Direct trajectory optimization using nonlinear programming and collocation. Journal of Guidance, Control, and Dynamics, 10(4), 338–342.

    Article  MATH  Google Scholar 

  14. Hauser, K. (2013a). Fast interpolation and time-optimization on implicit contact submanifolds. In Robotics: Science and systems.

  15. Hauser, K. (2013b). Robust contact generation for robot simulation with unstructured meshes. In International symposium on robotics research, Singapore.

  16. Hauser, K. (2014). Fast interpolation and time-optimization with contact. The International Journal of Robotics Research, 33(9), 1231–1250.

    Article  Google Scholar 

  17. Kunz, T., & Stilman, M. (2012). Time-optimal trajectory generation for path following with bounded acceleration and velocity. In Robotics: Science and systems.

  18. Lengagne, S., Ramdani, N., & Fraisse, P. (2011). Planning and fast replanning safe motions for humanoid robots. IEEE Transactions on Robotics, 27(6), 1095–1106. doi:10.1109/TRO.2011.2162998. ISSN 1552-3098.

    Article  Google Scholar 

  19. Lertkultanon, P., & Pham, Q.-C. (2014). Dynamic non-prehensile object transportation. In International conference on control automation robotics vision (ICARCV) (pp. 1392–1397).

  20. Lipp, T., & Boyd, S. (2014). Minimum-time speed optimisation over a fixed path. International Journal of Control, 87(6), 1297–1311. doi:10.1080/00207179.2013.875224.

    MathSciNet  Article  MATH  Google Scholar 

  21. Liu, C. K. (2009). Dextrous manipulation from a grasping pose. ACM Transactions on Graphics (TOG), 28(3), 59.

    Google Scholar 

  22. Luo, J., & Hauser, K. (2012). Interactive generation of dynamically feasible robot trajectories from sketches using temporal mimicking. In IEEE international conference on robotics and automation (ICRA) (pp. 3665–3670).

  23. Luo, J., & Hauser, K. (2015). Robust trajectory optimization under frictional contact with iterative learning. In Robotics: Science and systems.

  24. Lynch, K. M., & Mason, M. T. (1996). Dynamic underactuated nonprehensile manipulation. In IEEE/RSJ international conference on intelligent robots and systems (IROS) (Vol. 2, pp. 889–896). IEEE.

  25. Mordatch, I., Popović, Z., & Todorov, E. (2012). Contact-invariant optimization for hand manipulation. In Proceedings of the ACM SIGGRAPH/eurographics symposium on computer animation (pp. 137–144). Eurographics Association.

  26. Nguyen-Tuong, D., & Peters, J. (2011). Model learning for robot control: A survey. Cognitive processing, 12(4), 319–340.

    Article  Google Scholar 

  27. Pham, Q.-C., Caron, S., Lertkultanon, P., & Nakamura, Y. (2014). Planning truly dynamic motions: Path-velocity decomposition revisited. arXiv preprint arXiv:1411.4045.

  28. Posa, M., & Tedrake, R. (2012). Direct trajectory optimization of rigid body dynamical systems through contact. In Workshop on the algorithmic foundations of robotics.

  29. Posa, M., Cantu, C., & Tedrake, R. (2014). A direct method for trajectory optimization of rigid bodies through contact. The International Journal of Robotics Research, 33(1), 69–81.

    Article  Google Scholar 

  30. Schaal, S., & Atkeson, C. G. (2010). Learning control in robotics. IEEE Robotics & Automation Magazine, 17(2), 20–29.

    Article  Google Scholar 

  31. Shin, K., & McKay, N. (1985). Minimum-time control of robotic manipulators with geometric path constraints. IEEE Transactions on Automatic Control, 30, 531–541. doi:10.1109/TAC.1985.1104009.

    Article  MATH  Google Scholar 

  32. Slotine, J.-J. E., & Yang, H. S. (1989). Improving the efficiency of time-optimal path-following algorithms. IEEE Transactions on Robotics and Automation, 5(1), 118–124. doi:10.1109/70.88024. ISSN 1042-296X.

    Article  Google Scholar 

  33. 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. doi:10.1109/TAC.2009.2028959. ISSN 0018-9286.

    MathSciNet  Article  Google Scholar 

  34. von Stryk, O., & Bulirsch, R. (1992). Direct and indirect methods for trajectory optimization. Annals of Operations Research, 37(1), 357–373.

    MathSciNet  Article  MATH  Google Scholar 

Download references


This work is partially supported under NSF Grants IIS # 1218534 and CAREER # 3332066.

Author information



Corresponding author

Correspondence to Kris Hauser.

Additional information

This is one of several papers published in Autonomous Robots comprising the ”Special Issue on Robotics Science and Systems”.



Here we derive the formula for the derivative of the objective function of an inequality-constrained LP with respect to changes in the constraint RHS. Consider the LP

$$\begin{aligned} \min _{x}{c^{T}x}~\text {such that}~Ax - b(t) \le 0. \end{aligned}$$

By the first-order KKT conditions, the optimal solution satisfies

$$\begin{aligned}&c + A^{T}\mu = 0 \nonumber \\&Ax^* - b \le 0\nonumber \\&\mu ^{T}(Ax^* - b) = 0\nonumber \\&\mu \le 0 \end{aligned}$$

where \(\mu \) is the vector of KKT multipliers (unrelated to the friction coefficient) and \(x^*\) is the optimal solution. If the LP is bounded, \(x^*\) lies at a corner point of the feasible region, and is defined by active constraints with \(A_{i}x^* - b_i = 0\) and \(\mu _i < 0\). Denote \(\hat{A}\) and \(\hat{b}\) respectively as the matrix/vector containing the rows of A and b corresponding to active constraints, and let \(\hat{\mu }\) be the set of active multipliers. Then the equations

$$\begin{aligned} \begin{aligned} c + \hat{A}^{T}\hat{\mu }&= 0\\ \hat{A} x^* - \hat{b}&= 0 \end{aligned} \end{aligned}$$

must be satisfied, with \(\hat{A}\) an invertible matrix. Hence, \(x^*(t) = \hat{A}^{-1} \hat{b}(t)\), and \(\frac{d}{dt}(c^T x^*) = c^T \hat{A}^{-1}\hat{b}^{\prime }(t)\). Since \(\hat{\mu }^{T} = -c^{T}\hat{A}^{-1}\), we obtain

$$\begin{aligned} \frac{d}{dt}(c^T x^*) = -\hat{\mu }^{T} \hat{b}^{\prime }(t) = -\mu ^{T} b^{\prime }(t) \end{aligned}$$

as desired.

Rights and permissions

Reprints and Permissions

About this article

Verify currency and authenticity via CrossMark

Cite this article

Luo, J., Hauser, K. Robust trajectory optimization under frictional contact with iterative learning. Auton Robot 41, 1447–1461 (2017).

Download citation


  • Robotics
  • Trajectory optimization
  • Robust optimization
  • Model uncertainty
  • Contact modeling
  • Manipulation
  • Humanoid robots