Homotopy Methods for Motion and Manipulation Planning under Tight and Non-Smooth Constraints = 협소하고 비매끄러운 제약 조건 하에서의 모션 및 조작 계획을 위한 호모토피 기법

다국어 초록 (Multilingual Abstract)

Optimization plays a central role in robotics as a tool for solving various problems such as motion planning, manipulation, and control, and also serves as a mean of generating data. However, such optimization problems are highly sensitive to initialization and finding global optima is extremely challenging, due to characteristics such as discontinuity, non-smoothness, narrow feasible regions, and rugged optimization landscapes.
This paper introduces a homotopy optimization framework that mitigates these challenges by first relaxing the problem and then progressively recovering the original formulation.
In particular, we focus on two major difficulties: narrow passages collision avoidance constraints and contact dynamics constraints, and present methods to address them efficiently through homotopy optimization.

Narrow passages collision avoidance in a representative challenge in path planning, where efficient path finding becomes difficult.
To address this, we propose a homotopy optimization method that decomposes obstacles into convex objects and interpolates their signed distance functions (SDF) to gradually recover the original collision avoidance constraints.
This approach enables scalable and efficient path planning even in highly narrow passage.

Contact dynamics constraints pose another challenge, as their inherent nonlinearity and discontinuity complicate optimization.
Existing relaxation methods often fail to account for the the optimization landscape, sometimes making it more rugged during the relaxation process.
To overcome this, we introduce a damping parameter into the contact model, allowing optimization to start under smoothed dynamic conditions and progressively converge to the original dynamics.
This approach smooths the cost landscape and significantly improves the optimization.

국문 초록 (Abstract)

로보틱스 분야에서 최적화는 경로 계획, 조작, 제어 등 다양한 문제의 해를 구하는 수단이자, 학습을 위한 데이터 생성을 위한 중요한 도구로 활용된다. 그러나 이러한 최적화 문제들은 불연속성, 비매끄러움, 좁은 해 공간, 거친 최적화 지형과 같은 특성으로 인해 해의 초기값에 민감하고, 전역 최적해를 찾는 것이 매우 어렵다. 본 논문에서는 이러한 어려움을 극복하기 위해 문제를 완화된 형태로 변환한 후, 점진적으로 원래의 문제로 복귀시키면서 푸는 호모토피 최적화 기법을 도입한다.
특히, 좁은 통로에서의 충돌 회피 제약조건과 접촉을 포함한 동역학 제약조건이라는 두 가지 주요 어려움을 중심으로, 이들을 효율적으로 다루는 방법을 제시한다.

협소 통로 충돌 회피는 경로 계획에서 대표적인 어려움로, 효율적인 경로 탐색이 어렵다. 본 연구에서는 장애물을 다수의 볼록 부분으로 분해하고, 볼록 객체의 부호 거리 함수(SDF)를 보간을 이용하여 충돌 회피 제약조건을 점진적으로 강화하는 호모토피 최적화 기법을 제안한다. 이로써 협소한 자유 공간에 대해서도 확장성이 뛰어나고 효율적인 경로 계획이 가능함을 보인다.

또한, 접촉을 포함한 동역학 제약조건은 비선형성과 불연속성으로 인해 수많은 지역 최솟값을 유발하여 최적화를 어렵게 만든다.
기존의 완화 기법들은 최적화 지형에 대한 고려 없이 설계되어, 완화 과정에서 오히려 지형이 더 거칠어지는 문제를 초래하기도 한다.
이를 해결하기 위해, 본 연구는 접촉 모델에 감쇠 파라미터를 도입하여 초기에는 부드러운 동역학 조건 하에서 최적화를 수행하고, 점진적으로 실제 조건으로 수렴하도록 유도한다. 이를 통해 비용 함수 지형을 부드럽게 하고 수렴성을 개선한다.

목차 (Table of Contents)

