## Abstract

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.

## References

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.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).Betts, J. T. (1998). Survey of numerical methods for trajectory optimization.

*Journal of Guidance, Control, and Dynamics*,*21*(2), 193–207.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.Bristow, D. A., Tharayil, M., & Alleyne, A. G. (2006). A survey of iterative learning control.

*IEEE Control Systems*,*26*(3), 96–114.Cobb, G. W., Witmer, J. A., & Cryer, J. D. (1997).

*An electronic companion to statistics*. New York: Cogito Learning Media.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.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.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.Gill, P. E., Murray, W., & Saunders, M. A. (1997).

*An SQP algorithm for large-scale constrained optimization: Snopt*.GNU. (2015). Gnu linear programming kit (glpk). http://www.gnu.org/software/glpk/glpk.html. Accessed 16 April 2015.

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)*.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.Hauser, K. (2013a). Fast interpolation and time-optimization on implicit contact submanifolds. In

*Robotics: Science and systems*.Hauser, K. (2013b). Robust contact generation for robot simulation with unstructured meshes. In

*International symposium on robotics research*, Singapore.Hauser, K. (2014). Fast interpolation and time-optimization with contact.

*The International Journal of Robotics Research*,*33*(9), 1231–1250.Kunz, T., & Stilman, M. (2012). Time-optimal trajectory generation for path following with bounded acceleration and velocity. In

*Robotics: Science and systems*.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.Lertkultanon, P., & Pham, Q.-C. (2014). Dynamic non-prehensile object transportation. In

*International conference on control automation robotics vision (ICARCV)*(pp. 1392–1397).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.Liu, C. K. (2009). Dextrous manipulation from a grasping pose.

*ACM Transactions on Graphics (TOG)*,*28*(3), 59.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).Luo, J., & Hauser, K. (2015). Robust trajectory optimization under frictional contact with iterative learning. In

*Robotics: Science and systems*.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.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.Nguyen-Tuong, D., & Peters, J. (2011). Model learning for robot control: A survey.

*Cognitive processing*,*12*(4), 319–340.Pham, Q.-C., Caron, S., Lertkultanon, P., & Nakamura, Y. (2014). Planning truly dynamic motions: Path-velocity decomposition revisited. arXiv preprint arXiv:1411.4045.

Posa, M., & Tedrake, R. (2012). Direct trajectory optimization of rigid body dynamical systems through contact. In

*Workshop on the algorithmic foundations of robotics*.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.Schaal, S., & Atkeson, C. G. (2010). Learning control in robotics.

*IEEE Robotics & Automation Magazine*,*17*(2), 20–29.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.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.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.von Stryk, O., & Bulirsch, R. (1992). Direct and indirect methods for trajectory optimization.

*Annals of Operations Research*,*37*(1), 357–373.

## Acknowledgements

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

## Author information

### Affiliations

### Corresponding author

## Additional information

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

## Appendix

### Appendix

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

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

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

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

as desired.

## Rights and permissions

## About this article

### Cite this article

Luo, J., Hauser, K. Robust trajectory optimization under frictional contact with iterative learning.
*Auton Robot* **41, **1447–1461 (2017). https://doi.org/10.1007/s10514-017-9629-x

Received:

Accepted:

Published:

Issue Date:

### Keywords

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