Many problems in robotics require reasoning over a mix of continuous dynamics and discrete events, such as making and breaking contact in manipulation and locomotion. These problems are locally well modeled by linear complementarity quadratic programs (LCQPs), an extension to QPs that introduce complementarity constraints. While very expressive, LCQPs are non-convex, and few solvers exist for computing good local solutions for use in planning pipelines. In this work, we observe that complementarity constraints form a Lie group under infinitesimal relaxation, and leverage this structure to perform on-manifold optimization. We introduce a retraction map that is numerically well behaved, and use it to parameterize the constraints so that they are satisfied by construction. The resulting solver avoids many of the classical issues with complementarity constraints. We provide an open-source solver, Marble, that is implemented in C++ with Julia and Python bindings. We demonstrate that Marble is competitive on a suite of benchmark problems, and solves a number of robotics problems where existing approaches fail to converge.

翻译：许多机器人学问题需要对连续动力学与离散事件（例如操作与运动中接触的建立与解除）的交织进行推理。这类问题在局部上可被线性互补二次规划（LCQP）——在传统QP基础上引入互补约束的扩展模型——有效建模。尽管表达力极强，但LCQP具有非凸性，且现有算法难以在规划流程中高效计算其优质局部解。本文观察到互补约束在无穷小松弛下形成Lie群结构，并利用该结构进行流形上优化。我们提出一种数值表现良好的收缩映射，通过该映射对约束进行参数化使得约束自动满足。由此构建的求解器规避了互补约束的经典数值困难。我们开源了基于C++实现并配备Julia/Python接口的求解器Marble。实验表明Marble在基准测试集上具有竞争力，并能解决多个现有方法无法收敛的机器人学问题。