• 1 Introduction 1
• 1.1 Motivation and Background 1
• 1.2 Overview of Optimization in Robotics 4
• 1.3 Challenges in Robotic Optimization 7
• 1.3.1 Narrow feasible space 7
• 1.3.2 Discontinuity and non-smoothness 8
• 1.3.3 Rugged optimization landscape 9
• 1.4 Fundamentals of Homotopy Methods 10
• 1.5 Related Works on Homotopy Methods in Robotics 12
• 1.5.1 Homotopy Methods for Path Planning 13
• 1.5.2 Homotopy Methods for Trajectory Optimization 13
• 1.5.3 Incremental and Hierarchical Methods 14
• 1.5.4 Learning-Enhanced Homotopy Methods 15
• 1.6 Contribution 16
• 1.7 Organization 18
• 2 Narrow Passage Path Planning 20
• 2.1 Introduction 20
• 2.2 Related Works 24
• 2.2.1 Narrow Passage Path Planning 24
• 2.2.2 Shape Interpolation 26
• 2.2.3 Homotopy Method for Path Planning 27
• 2.2.4 Multilevel Planning 28
• 2.3 Notations and Mathematical Preliminaries 29
• 2.3.1 Signed Distance Function and Occupied Space 29
• 2.3.2 Abstract Simplicial Complex 31
• 2.3.3 Homotopy Equivalence 33
• 2.3.4 Nerve Complex 34
• 2.3.5 Dominated Vertices and Homotopy Equivalence 36
• 2.4 Problem Formulation 36
• 2.5 SDF Interpolation between Convex Objects 38
• 2.5.1 SDF Interpolation between Convex Objects 38
• 2.5.2 Alleviating Sharpness using Shaping Function 41
• 2.5.2.1 Linear interpolation 41
• 2.5.2.2 Shaped interpolation 42
• 2.5.2.3 Comparison of sharpness 43
• 2.6 Homotopy-Preserving Collision Constraints Interpolation 45
• 2.6.1 Collision Constraint Interpolation using SDF and Collapsible Sets 46
• 2.6.2 Sequence Construction for Convex Object Addition 49
• 2.7 Path Planning Optimization 50
• 2.7.1 Collision Detection of Interpolated Environment 51
• 2.7.2 Adaptive Increase of Interpolation Variable 54
• 2.7.3 Facilitating Continuous Collision Avoidance 58
• 2.8 Evaluations 60
• 2.8.1 Placing Dishes on the Rack 61
• 2.8.1.1 Ablation study 62
• 2.8.1.2 Comparison with baseline planners 63
• 2.8.1.3 Scalability with respect to narrowness 64
• 2.8.1.4 Real world experiment 65
• 2.8.2 Grasp-Aware Path Optimization for Object Extraction through Gaps 66
• 2.8.2.1 Comparison with baseline planners 66
• 2.8.2.2 Comparison with multilevel planners 69
• 2.8.2.3 Comparison on different shaping function 72
• 2.8.2.4 Real world experiment 73
• 2.8.3 Navigating a Maze with Nonholonomic Constraints 74
• 2.8.4 Humanoid Path Planning Under a Desk 77
• 2.8.5 Franka Cooperative Manipulation 77
• 2.8.6 Path Variability from Different Sequence of Object Additions 79
• 2.9 Discussions and Limitations 80
• 3 Handling Non-smooth Contact Constraints 82
• 3.1 Introduction 82
• 3.2 Related Works 85
• 3.2.1 Differentiable Simulations 85
• 3.2.2 Homotopy Optimization for Contact-Implicit Optimization 86
• 3.3 Preliminary 87
• 3.3.1 Dynamics Discretization 87
• 3.3.2 Contact Models 88
• 3.3.3 Contact Dynamics as an Optimization 90
• 3.3.4 Implicit Function Theorem 92
• 3.3.5 Calculation of the Simulation Gradient 93
• 3.4 Failure Modes of Gradients in Differentiable Simulation 95
• 3.4.1 Discontinuities in Contact Feature 95
• 3.4.2 Gradient Degradation due to Contact Mode Switching 96
• 3.4.3 Jittering in the Dynamics of Smoothed Contact Models 97
• 3.5 Homotopy Optimization using Damped Contact Model 101
• 3.5.1 Damped Contact Model 101
• 3.5.2 Probabilistic Homotopy Optimization Method 103
• 3.5.3 Interpretation as a Regularized Lagrangian 106
• 3.6 Evaluations 109
• 3.6.1 Comparison of the Cost Landscape 109
• 3.6.1.1 Rigid box flipping 109
• 3.6.1.2 Nonprehensile pushing 111
• 3.6.2 Trajectory Optimization 112
• 3.6.2.1 Nonprehensile Pushing 114
• 3.6.2.2 Finger Manipulation 116
• 3.7 Discussions and Limitations 118
• 4 Combining Collision Avoidance and Contact Dynamics Constraints 120
• 4.1 Introduction 120
• 4.2 Preliminary on ADMM 121
• 4.3 Joint Problem Formulation 123
• 4.3.1 ADMM Decomposition 124
• 4.3.2 Contact-Implicit Subproblem 125
• 4.3.3 Collision Avoidance Subproblem 126
• 4.3.4 Dual Update Step 126
• 4.4 Two-Phase Homotopy Scheduling 126
• 4.5 Implementation on Nonprehensile Planar Pushing with Obstacle Avoidance 127
• 5 Conclusion 130
• Appendix A Supporting Lemma 133
• Appendix B Proof of Proposition. 2.7 135
• Appendix C Proof of Thm. 2.10 137
• Appendix D Proof of Theorem 2.10 140
• Appendix E Proof of Thm. 2.12 142
• Appendix F Differentiable Contact Feature 144
• F.1 Handling Rigid Objects with Convex Decomposition and Differentiable Proxies 145
• F.2 Invariant Contact Set 148