* David et al. (2019b) David, T. J., Cody, A. M., Hedges, C. L., et al. 2019b, AJ, 158, 79, doi: 10.3847/1538-3881/ab290f
  * Desort et al. (2007) Desort, M., Lagrange, A. M., Galland, F., Udry, S., & Mayor, M. 2007, A&A, 473, 983, doi: 10.1051/0004-6361:20078144
  * Donati et al. (2020) Donati, J. F., Kouach, D., Moutou, C., et al. 2020, MNRAS, 498, 5684, doi: 10.1093/mnras/staa2569
  * Dong et al. (2022) Dong, J., Huang, C. X., Zhou, G., et al. 2022, ApJ, 926, L7, doi: 10.3847/2041-8213/ac4da0
  * dos Santos et al. (2020) dos Santos, L. A., Ehrenreich, D., Bourrier, V., et al. 2020, A&A, 640, A29, doi: 10.1051/0004-6361/202038802
  * Douglas et al. (2014) Douglas, S. T., Agüeros, M. A., Covey, K. R., et al. 2014, ApJ, 795, 161, doi: 10.1088/0004-637X/795/2/161
  * Drake (1971) Drake, G. W. 1971, Phys. Rev. A, 3, 908, doi: 10.1103/PhysRevA.3.908
  * Dressing et al. (2019) Dressing, C. D., Hardegree-Ullman, K., Schlieder, J. E., et al. 2019, AJ, 158, 87, doi: 10.3847/1538-3881/ab2895
  * Dring et al. (1997) Dring, A. R., Linsky, J., Murthy, J., et al. 1997, ApJ, 488, 760, doi: 10.1086/304738
  * Dupree et al. (1992) Dupree, A. K., Sasselov, D. D., & Lester, J. B. 1992, ApJ, 387, L85, doi: 10.1086/186311
  * Ehrenreich et al. (2011) Ehrenreich, D., Lecavelier Des Etangs, A., & Delfosse, X. 2011, A&A, 529, A80, doi: 10.1051/0004-6361/201016162
  * Ehrenreich et al. (2012) Ehrenreich, D., Bourrier, V., Bonfils, X., et al. 2012, A&A, 547, A18, doi: 10.1051/0004-6361/201219981
  * Feinstein et al. (2022) Feinstein, A. D., David, T. J., Montet, B. T., et al. 2022, ApJ, 925, L2, doi: 10.3847/2041-8213/ac4745
  * Feinstein et al. (2021) Feinstein, A. D., Montet, B. T., Johnson, M. C., et al. 2021, AJ, 162, 213, doi: 10.3847/1538-3881/ac1f24
  * Fossati et al. (2022) Fossati, L., Guilluy, G., Shaikhislamov, I. F., et al. 2022, A&A, 658, A136, doi: 10.1051/0004-6361/202142336
  * Fuhrmeister et al. (2019) Fuhrmeister, B., Czesla, S., Hildebrandt, L., et al. 2019, A&A, 632, A24, doi: 10.1051/0004-6361/201936193
  * Fuhrmeister et al. (2020) —. 2020, A&A, 640, A52, doi: 10.1051/0004-6361/202038279
  * Fulton et al. (2017) Fulton, B. J., Petigura, E. A., Howard, A. W., et al. 2017, AJ, 154, 109, doi: 10.3847/1538-3881/aa80eb
  * Gaidos et al. (2021) Gaidos, E., Hirano, T., Omiya, M., et al. 2021, Research Notes of the American Astronomical Society, 5, 238, doi: 10.3847/2515-5172/ac31bd
  * Gaidos et al. (2017) Gaidos, E., Mann, A. W., Rizzuto, A., et al. 2017, MNRAS, 464, 850, doi: 10.1093/mnras/stw2345
  * Gaidos et al. (2020a) Gaidos, E., Hirano, T., Mann, A. W., et al. 2020a, MNRAS, 495, 650, doi: 10.1093/mnras/staa918
  * Gaidos et al. (2020b) Gaidos, E., Hirano, T., Wilson, D. J., et al. 2020b, MNRAS, 498, L119, doi: 10.1093/mnrasl/slaa136
  * Gaidos et al. (2022) Gaidos, E., Hirano, T., Beichman, C., et al. 2022, MNRAS, 509, 2969, doi: 10.1093/mnras/stab3107
  * Gaidos et al. (2023) Gaidos, E., Hirano, T., Lee, R. A., et al. 2023, MNRAS, 518, 3777, doi: 10.1093/mnras/stac3301
  * Ginzburg et al. (2018) Ginzburg, S., Schlichting, H. E., & Sari, R. 2018, MNRAS, 476, 759, doi: 10.1093/mnras/sty290
  * Goldberg (1939) Goldberg, L. 1939, ApJ, 89, 673, doi: 10.1086/144092
  * Gossage et al. (2018) Gossage, S., Conroy, C., Dotter, A., et al. 2018, ApJ, 863, 67, doi: 10.3847/1538-4357/aad0a0
  * Guilluy et al. (2020) Guilluy, G., Andretta, V., Borsa, F., et al. 2020, A&A, 639, A49, doi: 10.1051/0004-6361/202037644
  * Gullikson (2014) Gullikson, K. 2014, TelFit: Fitting the telluric absorption spectrum.  http://ascl.net/1405.002
  * Gullikson et al. (2014) Gullikson, K., Dodson-Robinson, S., & Kraus, A. 2014, AJ, 148, 53, doi: 10.1088/0004-6256/148/3/53
  * Hartmann et al. (1979) Hartmann, L., Davis, R., Dupree, A. K., et al. 1979, ApJ, 233, L69, doi: 10.1086/183078
  * Hearty et al. (2014) Hearty, F., Levi, E., Nelson, M., et al. 2014, in Society of Photo-Optical Instrumentation Engineers (SPIE) Conference Series, Vol. 9147, Ground-based and Airborne Instrumentation for Astronomy V, ed. S. K. Ramsay, I. S. McLean, & H. Takami, 914752, doi: 10.1117/12.2056720
  * Hedges et al. (2021) Hedges, C., Hughes, A., Zhou, G., et al. 2021, AJ, 162, 54, doi: 10.3847/1538-3881/ac06cd
  * Hirano et al. (2020) Hirano, T., Krishnamurthy, V., Gaidos, E., et al. 2020, ApJ, 899, L13, doi: 10.3847/2041-8213/aba6eb
  * Hunter (2007) Hunter, J. D. 2007, Computing in Science and Engineering, 9, 90, doi: 10.1109/MCSE.2007.55
  * Husser et al. (2013) Husser, T. O., Wende-von Berg, S., Dreizler, S., et al. 2013, A&A, 553, A6, doi: 10.1051/0004-6361/201219058
  * Ikoma & Hori (2012) Ikoma, M., & Hori, Y. 2012, ApJ, 753, 66, doi: 10.1088/0004-637X/753/1/66
  * Jackson et al. (2012) Jackson, A. P., Davis, T. A., & Wheatley, P. J. 2012, MNRAS, 422, 2024, doi: 10.1111/j.1365-2966.2012.20657.x
  * Jin et al. (2014) Jin, S., Mordasini, C., Parmentier, V., et al. 2014, ApJ, 795, 65, doi: 10.1088/0004-637X/795/1/65
  * Johnson et al. (2022) Johnson, M. C., David, T. J., Petigura, E. A., et al. 2022, AJ, 163, 247, doi: 10.3847/1538-3881/ac6271
  * Johnstone et al. (2015) Johnstone, C. P., Güdel, M., Stökl, A., et al. 2015, ApJ, 815, L12, doi: 10.1088/2041-8205/815/1/L12
  * Kanodia & Wright (2018) Kanodia, S., & Wright, J. 2018, Research Notes of the American Astronomical Society, 2, 4, doi: 10.3847/2515-5172/aaa4b7
  * Kanodia et al. (2020) Kanodia, S., Cañas, C. I., Stefansson, G., et al. 2020, ApJ, 899, 29, doi: 10.3847/1538-4357/aba0a2
  * Kanodia et al. (2022) Kanodia, S., Libby-Roberts, J., Cañas, C. I., et al. 2022, AJ, 164, 81, doi: 10.3847/1538-3881/ac7c20
  * Kaplan et al. (2019) Kaplan, K. F., Bender, C. F., Terrien, R. C., et al. 2019, in Astronomical Society of the Pacific Conference Series, Vol. 523, Astronomical Data Analysis Software and Systems XXVII, ed. P. J. Teuben, M. W. Pound, B. A. Thomas, & E. M. Warner, 567
  * Kasper et al. (2020) Kasper, D., Bean, J. L., Oklopčić, A., et al. 2020, AJ, 160, 258, doi: 10.3847/1538-3881/abbee6
  * Katsova & Shcherbakov (1998) Katsova, M. M., & Shcherbakov, A. G. 1998, A&A, 329, 1080
  * Kirk et al. (2020) Kirk, J., Alam, M. K., López-Morales, M., & Zeng, L. 2020, AJ, 159, 115, doi: 10.3847/1538-3881/ab6e66
  * Kirk et al. (2022) Kirk, J., Dos Santos, L. A., López-Morales, M., et al. 2022, AJ, 164, 24, doi: 10.3847/1538-3881/ac722f
  * Kluyver et al. (2016) Kluyver, T., Ragan-Kelley, B., Pérez, F., et al. 2016, in Positioning and Power in Academic Publishing: Players, Agents and Agendas, ed. F. Loizides & B. Scmidt (IOS Press), 87–90.  https://eprints.soton.ac.uk/403913/
  * Kobanov et al. (2018) Kobanov, N., Chelpanov, A., & Pulyaev, V. 2018, Journal of Atmospheric and Solar-Terrestrial Physics, 173, 50, doi: 10.1016/j.jastp.2018.04.007
  * Kramida et al. (2021) Kramida, A., Yu. Ralchenko, Reader, J., & and NIST ASD Team. 2021, NIST Atomic Spectra Database (ver. 5.9), [Online]. Available: https://physics.nist.gov/asd [2022, May 13]. National Institute of Standards and Technology, Gaithersburg, MD.
  * Krishnamurthy et al. (2021) Krishnamurthy, V., Hirano, T., Stefánsson, G., et al. 2021, AJ, 162, 82, doi: 10.3847/1538-3881/ac0d57
  * Krolikowski et al. (2021) Krolikowski, D. M., Kraus, A. L., & Rizzuto, A. C. 2021, AJ, 162, 110, doi: 10.3847/1538-3881/ac0632
  * Kubyshkina et al. (2018) Kubyshkina, D., Lendl, M., Fossati, L., et al. 2018, A&A, 612, A25, doi: 10.1051/0004-6361/201731816
  * Lammer et al. (2003) Lammer, H., Selsis, F., Ribas, I., et al. 2003, ApJ, 598, L121, doi: 10.1086/380815
  * Lammer et al. (2014) Lammer, H., Stökl, A., Erkaev, N. V., et al. 2014, MNRAS, 439, 3225, doi: 10.1093/mnras/stu085
  * Lecavelier Des Etangs et al. (2010) Lecavelier Des Etangs, A., Ehrenreich, D., Vidal-Madjar, A., et al. 2010, A&A, 514, A72, doi: 10.1051/0004-6361/200913347
  * Liu et al. (2015) Liu, S.-F., Hori, Y., Lin, D. N. C., & Asphaug, E. 2015, ApJ, 812, 164, doi: 10.1088/0004-637X/812/2/164
  * Livingston et al. (2018) Livingston, J. H., Dai, F., Hirano, T., et al. 2018, AJ, 155, 115, doi: 10.3847/1538-3881/aaa841
  * Lopez & Fortney (2013) Lopez, E. D., & Fortney, J. J. 2013, ApJ, 776, 2, doi: 10.1088/0004-637X/776/1/2
  * Lopez et al. (2012) Lopez, E. D., Fortney, J. J., & Miller, N. 2012, ApJ, 761, 59, doi: 10.1088/0004-637X/761/1/59
  * Mahadevan et al. (2012) Mahadevan, S., Ramsey, L., Bender, C., et al. 2012, in Society of Photo-Optical Instrumentation Engineers (SPIE) Conference Series, Vol. 8446, Ground-based and Airborne Instrumentation for Astronomy IV, ed. I. S. McLean, S. K. Ramsay, & H. Takami, 84461S, doi: 10.1117/12.926102
  * Mahadevan et al. (2014) Mahadevan, S., Ramsey, L. W., Terrien, R., et al. 2014, in Society of Photo-Optical Instrumentation Engineers (SPIE) Conference Series, Vol. 9147, Ground-based and Airborne Instrumentation for Astronomy V, ed. S. K. Ramsay, I. S. McLean, & H. Takami, 91471G, doi: 10.1117/12.2056417
  * Mann et al. (2016) Mann, A. W., Gaidos, E., Mace, G. N., et al. 2016, ApJ, 818, 46, doi: 10.3847/0004-637X/818/1/46
  * Mann et al. (2017) Mann, A. W., Gaidos, E., Vanderburg, A., et al. 2017, AJ, 153, 64, doi: 10.1088/1361-6528/aa5276
  * Mann et al. (2018) Mann, A. W., Vanderburg, A., Rizzuto, A. C., et al. 2018, AJ, 155, 4, doi: 10.3847/1538-3881/aa9791
  * Mann et al. (2020) Mann, A. W., Johnson, M. C., Vanderburg, A., et al. 2020, AJ, 160, 179, doi: 10.3847/1538-3881/abae64
  * Martín et al. (2018) Martín, E. L., Lodieu, N., Pavlenko, Y., & Béjar, V. J. S. 2018, ApJ, 856, 40, doi: 10.3847/1538-4357/aaaeb8
  * Martin (1960) Martin, W. C. 1960, J. Res. Natl. Bur. Stand. (U.S.), Sect. A, 64, 19, doi: 10.6028/jres.064A.003
  * Matsakos et al. (2015) Matsakos, T., Uribe, A., & Königl, A. 2015, A&A, 578, A6, doi: 10.1051/0004-6361/201425593
  * Mauas et al. (2005) Mauas, P. J. D., Andretta, V., Falchi, A., et al. 2005, ApJ, 619, 604, doi: 10.1086/426428
  * Metcalf et al. (2019) Metcalf, A. J., Anderson, T., Bender, C. F., et al. 2019, Optica, 6, 233, doi: 10.1364/OPTICA.6.000233
  * Micela et al. (1985) Micela, G., Sciortino, S., Serio, S., et al. 1985, ApJ, 292, 172, doi: 10.1086/163143
  * Neff et al. (1986) Neff, J. E., Linsky, J. L., Landsman, W. B., & Carpenter, K. G. 1986, in ESA Special Publication, Vol. 263, New Insights in Astrophysics. Eight Years of UV Astronomy with IUE, ed. E. J. Rolfe & R. Wilson, 669
  * Newton et al. (2021) Newton, E. R., Mann, A. W., Kraus, A. L., et al. 2021, AJ, 161, 65, doi: 10.3847/1538-3881/abccc6
  * Newton et al. (2022) Newton, E. R., Rampalli, R., Kraus, A. L., et al. 2022, AJ, 164, 115, doi: 10.3847/1538-3881/ac8154
  * Ninan et al. (2018) Ninan, J. P., Bender, C. F., Mahadevan, S., et al. 2018, in Society of Photo-Optical Instrumentation Engineers (SPIE) Conference Series, Vol. 10709, High Energy, Optical, and Infrared Detectors for Astronomy VIII, ed. A. D. Holland & J. Beletic, 107092U, doi: 10.1117/12.2312787
  * Ninan et al. (2020) Ninan, J. P., Stefansson, G., Mahadevan, S., et al. 2020, ApJ, 894, 97, doi: 10.3847/1538-4357/ab8559
  * Nortmann et al. (2018) Nortmann, L., Pallé, E., Salz, M., et al. 2018, Science, 362, 1388, doi: 10.1126/science.aat5348
  * Núñez et al. (2015) Núñez, A., Agüeros, M. A., Covey, K. R., et al. 2015, ApJ, 809, 161, doi: 10.1088/0004-637X/809/2/161
  * Öberg et al. (2011) Öberg, K. I., Murray-Clay, R., & Bergin, E. A. 2011, ApJ, 743, L16, doi: 10.1088/2041-8205/743/1/L16
  * Obrien & Lambert (1986) Obrien, George T., J., & Lambert, D. L. 1986, ApJS, 62, 899, doi: 10.1086/191160
  * Oklopčić (2019) Oklopčić, A. 2019, ApJ, 881, 133, doi: 10.3847/1538-4357/ab2f7f
  * Oklopčić & Hirata (2018) Oklopčić, A., & Hirata, C. M. 2018, ApJ, 855, L11, doi: 10.3847/2041-8213/aaada9
  * Oliva et al. (2015) Oliva, E., Origlia, L., Scuderi, S., et al. 2015, A&A, 581, A47, doi: 10.1051/0004-6361/201526291
  * Orell-Miquel et al. (2022) Orell-Miquel, J., Murgas, F., Pallé, E., et al. 2022, A&A, 659, A55, doi: 10.1051/0004-6361/202142455
  * Owen & Mohanty (2016) Owen, J. E., & Mohanty, S. 2016, MNRAS, 459, 4088, doi: 10.1093/mnras/stw959
  * Owen & Wu (2013) Owen, J. E., & Wu, Y. 2013, ApJ, 775, 105, doi: 10.1088/0004-637X/775/2/105
  * Owen & Wu (2017) —. 2017, ApJ, 847, 29, doi: 10.3847/1538-4357/aa890a
  * Palle et al. (2020) Palle, E., Nortmann, L., Casasayas-Barris, N., et al. 2020, A&A, 638, A61, doi: 10.1051/0004-6361/202037719
  * Perez & Granger (2007) Perez, F., & Granger, B. E. 2007, Computing in Science and Engineering, 9, 21, doi: 10.1109/MCSE.2007.53
  * Pierrehumbert & Gaidos (2011) Pierrehumbert, R., & Gaidos, E. 2011, ApJ, 734, L13, doi: 10.1088/2041-8205/734/1/L13
  * Pizzolato et al. (2003) Pizzolato, N., Maggio, A., Micela, G., Sciortino, S., & Ventura, P. 2003, A&A, 397, 147, doi: 10.1051/0004-6361:20021560
  * Poppenhaeger (2022) Poppenhaeger, K. 2022, MNRAS, doi: 10.1093/mnras/stac507
  * Preibisch & Feigelson (2005) Preibisch, T., & Feigelson, E. D. 2005, ApJS, 160, 390, doi: 10.1086/432094
  * Rackham et al. (2018) Rackham, B. V., Apai, D., & Giampapa, M. S. 2018, ApJ, 853, 122, doi: 10.3847/1538-4357/aaa08c
  * Reback et al. (2020) Reback, J., McKinney, W., Jbrockmendel, et al. 2020, pandas-dev/pandas: Pandas 1.1.1, v1.1.1, Zenodo, doi: 10.5281/zenodo.3993412
  * Reiners (2012) Reiners, A. 2012, Living Reviews in Solar Physics, 9, 1, doi: 10.12942/lrsp-2012-1
  * Reiners et al. (2018) Reiners, A., Zechmeister, M., Caballero, J. A., et al. 2018, A&A, 612, A49, doi: 10.1051/0004-6361/201732054
  * Rizzuto et al. (2018) Rizzuto, A. C., Vanderburg, A., Mann, A. W., et al. 2018, AJ, 156, 195, doi: 10.3847/1538-3881/aadf37
  * Salz et al. (2018) Salz, M., Czesla, S., Schneider, P. C., et al. 2018, A&A, 620, A97, doi: 10.1051/0004-6361/201833694
  * Sanz-Forcada & Dupree (2008) Sanz-Forcada, J., & Dupree, A. K. 2008, A&A, 488, 715, doi: 10.1051/0004-6361:20078501
  * Schneider et al. (1998) Schneider, J., Rauer, H., Lasota, J. P., Bonazzola, S., & Chassefiere, E. 1998, in Astronomical Society of the Pacific Conference Series, Vol. 134, Brown Dwarfs and Extrasolar Planets, ed. R. Rebolo, E. L. Martin, & M. R. Zapatero Osorio, 241
  * Seager & Sasselov (2000) Seager, S., & Sasselov, D. D. 2000, ApJ, 537, 916, doi: 10.1086/309088
  * Shetrone et al. (2007) Shetrone, M., Cornell, M. E., Fowler, J. R., et al. 2007, PASP, 119, 556, doi: 10.1086/519291
  * Shields et al. (2016) Shields, A. L., Ballard, S., & Johnson, J. A. 2016, Phys. Rep., 663, 1, doi: 10.1016/j.physrep.2016.10.003
  * Spake et al. (2021) Spake, J. J., Oklopčić, A., & Hillenbrand, L. A. 2021, AJ, 162, 284, doi: 10.3847/1538-3881/ac178a
  * Spake et al. (2018) Spake, J. J., Sing, D. K., Evans, T. M., et al. 2018, Nature, 557, 68, doi: 10.1038/s41586-018-0067-5
  * Spake et al. (2022) Spake, J. J., Oklopčić, A., Hillenbrand, L. A., et al. 2022, ApJ, 939, L11, doi: 10.3847/2041-8213/ac88c9
  * Stefansson et al. (2016) Stefansson, G., Hearty, F., Robertson, P., et al. 2016, ApJ, 833, 175, doi: 10.3847/1538-4357/833/2/175
  * Stefansson et al. (2020) Stefansson, G., Cañas, C., Wisniewski, J., et al. 2020, AJ, 159, 100, doi: 10.3847/1538-3881/ab5f15
  * Tofflemire et al. (2021) Tofflemire, B. M., Rizzuto, A. C., Newton, E. R., et al. 2021, AJ, 161, 171, doi: 10.3847/1538-3881/abdf53
  * Tran et al. (2021) Tran, Q. H., Bowler, B. P., Cochran, W. D., et al. 2021, AJ, 161, 173, doi: 10.3847/1538-3881/abe041
  * van der Walt et al. (2011) van der Walt, S., Colbert, S. C., & Varoquaux, G. 2011, Computing in Science and Engineering, 13, 22, doi: 10.1109/MCSE.2011.37
  * Vanderburg et al. (2018) Vanderburg, A., Mann, A. W., Rizzuto, A., et al. 2018, AJ, 156, 46, doi: 10.3847/1538-3881/aac894
  * Vidal-Madjar et al. (2003) Vidal-Madjar, A., Lecavelier des Etangs, A., Désert, J. M., et al. 2003, Nature, 422, 143, doi: 10.1038/nature01448
  * Virtanen et al. (2020) Virtanen, P., Gommers, R., Oliphant, T. E., et al. 2020, Nature Methods, 17, 261, doi: 10.1038/s41592-019-0686-2
  * Vissapragada et al. (2021) Vissapragada, S., Stefánsson, G., Greklek-McKeon, M., et al. 2021, AJ, 162, 222, doi: 10.3847/1538-3881/ac1bb0
  * Wang & Dai (2021) Wang, L., & Dai, F. 2021, ApJ, 914, 98, doi: 10.3847/1538-4357/abf1ee
  * Wolff & Heasley (1984) Wolff, S. C., & Heasley, J. N. 1984, PASP, 96, 231, doi: 10.1086/131326
  * Wolfgang & Lopez (2015) Wolfgang, A., & Lopez, E. 2015, ApJ, 806, 183, doi: 10.1088/0004-637X/806/2/183
  * Wright & Eastman (2014) Wright, J. T., & Eastman, J. D. 2014, PASP, 126, 838, doi: 10.1086/678541
  * Zarro & Zirin (1986) Zarro, D. M., & Zirin, H. 1986, ApJ, 304, 365, doi: 10.1086/164170
  * Zhang et al. (2022a) Zhang, M., Knutson, H. A., Dai, F., et al. 2022a, arXiv e-prints, arXiv:2207.13099.  https://arxiv.org/abs/2207.13099
  * Zhang et al. (2022b) Zhang, M., Knutson, H. A., Wang, L., Dai, F., & Barragán, O. 2022b, AJ, 163, 67, doi: 10.3847/1538-3881/ac3fa7
  * Zhang et al. (2021) Zhang, M., Knutson, H. A., Wang, L., et al. 2021, AJ, 161, 181, doi: 10.3847/1538-3881/abe382
  * Zhang et al. (2022c) —. 2022c, AJ, 163, 68, doi: 10.3847/1538-3881/ac3f3b
  * Zirin (1975) Zirin, H. 1975, ApJ, 199, L63, doi: 10.1086/181849
  * Zirin (1976) —. 1976, ApJ, 208, 414, doi: 10.1086/154621


