Abstract
In this work, we propose a trajectory generation method for robotic systems with contact kinematics and force constraints based on optimal control and reachability analysis tools. Normally, the dynamics and constraints of a contact-constrained robot are nonlinear and coupled to each other. Instead of linearizing the model and constraints, we solve the optimal control problem directly to obtain feasible state trajectories and their corresponding control inputs. A tractable optimal control problem is formulated and subsequently addressed by dual approaches, which rely on sampling-based dynamic programming and rigorous reachability analysis tools. In particular, a sampling-based method together with a Partially Observable Markov Decision Process solution approach are used to break down the end-to-end trajectory generation problem by generating a sequence of subregions that the system’s trajectory will have to pass through to reach its final destination. The distinctive characteristic of the proposed trajectory optimization algorithm is its ability to handle the intricate contact constraints, coupled with the system dynamics, in a computationally efficient way. We validate our method using extensive numerical simulations with two legged robots.
This is a preview of subscription content, log in via an institution to check access.
Similar content being viewed by others
Explore related subjects
Discover the latest articles, news and stories from top researchers in related subjects.Notes
https://www.wolfram.com/mathematica/(ver.12).
https://www.mathworks.com/ (R2019b).
References
Arcak, M., & Maidens, J. (2017). Simulation-based reachability analysis for nonlinear systems using componentwise contraction properties. arXiv preprintarXiv:1709.06661.
Asarin, E., Bournez, O., Dang, T., & Maler, O. (2000). Approximate reachability analysis of piecewise-linear dynamical systems. In International workshop on hybrid systems: Computation and control (pp. 20–31). Springer.
Blanchini, F., & Miani, S. (2008). Set-theoretic methods in control. Berlin: Springer.
Budhiraja, R., Carpentier, J., Mastalli, C., & Mansard, N. (2018). Differential dynamic programming for multi-phase rigid contact dynamics. In Proceeding of the IEEE/RSJ international conference on humanoid robots (pp. 1–9). IEEE.
Burget, F., & Bennewitz, M. (2015). Stance selection for humanoid grasping tasks by inverse reachability maps. In Proceedings of the IEEE international conference on robotics and automation (pp. 5669–5674). IEEE.
Carius, J., Ranftl, R., Koltun, V., & Hutter, M. (2018). Trajectory optimization with implicit hard contacts. IEEE Robotics and Automation Letters, 3(4), 3316–3323.
Caron, S., Pham, Q.-C., & Nakamura, Y. (2015). Stability of surface contacts for humanoid robots: Closed-form formulae of the contact wrench cone for rectangular support areas. In Proceedings of the IEEE international conference on robotics and automation (pp. 5107–5112).
Carpentier, J., & Mansard, N. (2018). Multicontact locomotion of legged robots. IEEE Transactions on Robotics, 34(6), 1441–1460.
Duckham, M., Kulik, L., Worboys, M., & Galton, A. (2008). Efficient generation of simple polygons for characterizing the shape of a set of points in the plane. Pattern Recognition, 41(10), 3224–3236.
Fernbach, P., Tonneau, S., Stasse, O., Carpentier, J., & Taïx, M. (2020). C-croc: Continuous and convex resolution of centroidal dynamic trajectories for legged robots in multicontact scenarios. IEEE Transactions on Robotics, 36(3), 676–691.
Galton, A., & Duckham, M. (2006). What is the region occupied by a set of points? In International conference on geographic information science (pp. 81–98). Springer.
Gill, P. E., Murray, W., & Saunders, M. A. (2005). Snopt: An sqp algorithm for large-scale constrained optimization. SIAM Review, 47(1), 99–131.
Girard, A. (2005). Reachability of uncertain linear systems using zonotopes. In International workshop on hybrid systems: Computation and control (pp. 291–305). Springer.
Girard, A., Le Guernic, C., & Maler, O. (2006). Efficient computation of reachable sets of linear time-invariant systems with inputs. In International workshop on hybrid systems: Computation and control (pp. 257–271). Springer.
Guan, Y., Yokoi, K., & Zhang, X. (2008). Numerical methods for reachable space generation of humanoid robots. International Journal of Robotics Research, 27(8), 935–950.
Hamadeh, A., & Goncalves, J. (2008). Reachability analysis of continuous-time piecewise affine systems. Automatica, 44(12), 3189–3194.
Hänsch, P., Diab, H., Makhlouf, I. B., & Kowalewski, S. (2013). Reachability analysis of linear systems with stepwise constant inputs. Electronic Notes in Theoretical Computer Science, 297, 61–74.
Hendeby, G., & Gustafsson, F. (2007). On nonlinear transformations of Gaussian distributions. Technical Report from automatic control at link? pings Universitet.
Hereid, A., & Ames, A. D. (2017). Frost fast robot optimization and simulation toolkit. In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (pp. 719–726). IEEE.
Herzog, A., Rotella, N., Schaal, S., & Righetti, L. (2015). Trajectory generation for multi-contact momentum control. In Proceeding of the IEEE/RSJ international conference on humanoid robots (pp. 874–880). IEEE.
Islam, F., Nasir, J., Malik, U., Ayaz, Y., & Hasan, O. (2012). RRT*-smart: Rapid convergence implementation of RRT* towards optimal solution. In 2012 IEEE international conference on mechatronics and automation (pp. 1651–1656). IEEE.
Jorgensen, S. J., Vedantam, M., Gupta, R., Cappel, H., & Sentis, L. (2020). Finding locomanipulation plans quickly in the locomotion constrained manifold. Proceedings of the IEEE international conference on robotics and automation.
Kajita, S., Kanehiro, F., Kaneko, K., Fujiwara, K., Harada, K., Yokoi, K., et al. (2003). Biped walking pattern generation by using preview control of zero-moment point. Proceedings of the IEEE International Conference on Robotics and Automation, 3, 1620–1626.
Karaman, S., Walter, M. R., Perez, A., Frazzoli, E., & Teller, S. (2011). Anytime motion planning using the rrt*. In Proceedings of the IEEE international conference on robotics and automation (pp. 1478–1483). IEEE.
Kariotoglou, N., Summers, S., Summers, T., Kamgarpour, M., & Lygeros, J. (2013). Approximate dynamic programming for stochastic reachability. In Proceedings of European control conference (pp. 584–589).
Kavraki, L. E., Svestka, P., Latombe, J.-C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 12(4), 566–580.
Khatib, O. (1987). A unified approach for motion and force control of robot manipulators: The operational space formulation. International Journal of Robotics and Automation, 3(1), 43–53.
Kim, D., Ahn, J., Campbell, O., Paine, N., & Sentis, L. (2018). Investigations of a robotic testbed with viscoelastic liquid cooled actuators. IEEE/ASME Transactions on Mechatronics, 23(6), 2704–2714.
Kim, D., Jorgensen, S. J., Lee, J., Ahn, J., Luo, J., & Sentis, L. (2019). Dynamic locomotion for passive-ankle biped robots and humanoids using whole-body locomotion control. International Journal of Robotics Research, p. 0278364920918014.
Kim, J.-H. (2008). Improved ellipsoidal bound of reachable sets for time-delayed linear systems with disturbances. Automatica, 44(11), 2940–2943.
Kuffner, J. J., & LaValle, S. M. (2000). Rrt-connect: An efficient approach to single-query path planning. In Proceedings of the IEEE international conference on robotics and automation (Vol. 2, pp. 995–1001). IEEE.
Le Guernic, C., & Girard, A. (2010). Reachability analysis of linear systems using support functions. Nonlinear Analysis: Hybrid Systems, 4(2), 250–262.
Lee, I., & Oh, J.-H. (2016). Humanoid posture selection for reaching motion and a cooperative balancing controller. Journal of Intelligent & Robotic Systems, 81(3–4), 301–316.
Lee, J., Ahn, J., Bakolas, E., & Sentis, L. (2020). Reachability-based trajectory optimization for robotic systems given sequences of rigid contacts. In Proceedings of American control conference (pp. 2158–2165). IEEE.
Lee, J., Bakolas, E., & Sentis, L. (2019). Trajectory generation for robotic systems with contact force constraints. In Proceedings of American control conference (pp. 671–678). IEEE.
Lee, J., Grey, M. X., Ha, S., Kunz, T., Jain, S., Ye, Y., et al. (2018). Dart: Dynamic animation and robotics toolkit. The Journal of Open Source Software, 3(22), 500.
Liebenwein, L., Baykal, C., Gilitschenski, I., Karaman, S., & Rus, D. (2018). Sampling-based approximation algorithms for reachability analysis with provable guarantees. In Robotics: Science and systems, Pittsburgh, Pennsylvania. https://doi.org/10.15607/RSS.2018.XIV.014.
Liu, Y., Wensing, P. M., Orin, D. E., & Zheng, Y. F. (2015). Trajectory generation for dynamic walking in a humanoid over uneven terrain using a 3d-actuated dual-slip model. In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (pp. 374–380). IEEE.
Lofaro, D. M., Ellenberg, R., Oh, P., & Oh, J.-H. (2012). Humanoid throwing: Design of collision-free trajectories with sparse reachable maps. In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (pp. 1519–1524). IEEE.
Maidens, J., & Arcak, M. (2015). Reachability analysis of nonlinear systems using matrix measures. IEEE Transactions on Automatic Control, 60(1), 265–270.
Manchester, Z., & Kuindersma, S. (2019). Robust direct trajectory optimization using approximate invariant funnels. Autonomous Robots, 43(2), 375–387.
Mansard, N., Khatib, O., & Kheddar, A. (2009). A unified approach to integrate unilateral constraints in the stack of tasks. IEEE Transactions on Robotics, 25(3), 670–685.
Mastalli, C., Budhiraja, R., Merkt, W., Saurel, G., Hammoud, B., Naveau, M., Carpentier, J., Vijayakumar, S., & Mansard, N. (2019). Crocoddyl: An efficient and versatile framework for multi-contact optimal control. arXiv preprint arXiv:1909.04947.
Mastalli, C., Havoutis, I., Focchi, M., Caldwell, D. G., & Semini, C. (2020). Motion planning for quadrupedal locomotion: Coupled planning, terrain mapping and whole-body control. arXiv preprint arXiv:2003.05481.
Mitchell, I. M., Bayen, A. M., & Tomlin, C. J. (2005). A time-dependent Hamilton–Jacobi formulation of reachable sets for continuous dynamic games. IEEE Transactions on Automatic Control, 50(7), 947–957.
Moreira, A., & Santos, M. Y. (2007). Concave hull: A k-nearest neighbours approach for the computation of the region occupied by a set of points.
Nguyen, Q., Hereid, A., Grizzle, J. W., Ames, A. D., & Sreenath, K. (2016). 3d dynamic walking on stepping stones with control barrier functions. In Proceedings of the IEEE conference on decision and control (pp. 827–834). IEEE.
Nguyen, Q., & Sreenath, K. (2015). Optimal robust control for bipedal robots through control Lyapunov function based quadratic programs. In Robotics: Science and systems, Rome, Italy.
Posa, M., Cantu, C., & Tedrake, R. (2014). A direct method for trajectory optimization of rigid bodies through contact. International Journal of Robotics Research, 33(1), 69–81.
Posa, M., Kuindersma, S., & Tedrake, R. (2016). Optimization and stabilization of trajectories for constrained dynamical systems. In Proceedings of the IEEE international conference on robotics and automation (pp. 1366–1373). IEEE.
Radford, N. A., Strawser, P., Hambuchen, K., Mehling, J. S., Verdeyen, W. K., Donnan, A. S., et al. (2015). Valkyrie: Nasa’s first bipedal humanoid robot. Journal of Field Robotics, 32(3), 397–419.
Righetti, L., Buchli, J., Mistry, M., & Schaal, S. (2011). Inverse dynamics control of floating-base robots with external constraints: A unified view. In Proceedings of the IEEE international conference on robotics and automation (pp. 1085–1090). IEEE.
Rungger, M., & Zamani, M. (2018). Accurate reachability analysis of uncertain nonlinear systems. In Proceedings of the 21st international conference on hybrid systems: Computation and control (part of CPS week) (pp. 61–70). ACM.
Sakcak, B., Bascetta, L., Ferretti, G., & Prandini, M. (2019). Sampling-based optimal kinodynamic planning with motion primitives. Autonomous Robots, 43(7), 1715–1732.
Sentis, L., & Khatib, O. (2005). Synthesis of whole-body behaviors through hierarchical control of behavioral primitives. International Journal of Humanoid Robotics, 2(04), 505–518.
Sintov, A. (2019). Goal state driven trajectory optimization. Autonomous Robots, 43(3), 631–648.
Stephens, B. J., & Atkeson, C. G. (2010). Dynamic balance force control for compliant humanoid robots. In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (pp. 1248–1255).
Tassa, Y., Erez, T., & Todorov, E. (2012). Synthesis and stabilization of complex behaviors through online trajectory optimization. In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (pp. 4906–4913). IEEE.
Vahrenkamp, N., Asfour, T., & Dillmann, R. (2013). Robot placement based on reachability inversion. In Proceedings of the IEEE international conference on robotics and automation (pp. 1970–1975). IEEE.
Vahrenkamp, N., Berenson, D., Asfour, T., Kuffner, J., & Dillmann, R. (2009). Humanoid motion planning for dual-arm manipulation and re-grasping tasks. In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (pp. 2464–2470). IEEE.
Wächter, A., & Biegler, L. T. (2006). On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Mathematical Programming, 106(1), 25–57.
Wensing, P. M., & Orin, D. E. (2016). Improved computation of the humanoid centroidal dynamics and application for whole-body control. International Journal of Humanoid Robotics, 13(01), 1550039.
Wieber, P.-B. (2006). Holonomy and nonholonomy in the dynamics of articulated motion. In Fast motions in biomechanics and robotics (pp. 411–425). Springer.
Yang, Y., Merkt, W., Ferrolho, H., Ivan, V., & Vijayakumar, S. (2017). Efficient humanoid motion planning on uneven terrain using paired forward-inverse dynamic reachability maps. IEEE Robotics and Automation Letters, 2(4), 2279–2286.
Acknowledgements
The authors would like to thank the members of the Human Centered Robotics Laboratory at The University of Texas at Austin for their great help and support. This work was supported by an NSF Grant# 1724360 and partially supported by an ONR Grant# N000141512507. The second author acknowledges partial support by NSF under Grant #1924790.
Author information
Authors and Affiliations
Corresponding author
Additional information
Publisher's Note
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Appendices
Appendices
Proof of Proposition 1
Proof
Since the samples are uniformly distributed, it is possible to select any unit vector in \(\mathbb {R}^{n_y}\) as the PSV of \(s_i\), that is, \(\mathbf {R}_a\mathcal {V}(s_i)\) where \(\mathbf {R}_a \in \mathrm {SO}(3)\) is a rotation matrix. If we select the rotation matrix \(\mathbf {R}_a\) such that \(d^{\perp } = \mathbf {R}_a \mathcal {V}(s_i)\), which is orthogonal to \(\mathcal {V}(s_i)\), it follows that \(\mathcal {T}(s',s,a) = \langle \mathcal {V}(s_{i}), d \rangle = \langle d^{\perp }, d \rangle = 0\) for all \(a \in {\mathcal {A}}\). \(\square \)
Proof of Corollary 1
Proof
Let us consider a general hull of the set \(\mathcal {R}_y(T_d^{\varDelta t}, x_0)\), that is \(\mathrm {ghull}(\mathcal {R}_{y}^{D}(T_d^{\varDelta t},x_0))\), being compact. By the Heine− Borel theorem, all closed subsets of a compact set are also compact. Since \(\mathcal {R}_{y}(T_d^{\varDelta t},x_0) \subset \mathrm {ghull}(\mathcal {R}_{y}(T_d^{\varDelta t},x_0))\), the reachable set \(\mathcal {R}_{y}(T_d^{\varDelta t},x_0)\) is compact. \(\square \)
Proof of Corollary 2
Proof
Consider three sets: \(\mathcal {F}_{1} = \mathcal {R}_{x}(T_d^{\varDelta t}, x_0)\), \(\mathcal {F}_{2} = \mathcal {R}_{x}(t_k, x_0)\), and \(\mathcal {F}_{3} = \mathcal {F}_2 \cup \mathcal {F}_{1}'\), where \(\mathcal {F}_{1}'\) is the collection of states \(x\in \mathcal {F}_1\) producing the next feasible state by Definition 5 with respect to \(x \in \mathcal {F}_{1}\). It is true that \(\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)=\mathcal {F}_1\cup \mathcal {F}_2 = \mathcal {F}_{2} \cup \mathcal {F}_{3}\). Let us consider arbitrary two sets \(\mathcal {H}_1\) and \(\mathcal {H}_2\) satisfying \(\mathcal {R}_{x}(T_d^{\varDelta t}, x_0) = \mathcal {H}_1 \cup \mathcal {H}_2\) with \(\mathcal {H}_1 \cap \mathcal {H}_2 = \emptyset \). Let \(x \in F_{1}'\) and suppose \(x \in \mathcal {H}_{1}\). Then, \(\mathcal {H}_1 \cap \mathcal {F}_{1} \ne \emptyset \) and \(\mathcal {H}_1 \cap \mathcal {F}_{3} \ne \emptyset \). This implies that \(\mathcal {F}_{1} \subseteq \mathcal {H}_1\) and \(\mathcal {F}_{3} \subseteq \mathcal {H}_1\), hence, \(\mathcal {H}_{2} = \emptyset \). This proves that \(\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)\) is connected. Since the mapping \(f_y\) is continuous, we also conclude that the set \(\mathcal {R}_{y}(T_d^{\varDelta t}, x_0)\) is connected. \(\square \)
Proof of Theorem 1
Proof
Since \(\mathcal {R}_{y}(T_d^{\varDelta t}, x_0)\) is compact and \(\hat{f}_y\) is continuous, \(\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)\) is closed and \(\hat{f}_y^{-1}\) is also continuous. Then, \(\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)\) is connected because \(\mathcal {R}_{y}(T_d^{\varDelta t}, x_0)\) is connected and \(\hat{f}_y^{-1}\) is continuous. Therefore, there exists at least one trajectory connecting \(x_0\) to \(x(\tau )\) satisfying \(f_y(x(\tau )) = y^g\) in \(\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)\). \(\square \)
Specifications of Valkyrie
We consider the following joint position/velocity/torque constraints. Excluding the virtual joints for the floating base, the actuated joints (\(\mathbb {R}^{28}\)) are specified such as
where \(\dot{q}_{UB} = + \dot{q}_{B}\), \(\dot{q}_{LB} = - \dot{q}_{B}\), \(u_{UB} = + u_{B}\), and \(u_{LB} = - u_{B}\), respectively.
Rights and permissions
About this article
Cite this article
Lee, J., Bakolas, E. & Sentis, L. An efficient and direct method for trajectory optimization of robots constrained by contact kinematics and forces. Auton Robot 45, 135–153 (2021). https://doi.org/10.1007/s10514-020-09952-7
Received:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.1007/s10514-020-09952-7