Abstract:Many real-world sequential manipulation tasks involve a combination of discrete symbolic search and continuous motion planning, collectively known as combined task and motion planning (TAMP). However, prevailing methods often struggle with the computational burden and intricate combinatorial challenges, limiting their applications for online replanning in the real world. To address this, we propose Dynamic Logic-Geometric Program (D-LGP), a novel approach integrating Dynamic Tree Search and global optimization for efficient hybrid planning. Through empirical evaluation on three benchmarks, we demonstrate the efficacy of our approach, showcasing superior performance in comparison to state-of-the-art techniques. We validate our approach through simulation and demonstrate its reactive capability to cope with online uncertainty and external disturbances in the real world. Project webpage: <a class="link-external link-https" href="https://sites.google.com/view/dyn-lgp" rel="external noopener nofollow">this https URL</a>.
What problem does this paper attempt to address?
### Problems the paper attempts to solve
The paper aims to solve the high - dimensional combinatorial complexity problem in Combined Task and Motion Planning (TAMP). Specifically, TAMP usually involves the combination of discrete symbolic search and continuous motion planning, which leads to heavy computational burden and complex combinatorial challenges, limiting its application in practical online replanning. To solve these problems, the authors propose the Dynamic Logic - Geometric Program (D - LGP), a new method that achieves efficient hybrid planning by integrating dynamic tree search and global optimization.
### Main contributions
1. **Propose Dynamic Tree Search (DTS)**: Used for high - level task planning, it utilizes back - propagation to quickly find a feasible action skeleton from the solution space of combinatorial explosion.
2. **Propose an efficient D - LGP framework**: Combine DTS and global optimization to solve the TAMP problem efficiently and optimally.
3. **Closed - loop reactive planning**: Due to the fast computing ability of D - LGP, it can be designed in a closed - loop form, thus reacting to uncertainties and external disturbances in the real world.
### Method overview
#### A. Goal - centered LGP
D - LGP adopts a goal - centered LGP expression and determines the action skeleton by back - propagating from the goal configuration. The specific formula is as follows:
\[
\min_{a_k, x(t), u(t)} \int_{(k - 1)T}^{kT} c(x(t), u(t)) \, dt + c_T(x(kT), g_k)
\]
The constraints include:
- Initial state \( x(0)=x_0 \)
- Symbolic state \( s_k\in G(g_k) \)
- Goal configuration \( g_K = g_K \)
- Goal constraint \( h_{\text{goal}}(x(kT), g_k)=0 \)
- Path constraint \( h_{\text{path}}(x(t), u(t)|s_k(t)) = 0 \)
- Path inequality constraint \( g_{\text{path}}(x(t), u(t)|s_k(t))\leq0 \)
- Transition constraint \( h_{\text{switch}}(x(kT)|a_k, s_{k - 1}) = 0 \)
- Transition inequality constraint \( g_{\text{switch}}(x(kT)|a_k, s_{k - 1})\leq0 \)
- Action selection \( a_k\in A(s_{k - 1}) \)
- State transition \( s_k\in \text{succ}(s_{k - 1}, a_k) \)
- Sub - goal \( g_{k - 1}\in T^\dagger(g_k) \)
#### B. Mixed - integer motion planning
For the desktop rearrangement task, the position of each object is represented as \( P_n^t=(x_n^t, y_n^t, \theta_n^t)\in SE(2) \). By decomposing the non - convex problem into multiple convex subsets, the constraint conditions can be represented as a combination of four half - spaces:
\[
C_i: [a_i \, b_i] R(\theta_n^t)^\top u_t\geq c_i
\]
where \( R(\theta_n^t) \) is a rotation matrix, defined as:
\[
R(\theta_n^t)=\begin{bmatrix}
\cos(\theta_n^t)&-\sin(\theta_n^t)\\
\sin(\theta_n^t)&\cos(\theta_n^t)
\end{bmatrix}
\]
By introducing binary variables \( z_i \), the above constraint can be transformed into a Mixed - Integer Quadratic Programming (MIQP):
\[
C_n: [a_i \, b_