Author: Frank Dellaert

On June 1, I will be presenting CMC-Opt, new work by Yetong Zhang on constraint manifolds with corners, at the Frontiers of Optimization for Robotics workshop at ICRA. The paper is now available on arXiv:

CMC-Opt: Constraint Manifold with Corners for Inequality-Constrained Optimization, by Yetong Zhang and Frank Dellaert.

The paper presents a new way to use manifold optimization to solve large inequality-constrained optimization problems, such as state estimation and motion planning in legged systems: think quadrupeds and humanoids.

Speaking of legged systems, I’m also excited to announce more details and extensive experimental results on the four simple proprioceptive estimators for legged robots that we recently added to GTSAM. This work was done in collaboration with my recently graduated Ph.D. student Varun Agrawal, and with the awesome Chiyun Noh and Ayoung Kim from the RPM Robotics Lab at Seoul National University (SNU):

Four Simple Proprioceptive Estimators for Legged Robots by Frank Dellaert, Chiyun Noh, Varun Agrawal, and Ayoung Kim.

What these two papers have in common is that we turn the many constraints in legged systems (and robotic systems in general) into factor graphs that can be optimized to do both perception and planning. Some might say it’s sense-think-act with factor graphs.

CMC-Opt at ICRA

The core idea in CMC-Opt is to transform the hard problem of constrained optimization into an unconstrained problem directly on the feasible state space. Yetong Zhang, who is now on the motion planning team at Waymo, did a Ph.D. thesis with me on using geometry to make constrained robotic inference and planning problems easier to solve. The figure above illustrates constraint manifolds with corners, which can capture nonlinear equalities and inequalities.

Instead of using a monolithic classical constraint solver, his idea was to use the sparse graph structure and manifold optimization (as GTSAM provides) to create new manifolds out of the original variables and the constraints that involve them. Yetong created an algorithm to automatically identify “constraint-connected components” and turn them into new manifold types: lower-dimensional feasible spaces that create a new, coarser factor graph. This can really pay off in kinodynamic motion planning. For example, in the quadruped jumping example illustrated above, the search space drops from 32,194 dimensions to 2,260, and constraints will be satisfied by construction.

This is a continuation of the CM-Opt work that previously appeared at ICRA 2023 as Constraint Manifolds for Robotic Inference and Planning.

Legged robot estimators

The second paper is just as exciting! In a previous post, Legged State Estimation, I wrote about four simple proprioceptive estimators for legged robots, i.e., they only use an IMU and internal joint angles over time to determine the trajectory of the robot. They are basically KISS-style versions of the ideas first explored by Michael Bloesch et al. and Ross Hartley et al., where footholds are treated as landmarks: foot SLAM, so to speak.

Varun Agrawal was instrumental in creating legged-robot locomotion and estimation capabilities in the same factor-graph style used elsewhere in GTSAM, and those four estimators were directly inspired by his work. All four variants are available in GTSAM. But the question remained: how well do they perform on real, hardcore data?

Enter the collaboration with Chiyun Noh and Ayoung Kim at Seoul National University (SNU): they have done amazing work in legged robot state estimation and were kind enough to help us properly evaluate our new estimators, using the data they painstakingly collected with a Boston Dynamics Spot robot to create the GaRLILEO dataset. And, to top it off, Chiyun created a ROS2-compatible implementation, so you can try it yourself!

The main discovery, hinted at in the figure above: the simple GTSAM legged estimators are not half bad, one might even say they are pretty good! In this dataset they beat out all of the state-of-the-art estimators that we tested them against. This is absolutely not the end of the story: we are working to test these estimators in several other environments and against other recent estimators. But for something that originated as an example in my advanced robotics class, they are at the very least very good baselines to test against.

Of course, these “blind” estimators are just the basis for a practical robot perception system. You need to fuse these with an external sensor, such as vision, LiDAR, or radar, which is exactly what Ayoung Kim’s group excels at doing. But it’s good to start from an estimator that at least keeps very good track of where you walk while closing your eyes!

Further browsing

• Frontiers of Optimization for Robotics workshop
• arXiv link: CMC-Opt: Constraint Manifold with Corners for Inequality-Constrained Optimization.
• blog post: Legged State Estimation
• arXiv link: Four Simple Proprioceptive Estimators for Legged Robots.
• STAG: sense-think-act with factor graphs

Disclosure: AI was used to help draft this post and prepare the figures.