syteal bpgbrown rtpurple lpkviolet rjorange 11institutetext: Massachusetts
Institute of Technology, Cambridge, MA 02139, USA
11email: {shaoyuan, rhjiang, graesdal, lpk<EMAIL_ADDRESS>
# GCS*: Forward Heuristic Search on Implicit Graphs of Convex Sets

Shao Yuan Chew Chia  Rebecca H. Jiang  Bernhard Paus Graesdal
Leslie Pack Kaelbling  Russ Tedrake

###### Abstract

We consider large-scale, implicit-search-based solutions to the Shortest Path
Problems on Graphs of Convex Sets (GCS). We propose GCS*, a forward heuristic
search algorithm that generalizes A* search to the GCS setting, where a
continuous-valued decision is made at each graph vertex, and constraints
across graph edges couple these decisions, influencing costs and feasibility.
Such mixed discrete-continuous planning is needed in many domains, including
motion planning around obstacles and planning through contact. This setting
provides a unique challenge for best-first search algorithms: the cost and
feasibility of a path depend on continuous-valued points chosen along the
entire path. We show that by pruning paths that are cost-dominated over their
entire terminal vertex, GCS* can search efficiently while still guaranteeing
cost optimality and completeness. To find satisficing solutions quickly, we
also present a complete but suboptimal variation, pruning instead
reachability-dominated paths. We implement these checks using polyhedral-
containment or sampling-based methods. The sampling-based implementation is
probabilistically complete and asymptotically cost optimal, and performs
effectively even with minimal samples in practice. We demonstrate GCS* on
planar pushing tasks where the combinatorial explosion of contact modes
renders prior methods intractable and show it performs favorably compared to
the state-of-the-art. Project website: shaoyuan.cc/research/gcs-star/

###### Keywords:

Algorithmic Completeness and Complexity Task Planning Motion and Path Planning
Manipulation.

To suppress all color comments (to see how things will actually be laid out)
uncomment second line in preamble/preamble_shaoyuan.tex

##  1 Introduction

Many real-world planning problems involve making discrete and continuous
decisions jointly. Collision-free motion planning selects whether to go left
or right around an obstacle along with a continuous trajectory to do so. In
Task and Motion Planning (TAMP), discrete task-level decisions about the type
and sequence of actions are intimately coupled with continuous robot motions
and object configurations. For example, where a robot grasps a hockey stick
impacts its ability to hook an object [10]; in the construction of a tower,
the order in which materials are assembled, as well as their geometric
relationships, affect stability [6, 19].

Figure 1:  An $\epsilon$-suboptimal solution found in 21.9s by GCS* using
sampling-based ReachesCheaper domination checks on the STACK planar pushing
task. STACK is formulated as a GCS problem with approximately $1.3\times
10^{9}$ vertices and up to $8.5\times 10^{17}$ edges.

A natural representation of such a problem structure is the Graph of Convex
Sets (GCS) [12]. In a GCS, graph vertices represent these discrete choices,
with edges denoting allowed transitions. A continuous decision within each
vertex is encoded by a point constrained to lie within a convex set associated
with the vertex. For example, the free configuration space around an obstacle
may be (sometimes approximately) decomposed into a number of intersecting
convex regions in which trajectory segments can be planned. The point
associated with each vertex, which encodes the motion within the corresponding
free-space region, could be, for example, a sequence of positions at a fixed
number of knot points [11]. In the case of a single knot point, the associated
vertex’s convex set is simply the corresponding convex free-space set. In the
case of more knot points, the vertex’s set would be the higher-dimensional set
in which each knot point is contained within the free-space set. In this
example, intersecting free-space regions lead to graph edges. A valid way to
get around the obstacle involves a path – a sequence of vertices – through the
graph, along with a trajectory – a sequence of continuous-valued points
assigned to vertices on the path. Importantly, in such problems, there may be
constraints coupling points across edges, presenting a fundamental difference
from classical graph search problems. For example, for trajectory continuity,
knot points across an edge must coincide.

While Shortest-Path Problems (SPPs) in discrete graphs can be solved in
polynomial time, SPPs in GCS are NP-hard [12]. Small problems can be solved
with a Mixed-Integer Convex Program (MICP) formulation, and Marcucci et al.
improve scalability via a tight convex relaxation [12]. However, these
approaches become intractable as the graph grows. Many discrete-continuous
problems exhibit a combinatorial explosion of discrete modes, e.g., in a
manipulation task where contact dynamics between every pair of bodies are
considered. In TAMP problems, the search space is typically so large that
explicit enumeration of the search space is impractical.

(a)

(b)

Figure 2: An abstract discrete graph (a) and GCS (b), for example, mapping to
the path planning problem for a hopping robot tasked to hop from $s$ to $t$
across stepping stones. Arrows are edges between vertices. Vertices are
represented as black dots in (a) and via their (blue) convex sets in (b).
White dots are trajectory points on the GCS. Edge costs are the Euclidean
distances traversed between vertices. A discrete graph (a) has the optimal
substructure property while a GCS (b) does not. In (a), the optimal path
between $s$ and $C$ (red) is a sub-path of the optimal path between $s$ and
$t$ (green), which goes through $C$. In (b), the optimal path between $s$ and
$C$ is not a sub-path of the optimal path between $s$ and $t$, which goes
through $C$.

For such problems, we can instead define a graph implicitly via a source
vertex and a successor operator that acts on a vertex and returns all outgoing
edges. Implicit graph search algorithms use this representation to solve SPPs
without loading the entire graph in memory. A* is a notable implicit graph
search algorithm for SPP on discrete graphs [5]. However, key differences
between discrete graphs and GCS make search in GCS more challenging. TODO I
think she should pluralize "SPPs" when used in a plural context

The SPP on a discrete graph has the optimal substructure property [1]: All
sub-paths of an optimal path between two vertices are themselves optimal paths
between the first and last vertices they traverse. However, the SPP on a GCS
lacks this property. Importantly, as exemplified in fig. 2, a sub-path of an
optimal path on a GCS is not necessarily the cheapest way to go between that
subpath’s first and last vertices. Thus, when considering whether a partial
path to the target could be a sub-path of an optimal path, it is insufficient
to simply ask whether it is the cheapest way to get to its terminal vertex. A*
(or any best-first search algorithm) on a discrete graph prunes a path when
its cost-to-come is dominated by another path terminating in the same vertex
because it thus cannot be a sub-path of an optimal path [17]. However, a naive
application of A* to GCS could prune an important sub-path, preventing the
optimal path from being returned. We leverage the key insight that, while the
optimal substructure property does not hold on the vertex or convex-set level
in GCS, it does hold on the point level: Given an optimal path and trajectory,
any sub-path is optimal to reach its own final point. We allow paths to visit
repeated vertices in order to maintain this property [13]. The point-level
optimal substructure property holds because the optimal way to reach a point
is independent of the optimal way to continue on from that point. However,
this is only true if visiting a particular vertex on the way to that point
does not exclude that vertex from being visited when continuing on. It is
well-known that shortest paths on discrete graphs never revisit vertices [1];
however, this property does not hold on a GCS because a different point may be
visited each time. For readability, we continue to use the term “path” even
though this term traditionally excludes cycles.

More severe than the loss of optimality is the loss of completeness. Because a
GCS may have constraints coupling points across edges, a path may not be
feasible even if all edges connect the sequence of vertices. As a result, A*
on a GCS might prune a candidate path even though the cheaper alternate path
is infeasible for reaching the target. An example is shown in fig. 3.

Figure 3:  A contrived example of a GCS where points spanning edges $(s,A)$,
$(s,B)$, and $(C,t)$ are constrained to be vertically aligned, and points
spanning $(A,C)$ and $(B,C)$ horizontally aligned. A* prunes $[s,B,C]$ since
it has higher cost-to-come than $[s,A,C]$. However, $[s,A,C,t]$ is infeasible
(red), while $[s,B,C,t]$ is feasible (green).

In this paper, we address these challenges and generalize A* to the GCS
setting. We call our algorithm GCS* (sec. 4), leveraging two key insights.
First, to retain completeness, the algorithm must keep track of every path
that reaches previously unreached points within a set. Second, to retain
optimality, the algorithm must keep track of every path that reaches any point
in a set more cheaply than alternate paths. These insights give rise to
domination checks we call ReachesCheaper and ReachesNew (sec. 4.2.2). Using
ReachesCheaper with an admissible heuristic makes GCS* optimal, while using
ReachesNew prunes paths more aggressively, sacrificing optimality for speed,
but retaining completeness 111Similar to A* [17], GCS* is complete if a
solution exists or the user defines a limit on the length of paths that can be
returned.. We present polyhedral-containment-based implementations of these
checks, and prove completeness and optimality of GCS* (sec. 4.3.4). However,
we achieve significant speed improvements with only mildly suboptimal results
with sampling-based implementations, under which GCS* is complete and optimal
in the limit of infinite samples.

Finally, we formulate planar pushing as a GCS problem in sec. 5, and share
GCS* results for tasks with multiple moving objects and multi-step solutions
in sec. 6. Such problems are notorious for combinatorially exploding discrete
options. For example, the STACK task (fig. 1) leads to approximately $10^{9}$
sets and up to $10^{18}$ edges, making it wildly intractable for methods that
require loading the graph into memory, but sampling-based GCS* finds a
solution in 21.9 seconds. Two key strengths of the best-first search framework
extend to GCS*. First, GCS* is able to solve huge problems that are
intractable for methods which require explicit construction of the graph.
Second, search effort is proportional to query difficulty. In contrast,
methods that solve the problem as a single optimization program require full
effort regardless of query difficulty. Similar to A*, GCS* also guides the
search using a priority function that sums the cost-to-come and heuristic
cost-to-go. Exactly as in weighted A*, inflation of the heuristic can be used
to trade-off bounded sub-optimality with speed [16].

##  2 Related Work

We overview three approaches to discrete-continuous planning: sampling-based,
optimization-based, and hybrid methods that combine graph search with sampling
or optimization. Sampling-based methods, like Rapidly-exploring Random Trees
[9] and Probabilistic Roadmap Method [8] were developed for path planning with
obstacles. Unlike trajectory optimization, which can fail to find feasible
solutions, sampling-based methods are typically probabilistically complete.
However, incorporating continuous differential constraints on discrete samples
is challenging, often making kinodynamic versions of these algorithms less
effective [11].

To achieve both the completeness of sampling-based methods and trajectory
optimization’s ease of handling continuous constraints, some have proposed
posing the entire problem as a single MICP, solving a convex relaxation, and
applying a cheap rounding step to recover a feasible solution. This strategy
has been applied to motion planning around obstacles for high-dimensional
robotic systems [11] and planar pushing of a single object [4]. We extend
these benefits to the class of problems too large be posed as a single
optimization program.

To solve large problems modelled implicitly, many TAMP algorithms perform a
tree search, alternating between choosing a discrete action and sampling
continuous action parameters [3]. However, using sampling to determine
feasibility of a sequence of abstract actions does not provide a guaranteed
answer in finite time. Other hybrid methods combine graph search with
optimization over continuous variables. Using optimization to set path
expansion priority allows continuous considerations to closely inform discrete
search. Logic-Geometric Programming [19, 20] uses non-linear programming (NLP)
to guide the search, but the non-convexity of NLP leads to the loss of
completeness and optimality. We build on these ideas, additionally requiring a
convex decomposition of configuration space. While this can be non-trivial to
generate [2, 21], convex optimization allows us to determine the feasibility
of a path in GCS absolutely, and perform domination checks that enable
completeness and global optimality.

INSATxGCS (IxG) and IxG* [15] adapt INterleaved Search and Trajectory
Optimization (INSAT) [14] for collision-free GCS motion planning [11]. These
algorithms demonstrated impressive speedups over the convex relaxation and
rounding approach [11] across a range of tasks including multi-arm assembly.
However, IxG is only complete under the assumption that all edge constraints
along any path in the graph can be satisfied. It does not address the
challenge that a path with individually feasible edge constraints can still be
jointly infeasible, as in fig. 3. Edge constraints add significant
expressivity to GCS and are heavily used, including by Marcucci et al. [11]
and Graesdal et al. [4]. While IxG* is complete without this assumption, it
uses IxG as a subroutine to generate an upper bound on the optimal cost of the
solution. Supposing IxG fails to find a feasible path, IxG* will use infinity
as a trivial upper bound. Then IxG* will not prune _any_ paths, rendering
large problems intractable. Furthermore, while the main search of IxG and IxG*
can be done on implicit graphs, both methods use a precomputed heuristic that
requires explicit graphs. In this paper we build on the this approach,
addressing the case where graphs are too large to be explicitly constructed.
For example, we estimate it would take 337 days just to explicitly generate
all the sets of the graph of the STACK task shown in fig. 1.

##  3 Problem Formulation

A GCS [12] is a directed graph $G:=(\mathcal{V},\mathcal{E})$ defined by a
(potentially infinite) set of vertices $\mathcal{V}$ and edges
$\mathcal{E}\subset\mathcal{V}^{2}$, where $(u,v)\in\mathcal{E}$ if the graph
allows transitions from vertex $u$ to vertex $v$: $v$ is a _successor_ of $u$.
Each vertex $v\in\mathcal{V}$ is paired with a compact convex set
$\mathcal{X}_{v}$. These sets may live in different spaces. We assume each
vertex has a finite number of successors. The graph is implicitly defined via
source vertex $s\in\mathcal{V}$ and operator Successors that, when applied to
a vertex $u$, returns all successors $v_{i}$ of $u$. We define a path $\bf{v}$
in graph $G$ as a sequence of vertices where $\mathbf{v}_{i}$ gives the $i$-th
vertex in the sequence and each $\mathbf{v}_{i+1}$ is a successor of
$\mathbf{v}_{i}$. Likewise, we define a trajectory $\mathbf{x}$ on path
$\mathbf{v}$Right now you don’t really define what $\mathbf{x}$ is. I would
write something like "Likewise, we define a trajectory of length $N$ as
$\mathbf{x}=(\mathbf{x}_{1},\ldots,\mathbf{x}_{N})$ where
$\mathbf{x}_{i}\in\mathcal{X}_{\mathbf{x}_{i}},\,i=1,\ldots,N$ as a sequence
of points in the sets associated with $\mathbf{v}$,
$\mathbf{x}_{i}\in\mathcal{X}_{\mathbf{v}_{i}}$. We also overload the indexing
of $\mathbf{x}$ to go by vertex, such that
$\mathbf{x}_{\mathbf{v}_{i}}:=\mathbf{x}_{i}$. We denote the first and last
vertices in the path $\bf{v}$ as $\mathbf{v}_{\text{start}}:=\mathbf{v}_{0}$
and $\mathbf{v}_{\text{end}}$ respectively, and the edges traversed by
$\bf{v}$ as $\mathcal{E}_{\bf{v}}$. The cost of edge $e:=(u,v)$ is determined
by a proper, closed, convex, positive, bounded-away-from-zero function of its
endpoints $c(\mathbf{x}_{u},\mathbf{x}_{v})$. The latter stipulation ensures
finite optimal paths when cycles are permitted. Each edge can additionally
have constraints $(\mathbf{x}_{u},\mathbf{x}_{v})\in\mathcal{X}_{e}$, where
$\mathcal{X}_{e}$ is a closed convex set. For source vertex $s\in\mathcal{V}$
and target vertex $t\in\mathcal{V}$, the SPP in GCS is

$\displaystyle\operatorname*{minimize}_{\mathbf{v},\>\mathbf{x}}\quad$
$\displaystyle\sum_{(u,v)\in\mathcal{E}_{\mathbf{v}}}c(\mathbf{x}_{u},\mathbf{x}_{v})$
(1a) $\displaystyle\operatorname{subject\,to}\quad$
$\displaystyle\mathbf{v}_{\text{start}}=s,\>\mathbf{v}_{\text{end}}=t,$ (1b)
$\displaystyle\mathbf{v}_{i+1}\in\Call{Successors}{\mathbf{v}_{i}}$
$\displaystyle\forall i\in[0,\text{end}-1],$ (1c)
$\displaystyle\mathbf{x}_{v}\in\mathcal{X}_{v},$ $\displaystyle\forall
v\in\mathbf{v},$ (1d)
$\displaystyle(\mathbf{x}_{u},\mathbf{x}_{v})\in\mathcal{X}_{e},$
$\displaystyle\forall e:=(u,v)\in\mathcal{E}_{\mathbf{v}}.$ (1e)

If $\mathbf{v}$ is fixed, prog. (1) becomes convex and easy to solve; we call
this program ConvexRestriction. Given $\mathbf{v}$, solving ConvexRestriction
determines its corresponding optimal cost and optimal trajectory $\mathbf{x}$.

##  4 Approach

###  4.1 Graph Search Formulation

A* (alg. 1) searches over paths from the source, pruning some and expanding
others based on estimated costs of extending to reach the target, until a path
reaches the target and is returned. In particular, paths await expansion in a
priority queue ${Q}$ ordered by total cost estimate $\tilde{f}$ (7). Often,
instead of specifying $\tilde{f}$ directly, a user equivalently provides
$\tilde{h}$, a heuristic that estimates costs-to-go from vertices to the
target, and $\tilde{f}$ is computed as the sum of $\tilde{h}$ and cost-to-come
$\tilde{g}$. A path is pruned via a domination check if its cost-to-come
$\tilde{g}$ is higher than that of the current best path reaching the same
vertex (8). This current best path for each vertex is stored in a dictionary
$S$ (9). Equivalently, $\tilde{f}$ can be compared since paths reaching the
same vertex share an $\tilde{h}$ value. We use the term “domination check”
generically to refer to a function that determines whether a candidate path
should be pruned or added to $Q$. We call a path expanded if it has been
popped from $Q$ (7). We call a vertex expanded if any path terminating at it
has been expanded.

GCS* (alg. 2) proceeds similarly. The key difference is the use of a different
domination check $\textsc{NotDominated}\in$
$\\{\textsc{ReachesCheaper},\textsc{ReachesNew}\\}$ (defined in sec. 4.2.2).
To facilitate these domination checks, GCS* maintains a dictionary ${S}$
containing a set of un-pruned paths reaching each vertex. This is in contrast
to A*, which stores a single path reaching each vertex. For any vertex
$v\in\mathcal{V}$, we define $f^{*}(v)$ as the optimal cost of prog. (1) with
the additional constraint that $v\in\mathbf{v}$ (var. $\mathbf{v}$ from prog.
(1)). That is, $f^{*}(v)$ is the cost of an optimal solution constrained to
visit vertex $v$. Similarly, for any point $x\in\mathcal{X}_{v}$ for some
$v\in\mathcal{V}$, we overload $f^{*}(x)$ to be the optimal cost of prog. (1)
with the additional constraint that $x\in\mathbf{x}$ (var. $\mathbf{x}$ from
prog. (1)). That is, $f^{*}(x)$ maybe $f_{v}(x)$ or $f(x,v)$? i want to make
sure it’s ok to have overlapping vertices. in the more general gcs, the
continuous variables in different sets may not even live in the same space. do
we want to support that here, too? is the cost of an optimal solution
constrained to visit continuous-valued point $x$. Let $g^{*}(x)$ be the
optimal cost-to-come to point $x$ from $s$, and $h^{*}(x)$ be the optimal
cost-to-go from $x$ to $t$, both infinite if infeasible. If
$x\in\mathcal{X}_{v}$ is on the optimal path through $v$, we have
$f^{*}(v)=g^{*}(x)+h^{*}(x)$. (We do allow overlapping vertices, and for the
continuous variables in different sets to not live in the same space. I’ll say
both of these points more explicitly in the problem formulation) and propose
adding the following line to further clarify: Note that $f^{*}(x)$,
$g^{*}(x)$, and $h^{*}(x)$ are well defined even if $x$ is contained in
(intersecting) sets corresponding to multiple vertices.

In general, $f^{*},g^{*},$ and $h^{*}$ cannot be computed without solving
prog. (1). Instead, we define computationally viable functions
$\tilde{f},\tilde{g}$ and $\tilde{h}$I would use another word than
‘accessible‘ and state that their purpose is to be computationally viable
replacements for the true ‘f‘, ‘g‘, and ‘h‘ or something like this.. A* uses a
heuristic function for estimating the cost-to-go from a vertex to the target.
Extending this to GCS*, we use a heuristic function $\tilde{h}(x)$ that
estimates the cost-to-go from a point $x$ to the target. We assume
$\tilde{h}\geq 0$. Furthermore, $\tilde{h}$ must be pointwise admissible (def.
1) in order for optimality guarantees (sec. 4.3.4) to hold. We extend the
classical definition of admissibility over vertices to a pointwise definition:

###### Definition 1

A heuristic function $\tilde{h}$ is pointwise admissible if

$\tilde{h}(x)\leq h^{*}(x),\quad\forall x\in\mathcal{X}_{v},\quad\forall
v\in\mathcal{V}.$

Next, we define $\tilde{g}$ and $\tilde{f}$. No longer referring to the
variables from prog. (1), let $\mathbf{v}$ be a candidate partial path to the
target such that $\mathbf{v}_{\text{start}}=s$, but $\mathbf{v}_{\text{end}}$
is not necessarily $t$. Let $\mathbf{x}^{*}(\mathbf{v})$ denote an optimal
trajectory through $\mathbf{v}$ subject to the heuristic $\tilde{h}$ assigning
the cost from $\mathbf{x}^{*}(\mathbf{v})_{\text{end}}$ to $t$. Then
$\tilde{g}(\mathbf{v},x)$ for $x\in\mathcal{X}_{\mathbf{v}_{\text{end}}}$ is
the optimal cost-to-come to $x$ via $\mathbf{v}$. Then,
$\tilde{f}(\mathbf{v}):=\tilde{g}(\mathbf{v},\mathbf{x}^{*}(\mathbf{v})_{\text{end}})+\tilde{h}(\mathbf{x}^{*}(\mathbf{v})_{\text{end}})$.
To evaluate the total cost estimate for a path restricted to pass through a
specific $x\in\mathcal{X}_{\mathbf{v}_{\text{end}}}$, we overload
$\tilde{f}(\mathbf{v},x):=\tilde{g}(\mathbf{v},x)+\tilde{h}(x)$.
ConvexRestriction can be used to evaluate $\tilde{g}(\mathbf{v},x)$ and
$\tilde{f}(\mathbf{v},x)$, and additionally $\mathbf{x}^{*}(\mathbf{v})$ and
$\tilde{f}(\mathbf{v})$ if $\tilde{h}$ is convex.  Algorithm 1 Discrete A*
1:Input: $s$, $t$, $\tilde{f}$, $\tilde{g}$, Successors 2: 3:Output: Path from
$s$ to $t$ or Fail 4:$\mathbf{v}=[s]$ 5: $S\leftarrow\\{s:\mathbf{v}\\}$
$\triangleright$ Dict. of paths  6:$Q\leftarrow\text{priority queue ordered by
}{\tilde{f}}$ 7:$Q.\Call{add}{\mathbf{v}}$ $Q\neq\emptyset$
8:$\mathbf{v}\leftarrow$ $Q.\Call{Pop}{}$ $\mathbf{v}_{\text{end}}$ = t
$\mathbf{v}$ $v^{\prime}\in\Call{Successors}{\mathbf{v}_{\text{end}}}$
9:$\mathbf{v}^{\prime}=[\mathbf{v},v^{\prime}]$ $v^{\prime}\notin S$ or
$\tilde{g}(\mathbf{v}^{\prime})<\tilde{g}(S[v^{\prime}])$
10:$S[v^{\prime}]=\mathbf{v}^{\prime}$ 11:$Q.\Call{Add}{\mathbf{v}^{\prime}}$
12:Fail 1:Input: $s$, $t$, $\tilde{f}$, NotDominated, Successors 2:Output:
Path from $s$ to $t$ or Fail 3:$\mathbf{v}=[s]$ 4:
$S\leftarrow\\{s:\\{\mathbf{v}\\}\\}$ $\triangleright$ Dict. of sets of paths
5:$Q\leftarrow\text{priority queue ordered by }{\tilde{f}}$
6:$Q.\Call{add}{\mathbf{v}}$ $Q\neq\emptyset$ 7:$\mathbf{v}\leftarrow$
$Q.\Call{Pop}{}$ $\mathbf{v}_{\text{end}}$ = t $\mathbf{v}$
$v^{\prime}\in\Call{Successors}{\mathbf{v}_{\text{end}}}$
8:$\mathbf{v}^{\prime}=[\mathbf{v},v^{\prime}]$
$\Call{NotDominated}{\mathbf{v}^{\prime},S[v^{\prime}]}$
9:$S[v^{\prime}].\Call{Add}{\mathbf{v}^{\prime}}$
10:$Q.\Call{Add}{\mathbf{v}^{\prime}}$ 11:Fail Algorithm 2 GCS*

###  4.2 Domination Checks

GCS* uses one of two domination checks, ReachesCheaper or ReachesNew. In
general, we compute approximate checks, as exact checks are not tractable.
Under exact or conservative ReachesCheaper checks, GCS* is optimal.
Satisficing solutions can be found more quickly using ReachesNew. Under exact
or conservative ReachesNew checks, GCS* is complete.

(a) $\textsc{RC}=\texttt{False},\>\textsc{RN}=\texttt{False}$

(b) $\textsc{RC}=\texttt{True},\>\textsc{RN}=\texttt{True}$

(c) $\textsc{RC}=\texttt{True},\>\textsc{RN}=\texttt{False}$

(d) $\textsc{RC}=\texttt{True},\>\textsc{RN}=\texttt{False}$ (e)
$\textsc{RC}=\texttt{False},\>\textsc{RN}=\texttt{False}$

Figure 4: A variety of scenarios for ReachesCheaper (RC) and ReachesNew (RN),
where the dimension of $\mathbf{v}_{\text{end}}$ is 1. The candidate path
$\mathbf{v}$ (pink), and alternate paths
$\\{\mathbf{v}^{(i)}|i\in\\{1,2,3\\}\\}$ (blue) are the 1st and 2nd arguments,
respectively. Points $x$ for which $\tilde{g}(\mathbf{v},x)$ is not drawn
(outside dashed lines) are unreachable via path $\mathbf{v}$ due to
constraints, and $\tilde{g}(\mathbf{v},x)=\infty$.

We say that a candidate path $\mathbf{v}$ ReachesCheaper if there is a point
$x\in\mathcal{X}_{\mathbf{v}_{\text{end}}}$ such that the cost-to-come via
$\mathbf{v}$ is lower than via all alternate paths $\mathbf{v}^{\prime}$
stored in $S$ that also terminate at $\mathbf{v}_{\text{end}}$, as in sec. 4.2
(b), (c), and (d):

$\displaystyle\Call{ReachesCheaper}{&\mathbf{v},S[\mathbf{v}_{\text{end}}]}\>:=\>\exists\,x\in\mathcal{X}_{\mathbf{v}_{\text{end}}}$
(1b)
$\displaystyle\text{s.t.}\>\tilde{g}(\mathbf{v},x)<\tilde{g}(\mathbf{v}^{\prime},x),\>\forall\,\mathbf{v}^{\prime}\in
S[\mathbf{v}_{\text{end}}].$

The intuition for such a check guaranteeing optimality (proven in sec. 4.3.4)
is that GCS* can only prune $\mathbf{v}$ if it is not the strictly cheapest
way found to reach any point in $\mathbf{v}_{\text{end}}$.

We say candidate path $\mathbf{v}$ ReachesNew if there is a point
$x\in\mathcal{X}_{\mathbf{v}_{\text{end}}}$ that is feasible to reach via
$\mathbf{v}$ and infeasible via all alternate paths $\mathbf{v}^{\prime}$
stored in $S$ that also terminate at $\mathbf{v}_{\text{end}}$, as in sec. 4.2
(b):

$\displaystyle\Call{ReachesNew}{&\mathbf{v},S[\mathbf{v}_{\text{end}}]}\>:=\>\exists\,x\in\mathcal{X}_{\mathbf{v}_{\text{end}}}$
(1c)
$\displaystyle\text{s.t.}\>\left(\tilde{g}(\mathbf{v},x)<\infty\right)\>\wedge\>\left(\tilde{g}(\mathbf{v}^{\prime},x)=\infty,\>\forall\,\mathbf{v}^{\prime}\in
S[\mathbf{v}_{\text{end}}]\right).$

The same intuition can be applied to understanding ReachesNew, but considering
how edge constraints along the entire path affect feasibility rather than
cost. Note that ReachesNew implies ReachesCheaper since the cost of any
feasible path is finite, but ReachesCheaper does not imply ReachesNew.

####  4.2.1 Sampling-based Implementation.

The simplest and fastest approach to approximating ReachesCheaper and
ReachesNew queries is to check whether sample points
$x\in\mathcal{X}_{\mathbf{v}_{\text{end}}}$ satisfy the conditions in eq. 1b
and eq. 1c respectively, returning True if any such point is found. These
approximations are not conservative: the condition that any qualifying point
is found is sufficient but not necessary; for example, in sec. 4.2 (d), sparse
sampling may miss the interval where $\tilde{g}(\mathbf{v},x)$ is lower than
the others, leading ReachesCheaper to return False, signaling that
$\mathbf{v}$ is dominated, when in fact it is not. Samples are drawn uniformly
in $\mathcal{X}_{\mathbf{v}_{\text{end}}}$ and then projected onto the
reachable set (defined formally in eq. 1d). Under this implementation, GCS* is
probabilistically complete and asymptotically optimal, i.e. complete and
optimal in the limit of infinite samples per check. We discuss this further in
sec. 4.3.4.

####  4.2.2 Polyhedral-containment-based Implementation.

We will see in sec. 4.3.4 that conservative (necessary but not sufficient)
approximations of ReachesNew and ReachesCheaper enable completeness and
optimality guarantees. We formulate such checks via polyhedral containment
queries. In part, these checks are conservative in that they check only single
domination: In contrast to sampling, they compare $\mathbf{v}$ to individual
elements in $S[\mathbf{v}_{\text{end}}]$. These checks can detect reachability
domination like in sec. 4.2 (a) and (c) and cost domination like in sec. 4.2
(a), but not reachability domination like in (d) or (e), or cost domination
like in (e). For convenience, we define the set $\mathcal{P}$ in which a
trajectory $\mathbf{x}$ corresponding to a path $\mathbf{v}$ must reside,

$\mathcal{P}_{\mathbf{v}}:=\\{\mathbf{x}\mid\mathbf{x}_{v}\in\mathcal{X}_{v}\
\forall\,v\in\mathbf{v},\>(\mathbf{x}_{u},\mathbf{x}_{v})\in\mathcal{X}_{(u,v)}\
\forall\,(u,v)\in\mathcal{E}_{\mathbf{v}}\\}.$

I think there are some problems with this mathematical notation. For instance,
it is not really clear what the relationship between $\mathbf{x}$ and
$\mathbf{x}_{v}$ is, also it is unclear whether you mean the same $v$ between
the vertices and edges (I assume you mean different $u$’s, but that is not
clear from its current form). This suggestion is not perfect and still needs
work, but perhaps something more along these lines could be better:

"..to a path $\mathbf{v}$ of length $N$ must reside:

$\mathcal{P}_{\mathbf{v}}:=\\{(\mathbf{x}_{v_{1}},\ldots,\mathbf{x}_{v_{N}})\mid\mathbf{x}_{v}\in\mathcal{X}_{v}\
\forall\,v\in\mathbf{v},\>(\mathbf{x}_{u},\mathbf{x}_{w})\in\mathcal{X}_{(u,w)}\
\forall\,(u,w)\in\mathcal{E}_{\mathbf{v}}\\}.$

Or perhaps just say with words that $\mathcal{P}_{\mathbf{v}}$ is defined as
the set of trajectories that satisfy all the vertex and edge constraints. For
a path $\mathbf{v}$, the set in $\mathcal{X}_{\mathbf{v}_{\text{end}}}$
reachable via $\mathbf{v}$ is

$\mathcal{S}_{\mathbf{v}}:=\\{x\in\mathcal{X}_{\mathbf{v}_{\text{end}}}\mid\exists\,\mathbf{x}\in\mathcal{P}_{\mathbf{v}}\text{
s.t. }x=\mathbf{x}_{\text{end}}\\}.$ (1d)

A necessary and sufficient check for ReachesNew is then

$\mathcal{S}_{\mathbf{v}}\nsubseteq\bigcup_{\mathbf{v}^{\prime}\in
S[\mathbf{v}_{\text{end}}]}\mathcal{S}_{\mathbf{v}^{\prime}}.$

The epigraph of the optimal cost-to-come over the set
$\mathcal{X}_{\mathbf{v}_{\text{end}}}$ is

$\mathcal{C}_{\mathbf{v}}:=\\{(x,\,l)\in\mathcal{X}_{\mathbf{v}_{\text{end}}}\times\mathbb{R}\mid\exists\,\mathbf{x}\in\mathcal{P}_{\mathbf{v}}\text{
s.t.
}x=\mathbf{x}_{\text{end}},\>l\geq\sum_{(u,v)\in\mathcal{E}_{\mathbf{v}}}c(\mathbf{x}_{u},\mathbf{x}_{v})\\}.$

It is confusing that $x$ is not bold font here, but is bold in other places. I
assume it because of the "implicit definition" of $\mathbf{x}$ as the
concatenation of the $\mathbf{x}_{v_{i}}$’s throughout the paper Also, I think
it is generally better to say what the set is before presenting it, rather
than saying "Now consider the set…" which does not really add anything to the
paper. So here I would write "Consider now the epigraph of the optimal cost-
to-come over the set $\mathcal{X}_{\mathbf{v}_{\text{end}}}$: " A necessary
and sufficient check for ReachesCheaper is then

$\mathcal{C}_{\mathbf{v}}\nsubseteq\bigcup_{\mathbf{v}^{\prime}\in
S[\mathbf{v}_{\text{end}}]}\mathcal{C}_{\mathbf{v}^{\prime}}.$

As the union over convex sets is not generally itself convex, these necessary
and sufficient checks are impractical. Instead, we use

as necessary conditions for ReachesNew and ReachesCheaper respectively, where
we have checked for domination with respect to individual other paths, rather
than by the collective of all other paths.

These checks are in general non-trivial because they involve comparing
projections of convex sets, which are expensive to compute explicitly. In our
examples, vertex sets are polytopic and costs are linear, which allows us to
use Sadraddini and Tedrake’s sufficient condition for containment of affine
transformations of polyhedra [18], evaluated by solving a convex program.
Using this condition when checking eq. 1ea and eq. 1eb,
$\Call{NotDominated}{\mathbf{v},S[\mathbf{v}_{\text{end}}]}$ is conservative:
If $\mathbf{v}$ is not dominated, it certainly returns True, but if
$\mathbf{v}$ is dominated, it may return True or False. In the case of more
general convex vertex sets and costs, Jones and Morari’s work on computing
inner and outer polytopic approximations of convex sets [7] can be used to
conservatively check containment.

###  4.3 Properties of the Algorithm

We prove that, under conservative NotDominated or specifically  ReachesCheaper
checks, GCS* is complete or optimal, respectively: The proofs rely on these
checks being necessary but not sufficient conditions for eq. 1c and eq. 1b.
Optimality also relies on $\tilde{h}$ being pointwise admissible (def. 1).

####  4.3.1 Completeness.

We show that if prog. (1) is feasible, GCS* returns a feasible path in finite
iterations. However, like A* [17], GCS* as stated cannot be assured to
terminate in finite iterations in the case where prog. (1) is infeasible and
the graph is infinite or cycles are permitted. GCS* is made complete in the
infeasible case by adding a limit on path length to prog. (1) and placing an
upper limit on the lengths of paths that get added to $S$ and $Q$ in 9 and 10.

###### Lemma 1

For any path $\mathbf{v}$ in $Q$, GCS* either expands $\mathbf{v}$ after a
finite number of iterations or returns a feasible path to the target first.

###### Proof

Path $\mathbf{v}$ has some finite estimated total cost
$\tilde{f}(\mathbf{v})$. Because all edge costs are positive and bounded away
from zero, there exist a finite number of paths $\mathbf{v}^{\prime}$ with
cost-to-come that do not exceed $\tilde{f}(\mathbf{v})$,
$\tilde{g}(\mathbf{v}^{\prime})\leq\tilde{f}(\mathbf{v})$. As
$\tilde{f}(\mathbf{v}^{\prime})\geq\tilde{g}(\mathbf{v}^{\prime})$, there are
a finite number of paths $\mathbf{v}^{\prime}$ ever added to $Q$ such that
$\tilde{f}(\mathbf{v}^{\prime})\leq\tilde{f}(\mathbf{v})$. As GCS* always
expands the path in $Q$ with the lowest $\tilde{f}$ value, GCS* will
eventually exhaust all such paths and expand $\mathbf{v}$, unless it
terminates with a feasible path to the target first.

###### Lemma 2

If $t$ is reachable from $s$, then some path that reaches it is added to $Q$
in finite iterations.

###### Proof

Suppose, for contradiction, that $t$ is reachable from $s$, but no path that
reaches it is added to $Q$ in finite iterations. There is some feasible path
$\mathbf{v}$ where $\mathbf{v}_{\text{start}}=s$, $\mathbf{v}_{\text{end}}=t$,
and some feasible trajectory $\mathbf{x}\in\mathcal{P}_{\mathbf{v}}$. Let $j$
be the largest index such that there exists a path $\mathbf{v}^{\prime}$
(which may or may not be a subpath of $\mathbf{v}$) terminating in
$\mathbf{v}_{j}$ ($\mathbf{v}^{\prime}_{\text{end}}=\mathbf{v}_{j}$) and
reaching $\mathbf{x}_{j}$, that ever gets added to $Q$. Such a $j\geq 0$
exists because $Q$ is initialized with the path containing just $s$. By lemma
1, $\mathbf{v}^{\prime}$ is expanded in finite iterations, unless a path
reaching $t$ is returned first. If a path reaching $t$ is returned first, we
achieve contradiction trivially. Consider instead the case where
$\mathbf{v}^{\prime}$ is expanded in finite iterations. By assumption,
$\mathbf{v}_{j+1}$ is a successor of $\mathbf{v}_{j}$, and so the extended
path $[\mathbf{v}^{\prime},\mathbf{v}_{j+1}]$ must be added to $Q$ when
$\mathbf{v}^{\prime}$ is expanded. We can be sure it is added to $Q$ instead
of being rejected by NotDominated in line 10 because
$(\mathbf{x}_{j},\mathbf{x}_{j+1})\in\mathcal{X}_{(\mathbf{v}_{j},\mathbf{v}_{j+1})}$,
making $\mathbf{x}_{j+1}$ reachable via the extended path
$[\mathbf{v}^{\prime},\mathbf{v}_{j+1}]$, and, by assumption, no path
terminating in $\mathbf{v}_{j+1}$ and reaching $\mathbf{x}_{j+1}$ has been
added to $Q$ prior, and thus nor has it been added to $S[\mathbf{v}_{j+1}]$,
as $S$ is only added to in conjunction with $Q$. However, the extended path
$[\mathbf{v}^{\prime},\mathbf{v}_{j+1}]$ is now a path in $Q$ terminating in
$\mathbf{v}_{j+1}$ and reaching $\mathbf{x}_{j+1}$, creating a contradiction
with the definition of $j$.

###### Theorem 4.1

GCS* returns a path from $s$ to $t$ in finite iterations if one exists.

###### Proof

By lemma 2, a feasible path from $s$ to $t$ is added to $Q$ in finite
iterations if one exists. By lemma 1, either this path is expanded in finite
iterations (in which case it is immediately returned), or another such
feasible path from $s$ to $t$ is returned first. In both cases, a path from
$s$ to $t$ is returned in finite iterations.

####  4.3.2 Cost Optimality.

In this section, we assume NotDominated is an exact or conservative check for
ReachesCheaper.

###### Lemma 3

Suppose prog. (1) is feasible and $\mathbf{v}$ is an optimal path. If $t$ is
unexpanded,

there is a path $\mathbf{v}^{\prime}$ in $Q$ with
$\mathbf{v}^{\prime}_{\text{end}}=\mathbf{v}_{i}$ for some $i$, such that
$\mathbf{v}^{\prime}$ attains the optimal cost-to-come to
$\mathbf{x}^{*}(\mathbf{v})_{i}$,
$\tilde{g}(\mathbf{v}^{\prime},\mathbf{x}^{*}(\mathbf{v})_{i})=g^{*}(\mathbf{x}^{*}(\mathbf{v})_{i})$.

###### Proof

If GCS* has not completed one iteration, the lemma holds trivially via the
path containing only $s$, $\mathbf{v}^{s}:=[s]\in Q$. Suppose instead
$\mathbf{v}^{s}$ has been expanded. Let $\mathbf{v}$ be any optimal path from
$s$ to $t$, and let $\mathbf{x}:=\mathbf{x}^{*}(\mathbf{v})$ be an optimal
trajectory through $\mathbf{v}$. Let $i$ be the largest index for which there
exists an expanded path $\mathbf{v}^{\prime}$ such that
$\mathbf{v}_{i}=\mathbf{v}^{\prime}_{\text{end}}$, with optimal cost-to-come
to $\mathbf{x}_{i}$,
$\tilde{g}(\mathbf{v}^{\prime},\mathbf{x}_{i})=g^{*}(\mathbf{x}_{i})$. That
is, the $i$th vertex on $\mathbf{v}$ has been expanded on some path
$\mathbf{v}^{\prime}$ which may or may not be a subpath of $\mathbf{v}$ but
attains the same (optimal) cost-to-come to $\mathbf{x}_{i}$. Since $s$ has
been expanded, and $t$ has not, $i\in[0,\text{end}-1]$. Because
$(\mathbf{x}_{i},\mathbf{x}_{i+1})$ is an edge on the optimal trajectory
$\mathbf{x}$, the extended path $[\mathbf{v}^{\prime},\mathbf{v}_{i+1}]$
reaches $\mathbf{x}_{i+1}$ with optimal cost-to-come:
$g^{*}(\mathbf{x}_{i+1})=g^{*}(\mathbf{x}_{i})+c(\mathbf{x}_{i},\mathbf{x}_{i+1})$.

We now argue that once $\mathbf{v}^{\prime}$ was expanded, there must have
been, at some point present or prior, a path added to $Q$ terminating at
$\mathbf{v}_{i+1}$ and reaching $\mathbf{x}_{i+1}$ with optimal cost-to-come.
Then, we argue that it is still in $Q$. When $\mathbf{v}^{\prime}$ was
expanded, the extended path $[\mathbf{v}^{\prime},\mathbf{v}_{i+1}]$ was
placed on $Q$ unless it was rejected by ReachesCheaper returning False in line
10. If ReachesCheaper returned False, then another path was already in
$S[\mathbf{v}_{i+1}]$ that terminated at $\mathbf{v}_{i+1}$ and reached
$\mathbf{x}_{i+1}$ with optimal cost-to-come. Paths are added to $S$ only when
added to $Q$, so this path was added to $Q$. By the definition of $i$,
whichever such path was at any point in $Q$ has not been expanded and is still
in $Q$.

###### Corollary 1

Suppose prog. (1) is feasible, and $\mathbf{v}$ is an optimal path from $s$ to
$t$. If alg. 2 has not terminated, there is some $\mathbf{v}^{\prime}$ in $Q$
such that $\mathbf{v}^{\prime}_{\text{end}}\in\mathbf{v}$ and the estimated
total cost for $\mathbf{v}^{\prime}$ does not exceed the true optimal cost,
$\tilde{f}(\mathbf{v}^{\prime})\leq f^{*}(s)$.

###### Proof

As alg. 2 terminates only when $t$ is expanded, $t$ is unexpanded under these
assumptions. By lemma 3, there is an unexpanded search path
$\mathbf{v}^{\prime}$ in $Q$ with
$\mathbf{v}^{\prime}_{\text{end}}=\mathbf{v}_{i}$ for some $i$ and an optimal
cost-to-come to $\mathbf{x}^{*}(\mathbf{v})_{i}$.

We know
$\tilde{f}(\mathbf{v}^{\prime})\leq\tilde{f}(\mathbf{v}^{\prime},\mathbf{x}^{*}(\mathbf{v})_{i})$
because,

more generally speaking, a path constrained to go through
$\mathbf{v}^{\prime}$ is estimated to be at least as cheap as a path
additionally constrained to go through any particular point.

Then,

$\displaystyle\tilde{f}(\mathbf{v}^{\prime},\mathbf{x}^{*}(\mathbf{v})_{i})$
$\displaystyle=\tilde{g}(\mathbf{v}^{\prime},\mathbf{x}^{*}(\mathbf{v})_{i})+\tilde{h}(\mathbf{x}^{*}(\mathbf{v})_{i})\quad\textnormal{(By
definition of $\tilde{f}$)}$
$\displaystyle=g^{*}(\mathbf{x}^{*}(\mathbf{v})_{i})+\tilde{h}(\mathbf{x}^{*}(\mathbf{v})_{i})\quad\textnormal{(By
lemma \ref{lemma:Q_has_node_with_optimal_cost_to_come})}$ $\displaystyle\leq
g^{*}(\mathbf{x}^{*}(\mathbf{v})_{i})+{h}^{*}(\mathbf{x}^{*}(\mathbf{v})_{i})\quad\textnormal{(By
$\tilde{h}$ pointwise admissibility)}$
$\displaystyle=f^{*}(\mathbf{x}^{*}(\mathbf{v})_{i})=f^{*}(s)\quad\textnormal{(Since
$\mathbf{x}^{*}(\mathbf{v})_{i}$ is on an optimal trajectory).}$

Thus, we have $\tilde{f}(\mathbf{v}^{\prime})\leq f^{*}(s).$

###### Theorem 4.2

GCS* is cost optimal: GCS* returns an optimal trajectory from $s$ to $t$ in
finite iterations if one exists.

###### Proof

By theorem 4.1, GCS* returns a feasible path $\mathbf{v}^{\prime}$ from $s$ to
$t$ in finite iterations. Suppose for contradiction that the cost-to-come to
$t$ through $\mathbf{v}^{\prime}$ is suboptimal,
$\tilde{g}(\mathbf{v}^{\prime},\mathbf{x}^{*}(\mathbf{v}^{\prime})_{\text{end}})>f^{*}(s)$.
By admissibility, $h^{*}(x)=\tilde{h}(x)=0\ \forall x\in\mathcal{X}_{t}$.
Thus,
$\tilde{f}(\mathbf{v}^{\prime})=\tilde{g}(\mathbf{v}^{\prime},\mathbf{x}^{*}(\mathbf{v}^{\prime})_{\text{end}})>f^{*}(s)$.
By corollary 1, just before termination, there existed a path $\mathbf{v}$ in
$Q$ with $\tilde{f}(\mathbf{v})\leq f^{*}(s)<\tilde{f}(\mathbf{v}^{\prime})$.
However, alg. 2 always pops the path in $Q$ with the lowest $\tilde{f}$ value,
and returns a path only after popping it. This contradicts the assumption that
$\mathbf{v}^{\prime}$ was returned.

####  4.3.3 Efficiency.

Similar to A*, GCS* with exact ReachesCheaper and an admissible heuristic is
optimally efficient, i.e., any path $\mathbf{v}$ that is expanded could have
been a subpath of an optimal path from source to target given the information
available. Since if
$h^{*}(\mathbf{x}^{*}(\mathbf{v})_{\text{end}})=\tilde{h}(\mathbf{x}^{*}(\mathbf{v})_{\text{end}})$,
then $\mathbf{v}$ would be a subpath of an optimal path. It follows that if
the heuristic were the optimal cost-to-go, i.e.,
$\tilde{h}(x)=h^{*}(x),\>\forall x\in\mathcal{X}_{v}\>\forall
v\in\mathcal{V}$, then GCS* would only expand vertices on optimal paths.
However, we do not have a tractable exact implementation of ReachesCheaper.
Under conservative ReachesCheaper, GCS* is not optimally efficient since
dominated paths may be added to $Q$ and expanded. However, with access to the
true cost-to-go, $h^{*}=\tilde{h}$, GCS* with conservative ReachesCheaper
regains optimal efficiency and only expands vertices on optimal paths.

####  4.3.4 Properties when Using Sampling-Based Domination Checks.

While we have proven that GCS* is cost-optimal and complete under conservative
IsDominated checks, a sampling-based implementation is sometimes preferred, as
discussed in sec. 4.2.2. In the limit of infinite samples, the sampling-based
implementations of NotDominated are exact. Thus it follows from theorems 4.1
and 4.2 that sampling-based GCS* is probabilistically complete and
asymptotically cost-optimal. One might raise concern that the backward-
reachable set from the target in some set $\mathcal{X}_{v}$ may never be
sampled, leading some important feasible path to be pruned. However, because
all $\mathcal{P}_{\mathbf{v}}$ are compact, the sets being checked for
containment, $\mathcal{S}_{\mathbf{v}}$, are closed.

Therefore, $\mathcal{S}_{\mathbf{v}}\setminus\bigcup_{\mathbf{v}^{\prime}\in
S[\mathbf{v}_{\text{end}}]}\mathcal{S}_{\mathbf{v}^{\prime}}$ with respect to
the topology of $\mathcal{S}_{\mathbf{v}}$ (the space being sampled) is open
and thus either empty or positive-measure. If it has positive measure, it will
eventually be sampled.

##  5 Application to Planar Pushing

Tasks involving making and breaking contact while constraining non-penetration
notoriously lead to an explosion of discrete modes, especially as the number
of bodies scales. Indeed, the formulation in sec. 5.1 sees this combinatorial
growth. The strength of our proposed method is the ability to approach such
problems.

###  5.1 Formulation

We implement a simplified planar pushing model with polyhedral robots and
objects that can translate but not rotate, and static obstacles. Contact is
frictionless, and motion quasi-static, where bodies’ velocities equal the net
forces applied to them. We enforce non-penetration between all pairs of
bodies, defining collision-free sets over robot and object positions. We also
define sets over positions and contact forces defining the permissible
physical contact behavior for each pair of bodies. Together, these two kinds
of discrete modes define the sets of the graph $G$. Each point
$\mathbf{x}_{v}\in\mathcal{X}_{v}$ in prog. 1 is comprised of $n_{k}$ knot
points, defining the planar positions for each robot and object, actuation
forces for each robot, and a force magnitude for each pair of bodies in
contact. All constraints are linear, leading to polyhedral sets. However,
extensions of this model to include rotations could involve semidefinite
relaxations, leading to spectrahedral sets, in a formulation similar to that
of Graesdal et al. [4]. This work could also be extended to borrow other
components of the GCS formulation of planar pushing from Graesdal et al., like
frictional contact. We omit these features for simplicity. However, the
assumption that bodies are polyhedral is harder to relax, as it allows for an
exact polyhedral decomposition of collision-free space.

####  5.1.1 Non-penetration.

We constrain that each pair of bodies do not penetrate one another by imposing
that one face of one of the two polyhedra is a separating hyperplane: all
vertices of one body lie on one side, and all vertices of the other body lie
on the other side.

####  5.1.2 Contact.

Every pair of bodies may have any of the following kinds of contact: no
contact, any face of one body in contact with any face of another, or any face
of one body in contact with any vertex of another. Gathering these options
over all pairs of bodies produces a contact set. Each pair of bodies in
contact in each contact set is accompanied by a force magnitude variable,
leading to variation in dimension between sets. This force acts normal to the
face in contact. Position constraints are added to ensure the associated
contact is physically feasible. Relative sliding of features in contact is
allowed within a single contact set, as long as they remain in contact.

####  5.1.3 Quasi-static dynamics.

For each object and robot, we enforce that between consecutive knot points,
translation is equal to the sum of forces on the body, including a robot’s
actuation force. Edge constraints enforce that robot and object positions
remain the same across edges – for an edge $(u,v)$ in eq. 1, the positions
from the last knot point in $\mathbf{x}_{u}$ must equal the positions from the
first knot point in $\mathbf{x}_{v}$.

###  5.2 Implementation Details

We use $n_{k}=2$ knot points per set. Our edge cost is the $L_{1}$ norm
distance between knot points, plus a constant penalty for switching modes.
That is, for $v\neq t$,
$c(\mathbf{x}_{u},\mathbf{x}_{v}):=1+\sum_{i}w_{i}||p^{i}_{v,1}-p^{i}_{v,0}||_{1}$,
for weights $w_{i}$, where $p^{i}_{v,j}$ gives the position of the $i$th
moving body (object or robot) at the $j$th knot point in vertex $v$.
$c(\mathbf{x}_{u},t)=0$ (no movement in the target set). We use $w_{i}=1$ for
all $i$. Our sampling-based implementations can handle $L_{2}$ norm or $L_{2}$
norm squared edge costs easily. However, as discussed in sec. 4.2.2, our
containment-based implementations need modification to accommodate non-linear
costs.

For simplicity, our nominal $\tilde{h}$ is the cost of a “shortcut” edge
between the point $x$ and some point in the target set
$x_{t}\in\mathcal{X}_{t}$ chosen to minimize $\tilde{h}$. If an edge does
exist directly to the target, the true cost of that edge is used. Otherwise,
$\tilde{h}(x)\leftarrow 1+c(x,x_{t})$, using a weight of $0.2$ for robot
displacements instead of the true weights $w_{i}=1$. Because the weights do
not exceed the true weights, this yields an admissible heuristic. Note that
$1$ is added to $c(x,x_{t})$ because we know that a constant penalty of $1$ is
applied for every mode switch and there will be at least one additional mode
switch before reaching the target. For speed, we also conduct experiments with
an $\epsilon$-suboptimal heuristic instead, where we scale $\tilde{h}$ by some
$\epsilon$, $\tilde{h}(x)\leftarrow\epsilon\cdot(1+c(x,x_{t}))$, to prune more
aggressively, using $\epsilon=10$. The shortcut edge ignores contact dynamics,
making this heuristic less informative in some scenarios than others.
$\tilde{h}$ can be further optimized, but this is not the focus of this paper.
Practitioners may apply domain knowledge to formulate informative heuristics,
trading off between informativeness and computational cost.

For sampling-based domination checks, we use only a single sample per check.
As we will discuss, empirically, this is sufficient. For containment-based
methods, we first perform a cheap single-sample NotDominated check, and only
check containment if it returns False. Additionally, taking advantage of the
unique structure of our planar pushing formulation, we only check the
domination conditions on the last position knot point instead of all variables
in the set. To reduce problem size for domination checks, we “solve away”
equality constraints by parameterizing in their nullspace.

##  6 Results

(a)

(b)

Figure 5: The $\epsilon$-suboptimal solution trajectories found by GCS* using
the sampling-based ReachesCheaper domination check on the AROUND Task (a) and
SQUEEZE Task (b).

Table 1 presents results for three planar pushing tasks: AROUND, SQUEEZE, and
STACK shown in figs. 5 and 1. AROUND is the simplest task: its graph has 194
vertices and 8,328 edges. SQUEEZE has a larger graph with 628 vertices and
56,806 edges. The dimensions of sets in AROUND and SQUEEZE range from 4 to 12.
STACK (fig. 1) has 5 bodies, leading to a combinatorial explosion of
approximately $1.3\times 10^{9}$ sets and up to $8.5\times 10^{17}$ edges. The
dimensions of its sets range from 8 to 24. Due to the sizes of the graphs, the
direct convex optimization approach [12] cannot be used, thus we compare GCS*
against IxG and IxG* [15], the state-of-the-art for incrementally solving SPP
on GCS.

Table 1: Here we show the solve time, solution cost, and number of expanded paths (Exp) for three planar pushing tasks. The shown methods (M) are: sampling (AS) and containment-based (AC) ReachesCheaper with an admissible heuristic. Sampling (BS) and containment-based (BC) ReachesCheaper with an $\epsilon$-inadmissible heuristic. Sampling (CS) and containment-based (CC) ReachesNew with an $\epsilon$-inadmissible heuristic. And baselines: IxG* with an admissible (X) and $\epsilon$-inadmissible (Y) heuristic; and IxG with an $\epsilon$-inadmissible heuristic (Z). A dagger ($\dagger$) indicates that the property holds probabilistically or asymptotically. Some experiments returned infeasible (Fail), were stopped after 10h ($\times$), or could not be run as the graph could not be explicitly constructed (-). The baselines required preprocessing times of 1.1m and 1.7h for AROUND and SQUEEZE respectively. Our methods require no preprocessing time. All computation was performed on an AMD Ryzen 9 7950x 16-core 32-thread processor.  |  |  | AROUND | SQUEEZE | STACK
---|---|---|---|---|---
M | Complete | Optimality | Time | Cost | Exp | Time | Cost | Exp | Time | Cost | Exp
AS | Yes† | Optimal† | 26.2s | 27.5 | 432 | 4m | 47.80 | 2114 | $\times$ | $\times$ | $\times$
AC | Yes | Optimal | 7.5h | 27.5 | 734 | $\times$ | $\times$ | $\times$ | $\times$ | $\times$ | $\times$
X | Yes | Optimal | $\times$ | $\times$ | $\times$ | $\times$ | $\times$ | $\times$ | - | - | -
BS | Yes† |  $\epsilon$-suboptimal† | 2.9s | 27.5 | 51 | 38.3s | 54.55 | 381 | 21.9s | 67.53 | 57
BC | Yes |  $\epsilon$-suboptimal | 4.6m | 27.5 | 73 | $\times$ | $\times$ | $\times$ | $\times$ | $\times$ | $\times$
Y | Yes | $\epsilon$-suboptimal | $\times$ | $\times$ | $\times$ | $\times$ | $\times$ | $\times$ | - | - | -
CS | Yes† | Suboptimal | 2.5s | 27.5 | 44 | 20s | 57.42 | 259 | 21.8s | 67.53 | 57
CC | Yes | Suboptimal | 35.4s | 28.5 | 75 | 20.5m | 57.42 | 458 | 3.5m | 67.53 | 64
Z | No |  Suboptimal | 4.7s | 28 | 31 | 2.3m | Fail | 615 | - | - | -

For sampling-based domination checks, as we increase the number of samples per
check we approach completeness and optimality, expanding more paths. Due to
the curse of dimensionality, such coverage demands exponentially more samples
as set dimensions increase. However, in our tasks we did not find sparse
sampling – in particular, a single sample per domination check – to
dramatically degrade solution quality. We understand this to be a property
particular to our problem structure, in which the condition that two paths
reach the same final knot point within the same vertex is relatively strong.
This is an interesting result that enables an efficient and de facto nearly
optimal implementation.

Containment-based domination checks scale poorly with path length, or, more
precisely, the dimension of the full trajectory through it. This is reflected
in the solve times in the AC, BC, and CC rows of table 1.

The number of paths expanded is impacted greatly by the strength of the
heuristic in the context of the particular task. In general, the “shortcut”
edge heuristic is strong for CORNER and STACK and weaker for SQUEEZE which
requires squeezing through a narrow gap then maneuvering around the object to
push it from a different direction multiple times. As expected, use of an
inadmissible heuristic leads to cost increase. Typically, containment-based
checks lead to more paths being expanded as compared to sampling-based checks
(as seen in table 1) because of false negatives from sampling, as well as
conservatism of the single domination criteria used by our containment-based
checks. However, this could also lead to fewer paths being expanded because
important paths are not wrongly pruned (not observed in table 1).

In comparison to our methods, the baselines require significant time to
compute heuristic values for the entire graph before starting their query
phase. While this approach can yield a stronger heuristic (as in Z, which
expands fewer paths than CC and CS for AROUND), for these tasks the solve
times can be dominated by this pre-processing phase. When the graph is very
large, this pre-processing can become intractable (as in STACK). Because IxG*
prunes paths based on a global upper bound found by IxG, in cases where IxG is
unable to return a solution (as in SQUEEZE), or simply when an admissible
heuristic is used (as in X), no paths will be pruned, resulting in intractably
large search spaces. As GCS* is complete, and our domination checks do not
rely on a global upper bound, GCS* does not experience these particular
limitations.

##  7 Conclusion

We propose GCS*, a forward heuristic search algorithm for solving large
discrete-continuous planning problems formulated as GCS. We define two
domination checks ReachesNew and ReachesCheaper, as well as containment and
sampling-based implementations of those checks that allow GCS* to be complete
and optimal, or have probabilistic/asymptotic versions of those properties,
respectively. GCS* provides a principled way of addressing the challenges of
applying graph search to the discrete-continuous setting. We demonstrate
settings in which GCS* performs favourably compared to the state-of-the-art.
For application to real-world planar pushing tasks, further work would be
required to handle rotations [4]. Incorporating these insights into algorithms
that leverage hierarchy, factorization or learned heuristics could solve more
complex problems faster.

####  7.0.1 Acknowledgements

This work was supported by the Aker Scholarship; Amazon.com, PO No.
2D-12585006; The Charles Stark Draper Laboratory, Inc., where Rebecca H. Jiang
is a Draper Scholar; and The AI Institute, award ID Agmd Dtd 8/1/2023.

####  7.0.2

The authors have no competing interests to declare that are relevant to the
content of this article.

## References

  * [1] Cormen, T.H., Leiserson, C.E., Rivest, R.L., Stein, C.: Introduction to algorithms. The MIT Press, 4th edn.
  * [2] Dai, H., Amice, A., Werner, P., Zhang, A., Tedrake, R.: Certified polyhedral decompositions of collision-free configuration space . https://doi.org/10.1177/02783649231201437
  * [3] Garrett, C.R., Chitnis, R., Holladay, R., Kim, B., Silver, T., Kaelbling, L.P., Lozano-Pérez, T.: Integrated task and motion planning 4, 265–293. https://doi.org/10.1146/annurev-control-091420-084139, https://www.annualreviews.org/content/journals/10.1146/annurev-control-091420-084139
  * [4] Graesdal, B.P., Chew Chia, S.Y., Marcucci, T., Morozov, S., Amice, A., Parrilo, P.A., Tedrake, R.: Towards tight convex relaxations for contact-rich manipulation. https://doi.org/10.48550/arXiv.2402.10312, http://arxiv.org/abs/2402.10312
  * [5] Hart, P.E., Nilsson, N.J., Raphael, B.: A formal basis for the heuristic determination of minimum cost paths 4(2), 100–107. https://doi.org/10.1109/TSSC.1968.300136, https://ieeexplore.ieee.org/document/4082128, conference Name: IEEE Transactions on Systems Science and Cybernetics
  * [6] Hartmann, V.N., Orthey, A., Driess, D., Oguz, O.S., Toussaint, M.: Long-horizon multi-robot rearrangement planning for construction assembly 39(1), 239–252. https://doi.org/10.1109/TRO.2022.3198020, https://ieeexplore.ieee.org/document/9868234/
  * [7] Jones, C.N., Morari, M.: Polytopic approximation of explicit model predictive controllers 55(11), 2542–2553. https://doi.org/10.1109/TAC.2010.2047437, http://ieeexplore.ieee.org/document/5617688/
  * [8] Kavraki, L., Svestka, P., Latombe, J.C., Overmars, M.: Probabilistic roadmaps for path planning in high-dimensional configuration spaces 12(4), 566–580. https://doi.org/10.1109/70.508439, https://ieeexplore.ieee.org/document/508439, conference Name: IEEE Transactions on Robotics and Automation
  * [9] LaValle, S.M.: Rapidly-exploring random trees: A new tool for path planning, https://msl.cs.uiuc.edu/~lavalle/papers/Lav98c.pdf
  * [10] Mao, J., Tenenbaum, J.B., Lozano-Perez, T., Kaelbling, L.P.: Learning reusable manipulation strategies. In: 7th Annual Conference on Robot Learning. https://openreview.net/forum?id=ihqTtzS83VS
  * [11] Marcucci, T., Petersen, M., von Wrangel, D., Tedrake, R.: Motion planning around obstacles with convex optimization 8(84), eadf7843. https://doi.org/10.1126/scirobotics.adf7843, https://www.science.org/doi/10.1126/scirobotics.adf7843
  * [12] Marcucci, T., Umenberger, J., Parrilo, P., Tedrake, R.: Shortest paths in graphs of convex sets 34(1), 507–532. https://doi.org/10.1137/22M1523790, https://epubs.siam.org/doi/full/10.1137/22M1523790
  * [13] Morozov, S., Marcucci, T., Amice, A., Graesdal, B.P., Bosworth, R., Parrilo, P., Tedrake, R.: Multi-query shortest-path problem in graphs of convex sets. In: (Under Review)
  * [14] Natarajan, R., Choset, H., Likhachev, M.: Interleaving graph search and trajectory optimization for aggressive quadrotor flight 6(3), 5357–5364. https://doi.org/10.1109/LRA.2021.3067298, http://arxiv.org/abs/2101.12548
  * [15] Natarajan, R., Liu, C., Choset, H., Likhachev, M.: Implicit graph search for planning on graphs of convex sets. In: (Under Review)
  * [16] Pohl, I.: Heuristic search viewed as path finding in a graph 1(3), 193–204. https://doi.org/10.1016/0004-3702(70)90007-X, https://linkinghub.elsevier.com/retrieve/pii/000437027090007X
  * [17] Russell, S.J., Norvig, P., Chang, M.W., Devlin, J., Dragan, A., Forsyth, D., Goodfellow, I., Malik, J., Mansinghka, V., Pearl, J., Wooldridge, M.J.: Artificial intelligence: a modern approach. Pearson series in artificial intelligence, Pearson, 4th edn., HOLLIS number: 99154334206503941
  * [18] Sadraddini, S., Tedrake, R.: Linear encodings for polytope containment problems. In: 2019 IEEE 58th Conference on Decision and Control (CDC). pp. 4367–4372. IEEE. https://doi.org/10.48550/arXiv.1903.05214, http://arxiv.org/abs/1903.05214
  * [19] Toussaint, M.: Logic-geometric programming: An optimization-based approach to combined task and motion planning. In: Proceedings of the 24th International Conference on Artificial Intelligence. pp. 1930–1936. IJCAI’15, AAAI Press
  * [20] Toussaint, M., Lopes, M.: Multi-bound tree search for logic-geometric programming in cooperative manipulation domains. In: 2017 IEEE International Conference on Robotics and Automation (ICRA). pp. 4044–4051. IEEE. https://doi.org/10.1109/ICRA.2017.7989464, http://ieeexplore.ieee.org/document/7989464/
  * [21] Werner, P., Amice, A., Marcucci, T., Rus, D., Tedrake, R.: Approximating robot configuration spaces with few convex sets using clique covers of visibility graphs. https://doi.org/10.48550/arXiv.2310.02875, http://arxiv.org/abs/2310.02875, version: 2