We propose a manifold optimization approach for solving constrained inference and planning problems. The approach employs a framework that transforms an arbitrary nonlinear equality constrained optimization problem into an unconstrained manifold optimization problem. The core of the transformation process is the formulation of constraint manifolds that represent sets of variables subject to equality constraints. We propose various approaches to define the tan-gent spaces and retraction operations of constraint manifolds, which are crucial for manifold optimization. We evaluate our constraint manifold optimization approach on multiple constrained inference and planning problems, and show that it generates strictly feasible results with increased efficiency as compared to state-of-the-art constrained optimization methods.

We present a manifold optimization approach to solve inference and planning problems with range constraints. The core of our approach is the definition of a manifold that represents points or poses with range constraints. We discover that the manifold of range-constrained points is homogeneous under the rigid transformation group action, and utilize the group action to derive the tangent space, retraction and topology of the manifold. We evaluate the performance of manifold optimization approach on solving range-constrained inference problems over state-of-the-art constrained optimization methods. The results show that manifold optimization with the range-constraint manifold achieves both faster speed and better constraint satisfaction. We further study the conditions of inference problems that we can treat range measurements as constraints in practice.

Roboticists have increasingly sought to incorporate mechanical compliance into legged robots to realize a range of potential benefits, from improved agility to resilience in complex environments. A promising approach for building compliance into robot legs is to utilize the pneumatic artificial muscle, a pneumatic actuator with inherent compliance due to the compressibility of air. While previous work has explored the capabilities of pneumatic-muscle driven robots in highly dynamic tasks like jumping, there is a lack of trajectory planning strategies for such robots. In this paper, we detail our approach to planning vertical jumping trajectories for a planar two-legged robot driven by four pneumatic artificial muscles using on/off "burst inflation" control. The trajectory optimization problem is represented as a factor graph and solved with the GTSAM optimizer. A hybrid dynamics approach is used to handle foot-ground contacts. The average jump height error between simulation and experiment across multiple jumping trajectories of varying heights was 9.5 cm; the average RMS error between all four joints was 5.6 deg. This work provides a basis to plan more complex jumping and leaping trajectories for pneumatic muscle-driven robots.

This paper presents a novel factor graph-based approach to solve the discrete-time finite-horizon Linear Quadratic Regulator problem subject to auxiliary linear equality constraints within and across time steps. We represent such optimal control problems using constrained factor graphs and optimize the factor graphs to obtain the optimal trajectory and the feedback control policies using the variable elimination algorithm with a modified Gram-Schmidt process. We prove that our approach has the same order of computational complexity as the state-of-the-art dynamic programming approach. Furthermore, current dynamic programming approaches can only handle equality constraints between variables at the same time step, but ours can handle equality constraints among any combination of variables at any time step while maintaining linear complexity with respect to trajectory length. Our approach can be used to efficiently generate trajectories and feedback control policies to achieve periodic motion or repetitive manipulation.

Simultaneous localization and mapping (SLAM) is a crucial functionality for exploration robots and virtual/augmented reality (VR/AR) devices. However, some of such devices with limited resources cannot afford the computational or memory cost to run full SLAM algorithms. We propose a general client-server SLAM optimization framework that achieves accurate real-time state estimation on the device with low requirements of on-board resources. The resource-limited device (the client) only works on a small part of the map, and the rest of the map is processed by the server. By sending the summarized information of the rest of map to the client, the on-device state estimation is more accurate. Further improvement of accuracy is achieved in the presence of on-device early loop closures, which enables reloading useful variables from the server to the client. Experimental results from both synthetic and real-world datasets demonstrate that the proposed optimization framework achieves accurate estimation in real-time with limited computation and memory budget of the device.

We present multi-robot iSAM2 (MR-iSAM2), an efficient incremental smoothing and mapping (iSAM) algorithm to solve multi-robot simultaneous localization and mapping (SLAM) inference problems. MR-iSAM2 is based on a novel data structure multi-root Bayes tree (MRBT), which packs multiple Bayes trees with the same undirected clique structure. In multi-robot scenarios, the MRBT enables new measurements from different robots to be updated in different root branches, while all updates are performed around the single root of the Bayes tree in the original iSAM2 algorithm. As a result, the MRBT better reveals the underlying sparsity and information flow in multi-robot SLAM inference problems than the Bayes tree. Based on this insight, we further develop MR-iSAM2 to incrementally update and maintain the sparsity structure of the MRBT and enable efficient information propagation among the roots for inter-robot inference. We analyze the properties of the MR-iSAM2 algorithm, and show with both synthetic and real world datasets that it significantly outperforms iSAM2 in efficiency when solving multi-robot SLAM problems.