Logic Dynamic Movement Primitives for Long-horizon Manipulation Tasks in Dynamic Environments
Yan Zhang Teng Xue Amirreza Razmjoo Sylvain Calinon







A Survey on Intermediate Fusion Methods for Collaborative Perception Categorized by Real World Challenges
    Melih Yazgan23⁎,
Thomas Graf3⁎,
Min Liu3⁎, Tobias Fleck23 and J. Marius Zöllner23

2FZI Research Center for Information Technology, Germany

3Karlsruhe Institute of Technology, Germany
 
    25 April 2024
==============================================================================================================================================================================================

empty
empty


    Learning from Demonstration (LfD) stands as an efficient framework for imparting human-like skills to robots. Nevertheless, designing an LfD framework capable of seamlessly imitating, generalizing, and reacting to disturbances for long-horizon manipulation tasks in dynamic environments remains a challenge. To tackle this challenge, we present Logic Dynamic Movement Primitives (Logic-DMP), which combines Task and Motion Planning (TAMP) with an optimal control formulation of DMP, allowing us to incorporate motion-level via-point specifications and to handle task-level variations or disturbances in dynamic environments. We conduct a comparative analysis of our proposed approach against several baselines, evaluating its generalization ability and reactivity across three long-horizon manipulation tasks. Our experiment demonstrates the fast generalization and reactivity of Logic-DMP for handling task-level variants and disturbances in long-horizon manipulation tasks.
    
    

















§ INTRODUCTION

Learning from Demonstrations (LfD) has proven to be an effective approach for enabling robots to tackle complex manipulation tasks by imitating expert demonstrations <cit.>.
Recent advancements in LfD have extended its applicability to long-horizon manipulation tasks, such as table rearrangement or kitchen activities, by segmenting demonstrations into sub-tasks <cit.>, keyframes <cit.>, or skills <cit.>. However, existing works often focus solely on reproducing demonstrations, assuming a sequential execution of learned skills can achieve task goals, which is not always applicable in real-world scenarios <cit.>. While traditional LfD methods like Dynamic Movement Primitives (DMP) <cit.>, Task-parameterized Gaussian Mixture Model (TP-GMM) <cit.>, or Dynamical Systems (DS) <cit.> can generalize demonstrated trajectories for new tasks or react to disturbances at the motion level, long-horizon planning necessitates not only motion-level generalization but also the ability to handle task-level variations and disturbances. Consequently, designing an LfD framework that can imitate, generalize, and react to disturbances while solving multi-step manipulation tasks remains a challenge <cit.>.

In the domain of long-horizon planning, Task and Motion Planning (TAMP) has emerged as a powerful tool for solving multi-step manipulation tasks <cit.>. TAMP involves searching over a set of abstracted actions, with parameters often determined through sampling- or gradient-based motion planning. While theoretically capable of solving any feasible long-horizon manipulation task, TAMP relies on accurate dynamics modeling <cit.> and combinatorial searching of task instances and valid motions <cit.>. This limits the application of TAMP methods in real-world manipulation scenarios, involving model uncertainty and external disturbances.



In this work, we propose Logic-DMP, an LfD approach that generalizes one multi-step demonstration to solve new similar multi-step tasks, by combing the advantages of DMP and TAMP solvers. Logic-DMP leverages an optimal control formulation of DMP for motion modulation and extends it to incorporate via-point specifications for solving contact-rich manipulation sub-tasks, like pulling a cube with a hook in Figure. <ref>, while deploying TAMP solvers. Combing LfD with TAMP solvers can mitigate the challenges associated with modeling complex contact dynamics. Besides, we show that the demonstration can be formulated into multi-goal specifications for TAMP solver so that it can try to achieve all the states on the demonstrated task-and-motion trajectory in parallel, which significantly accelerates the planning process while generalizing the demonstration for solving new tasks. We further extend our approach to introduce a reactive TAMP framework by deploying Logic-DMP in a closed-loop fashion for real-world tasks in dynamic environments.

Notably, our choice of DMP as the motion modulation block is motivated by its extrapolation generalization ability based on a single demonstration. However, conventional DMP suffers from poor via-points encoding ability, limiting its applicability for tasks with specific via-points requirements that can facilitate collision avoidance or contact-rich manipulation tasks <cit.>. In our prior work <cit.>, we propose Linear Quadratic Tracking with Control Primitives (LQT-CP), an optimal control formulation of DMP that can freely modulate the way in which the system reacts to perturbations. In this paper, we explore the flexible motion modulation abilities of LQT-CP and further extend it to incorporate via-points tracking and generalization capability, addressing the limitation inherent in classical DMP.

In summary, our work proposes the following technical advancements:

    
  * Extend LQT-CP to handle long-horizon manipulation tasks and further develop its capability to incorporate via-point tracking and generalization for addressing contact-rich manipulation sub-tasks;
    
  * Propose Logic-DMP, a novel integration of TAMP solver with DMP. This integration improves the generalization ability and reactivity of DMP in multi-step manipulation tasks;
    
  * Develop a Reactive TAMP approach, leveraging the fast generalization capability of Logic-DMP. This approach facilitates the solution of long-horizon manipulation tasks in dynamic environments.




§ RELATED WORK






 §.§ LfD for long-horizon manipulation tasks

Previous LfD methods typically address long-horizon manipulation tasks by segmenting demonstrations and composing movement primitives trained with the segmented demonstration <cit.>. Schwenkel et al. and Jaquier et al. propose methods to find the best sequence of primitives based on demonstrations for the target tasks <cit.>. Our Logic-DMP shares similarities in composing skill primitives for complex long-horizon manipulation tasks. However, unlike other methods, it does not assume a sequential execution of those primitives (namely, without generalization or replanning). We show in experiments that the removal of this assumption can improve the generalization ability and reactivity of the system to handle task variants and disturbances.

Under the umbrella of combing LfD with TAMP, Mandlekar et al. propose human-in-the-loop TAMP where human teleoperation assists TAMP solvers to find feasible motions for complex contact-rich manipulation tasks without complicated dynamics modeling <cit.>. Mcdonald et al. and Dalal et al.<cit.> propose to train a policy imitating a TAMP planner with a large-scale database of demonstrations collected offline, achieving good performance on generalization and reactivity. In contrast, we show that our Logic-DMP can achieve better performance by relying on a single demonstration.



 §.§ Reactive Task and Motion Planning

Our closed-loop Logic-DMP operates within the domain of reactive TAMP, where rapid replanning is essential to respond to disturbances in dynamic environments. In recent studies <cit.> <cit.>, temporal logic-based reactive synthesis has been combined with Dynamical Systems (DS) or behavior tree-based control strategies for reactive action selection and plan switching to address task-level disturbances in multi-step manipulation tasks. In contrast, our approach delves into a PDDL-based (Planning Domain Definition Language) TAMP planning framework, which includes logical state and action abstraction, thus facilitates the integration of action primitives learned from demonstration. 

In <cit.>, Migimatsu et al. propose to execute motions of the TAMP plan in object-centric Cartesian coordinates, demonstrating efficiency in reacting to motion-level disturbances. However, their method does not extend to handling task-level disturbances, setting it apart from our approach. In <cit.>, Xue et al. introduce the Dynamic Logic-Geometric Program (D-LGP), showcasing impressive time efficiency for TAMP. However, D-LGP relies on keyframe-based action skeleton planning, making it incapable of handling disturbances within the keyframes. Receding-Horizon TAMP (RH-TAMP) approaches iteratively solve a reduced planning problem over a receding window of shorter action skeleton, accelerating the TAMP planning process for handling interventions in changing environments <cit.>. Nevertheless, a shorter prediction horizon increases the undesired infeasibility of action skeletons. Our Logic-DMP also accelerates TAMP by reducing the planning problem into a partial problem. However, it does not adopt a receding-horizon fashion, thus avoiding the influence on the feasibility.

In <cit.>, Harris et al. propose a Feasibility-based Control Chain Coordination (FC^3) approach, showcasing impressive reactiveness by constructing offline a set of possible action plans and by switching between them in an online manner. However, the success of FC^3 heavily relies on the constructed plan library and switching strategy. The closest work to ours is Robust Logical-Dynamical Systems (RLDS) <cit.>, which modifies the initial action plan by jumping backward or forward. However, RLDS assumes task disturbances can be handled without action plan switching, significantly limiting its reactiveness under substantial task perturbations. In several following experiments, our method demonstrates superior reactiveness to various levels of task disturbances than RLDS, which is achieved by online generalization of the demonstrated nominal plan, without the need of constructing an action plan library offline as in <cit.>.



§ PRELIMINARY



 §.§ Planning Domain Definition Language (PDDL)

In this section, we present essential concepts related to the Planning Domain Definition Language (PDDL) using the block stacking task illustrated in Figure <ref> as an example. PDDL serves as an interface for defining the world by specifying a set of facts that describe relationships among objects in the environment. For instance, in ℒ_1 of Figure <ref>, the cube C is positioned on the cube D, and this relationship is represented by the fact (on C D). Throughout this paper, we use italicized symbols to denote these facts and the term scene graph to collectively represent the entire set of facts.

Among these facts, those that remain constant throughout the planning process are termed static facts (e.g., (cube C)). Conversely, facts that vary during the process are referred to as fluents (e.g., (on C D), which changes with the robot's operation).

PDDL also provides the concept of action abstraction. An action is defined by a set of parameters, preconditions that must be satisfied before executing the action, and effects that occur after the action. Taking the action pick as an example, its parameters may include specifying which robot(s) pick which object(s). If, for instance, we have (robot panda) and (cube C) as the parameters, one precondition for executing this action should be (clear C), indicating that there are no cubes on C, making it possible to grasp. The resultant effect is denoted by (inhand panda C), signifying that the cube C is now in the hand of the robot arm panda.



















§ METHOD




 §.§ LQT-CP: an optimal control formulation of DMP



DMP is constructed in two parts to converge to the final goal position while tracking the acceleration profiles for mimicking the shape of a demonstrated trajectory. Similarly, Linear Quadratic Tracking (LQT), the most basic form of optimal control, is described by a cost typically composed of several parts acting at different state and control levels, with references either in the form of full trajectory or final goal positions. In particular, the cost can be specified to track a reference velocity or acceleration profile while reaching a target state at the end of the movement, with weight matrices balancing the importance of tracking the desired profiles and reaching the final goal position. With this similarity, we can reformulate DMP into a LQT fashion with the acceleration profile of the demonstrated trajectory as reference trajectory and the attractor as the goal to be reached at the end.

Similarly to DMP, our system is a virtual point mass driven by a linear integrator system to describe the evolution of the system. As shown in <cit.>, the control profile can be encoded with basis functions, resulting in a Control Primitive (CP) formulation of LQT that estimates superposition weights instead of the full list of control commands. The resulting LQT-CP yields a DMP by minimizing the cost function:

    c = (μ - x)^⊤Q (μ - x) + u^⊤Ru,

where μ=[μ^_0, μ^_1, …, μ^_T]^ indicates the concatenated vector of the position, velocity, acceleration profiles of the demonstrated trajectory and μ_T includes the goal position g to be reached at the end. x=[x^_0, x^_1, …, x^_T]^ represents the concatenated system state variables. The matrices Q = diag(Q_0, Q_1, …, Q_T) and R = diag(R_0, R_1, …, R_T) are a block-diagonal matrices showing the evolution of weight matrices Q_t and R_t for for variable x and command u = [u^_0, u^_1, …, u^_T]^, correspondingly. The evolution of the state is described by a linear integrator x_t+1=A_t x_t+B_t u_t, yielding x=S_xx_0+S_uu at trajectory level, where S_x and S_u are composed of A_t and B_t, see <cit.> for details.

Similarly as in DMP, where basis functions are used as movement primitives to represent the non-linear forcing term, basis functions can be used to represent the corresponding control commands. By leveraging a least squares formulation of recursive LQR, we showed in <cit.> that the control commands can be expressed as u = -Fx̃_0 = -ΨWx̃_0 where Ψ are the basis functions stored in matrix form, W is the weight matrix, and x̃_0 is the augmented initial state for transferring the original LQT problem into an LQR problem with control primitives:

    min_W
                = x^⊤Qx + (ΨWx̃_0)^⊤R (ΨWx̃_0), 
       s.t.  x̃=(S̃_x - S̃_uΨW)x̃_0,

where x is the concatenation of augmented state x_t = [x^_t, 1]^⊤, Q is the concatenation of augmented weight matrices computed as

    Q̃_t=[[      I      0; -μ_t^⊤      1 ]][[ Q_t   0;   0   1 ]][[    I -μ_t;    0    1 ]],

S̃_x and S̃_u are  state transfer matrices composed of Ã_t = [[ A_t   0;   0   1 ]] and B̃_t = [ B_t;   0 ] that define the augmented system dynamics x̃_t+1=Ã_t x̃_t+B̃_t u_t.

Solving (<ref>) results in the optimal weight matrix estimations:

    ŵ = (Ψ^⊤S̃_u^⊤Q̃S̃_uΨ + Ψ^⊤RΨ)^-1Ψ^⊤S̃_u^⊤Q̃S̃_x,



The above solution can be used as a recursive formulation to generate a feedback controller that can handle external perturbation in real-time execution, providing a feedbcak controller







u_t = -K̃_t x̃_t where K̃_t is the feedback gain matrix at time step t. We can therefore recursively calculate the feedback gains of our feedback controller LQT-CP with

    K̃_t    = Ψ_t ŴP_t, 
    P_t    = P_t-1 (Ã_t-1 - B̃_t-1K̃_t-1)^-1,
 
where K̃_0 = Ψ_0W, P_0 = I, see <cit.> for details. 

LQT-CP provides additional flexibility in the design of the precision matrix Q to encode the importance of tracking different profiles of the demonstrated trajectory as well as capturing their correlation constraints. Therefore, we can design Q to track the acceleration profiles and goal position of the demonstrated trajectory to generate an optimal control formulation of DMP. In this paper, we further propose incorporating via-point specifications into the reference trajectory μ and adjusting the weight matrix Q to define these specifications. This extension allows us to address contact-rich manipulation sub-tasks while concurrently extending LQT-CP to handle long-horizon manipulation tasks. We illustrate this modification in Section <ref> with the example of pulling a cube using a hook.





 §.§ Logic-DMP

In this section, we introduce Logic-DMP for Task and Motion Planning (TAMP).
The primary enhancement involves augmenting the classical demonstration representation, typically consisting of geometrical state sequences (e.g., position trajectories), with logical state and action abstractions derived from PDDL, and more importantly integrating TAMP solver with the proposed LQT-CP.

Consider a long-horizon manipulation task with N sub-tasks to be solved sequentially. The demonstration, denoted as 𝒫 = {{ℒ_i, a_i}_i=1,…,N, {τ_k}_k=1,…, K}, encompasses both task- and motion-level trajectories. Here, {ℒ_i}_i=1,…,N represents the logical state sequence from the initial to the goal scene graph, akin to a task-level trajectory ℒ. a_i denotes the abstracted action, defining the transition from ℒ_i to ℒ_i+1, and specifies which robot arm(s) executes the motion-level trajectory for manipulating which particular object(s). In contrast to the straightforward propagation of geometrical states through Euclidean or unit quaternion operations (⊕, ⊖), the logical state propagation is confined to a feasible action set 𝒜={a_k}_k=1,…, K for the target task. Consequently, the corresponding demonstration for Logic-DMP should include not only the logical and geometrical state sequences, but also the corresponding action sequence {a_i}_i=1,…,N indicating the feasible propagation of logical states. 

Given the demonstration 𝒫, we offline train the feedback gains of a set of LQT-CPs for each action primitive in 𝒜 with the corresponding motion-level demonstrations {τ_k}_k=1,…, K. Replacing τ_k in 𝒫 with the corresponding LQT-CP_k, by exploring the generalization ability of LQT-CP in motion level, we are able to generate an initial plan 𝒫_I which can handle new long-horizon manipulation tasks that only require the generalization of geometrical states. However, this limited generalization ability cannot address new tasks that require generalization in both task and motion levels.

Therefore, we further propose a new integration of TAMP solver with the proposed LQT-CP method. This integration results in a Logic-DMP that quickly generalizes the demonstration 𝒫 to solve new long-horizon tasks that share the same task goal state G but have different initial states than the template task, thus requiring generalization in both task and motion levels.

The overview of the whole framework is depicted in Figure <ref>, shown with a four-block stacking scenario. Supposing the new task starts with a new logical state ℒ_0^', Logic-DMP efficiently solves this new task with a two-step planning process and only needs to solve a partial TAMP problem. The first step involves finding a feasible task-and-motion trajectory 𝒫^new that transitions ℒ_0^' to its closest logical state ℒ_1∈ℒ in the demonstration 𝒫, then concatenating 𝒫^new with the corresponding segmentation of the initial plan ℒ_1→ℒ_2→G to establish a feasible task plan transitions ℒ_0^' to G within the smallest time cost. Secondly, Logic-DMP generates the feasible motion-level trajectories by exploring the generalization ability of the proposed LQT-CP. The state in 𝒫^new closest to the new initial state ℒ_0^' is indirectly determined by reformulating the demonstrated task-level trajectory ℒ = {ℒ_i}_i=1,…,N as multiple goals 𝒢 and running the PDDLStream <cit.> algorithm to reach theses goals concurrently, with the one reached in the shortest time considered as the target closest state. It is worth noting that our Logic-DMP framework treats the TAMP solver as a black-box tool, allowing for the integration of any TAMP solver into the framework, demonstrating its generality. 


 §.§ Reactive TAMP with Logic-DMP

Logic-DMP ensures fast discovery of a feasible task and motion plan. Since the closest state is usually reached much earlier than the goal state, Logic-DMP often resolves only a partial TAMP planning process while reacting to task-level disturbances. Considering that the planning time of a TAMP solver tends to increase significantly with the length of the final action skeleton, solving only a partial TAMP problem allows Logic-DMP to perform notably faster than the original TAMP solver. Therefore, we further extend the Logic-DMP into a close-loop TAMP framework (Algorithm <ref>) which promptly reacts to task-and-motion-level disturbances while executing the nominal plan in dynamic environments.


It assumes the task goal G, demonstration 𝒫, and an initial plan 𝒫_I are given. In the initialization process, multi-goal specifications 𝒢 are generated based on the demonstration and the task goal with designed MultiGoalsSpecification function in Line 4. This functions filters out the static facts in ℒ and keeps the sequence of fluents in 𝒫. The designed SceneGraph function generates the facts about current environment based on captured information about objects and robots in the environment and outputs the filtered key facts. 

To solve the target task in closed-loop, we continuously estimate the key logical states 𝒮_C after executing each action a in the current plan 𝒫_C. If the target state G is not a subset of 𝒮_C, indicating the task is not achieved yet. We then invoke the Logic-DMP to swiftly generate a new plan. With the LogicIn function in Line 8, we check if current state 𝒮_C is a subset of the multi-goal specifications 𝒢. If it is, we also generate its corresponding index id in the sequence. Then, the action 𝒫_I[id] in the initial plan will be a feasible action that transitions current state 𝒮_C to the goal state G. If 𝒮_C is not a subset of 𝒢, it indicates a severe task-level disturbance in the environment, and PDDLStream with multi-goal specification 𝒢 is applied to find a new feasible plan 𝒫^new that converges to the demonstration 𝒫. The first action in 𝒫^new is executed as the feasible action for the current state. The action a not only shows which action should be executed, but also indicates which robot arm(s) should execute that action for manipulating which object(s). The LQT-CP is then applied in an object-centric manner to generalize the reference trajectory for generating the corresponding motion-level trajectory τ^new to execute action a.



§ EXPERIMENTS

In this section, we present a comparative analysis between Logic-DMP and several baselines across three long-horizon manipulation planning problems. Figure <ref> shows the experiment setups and the demonstrated tasks for the benchmarks. For each benchmark, we demonstrated the robot with an initial task and motion plan based on available action sets. Subsequently, we introduced distinct initial setups or disturbances to assess the performance of Logic-DMP against the selected baselines. 





 §.§ Benchmarks



  §.§.§ Tower Construction (B1)

This experiment involves controlling a robotic arm to manipulate a set of blocks using actions from the set 𝒜 = {pick, place, stack, unstack} to arrange them in a specific order. Using the example of constructing a four-block tower, the template task begins with an initial state where all four blocks are placed on the table (i.e., without stacked blocks). The task goal state is achieved when the blocks are stacked in the sequence A on B on C on D. During the interaction between the robot and blocks, both task- and motion-level constraints should be considered. For instance, the action pick for block A is only permissible if it is clear (no blocks on it) and if a collision-free trajectory exists. 



  §.§.§ Workspace Reach (B2)

As an extension of the Tower Construction benchmark, this scenario involves the robot arm manipulating blocks with tools to address more complex long-horizon manipulation tasks. Blocks may be positioned beyond the reach of the robot arm. The template task begins with an initial state where a few blocks and one hook are placed on the table within the robot arm's workspace. It ends with the task goal state, where all blocks are placed on the shelf on the table. Actions stack and unstack are excluded to simplify the definition of the pull action, which describes instances where the hook is grasped by the robot arm to extend its workspace and pull the block within its original workspace for picking or placing. 



  §.§.§ Dual-arm Block Transportation (B3)

This benchmark further extends the Tower Construction experiment to validate the proposed algorithm in more complex multi-arm scenarios. The feasible action set 𝒜 = {pick, place, stack, unstack} remains the same, but with augmented parameters introducing two arms. This complexity leads to more instantiations of abstracted actions and results in a more intricate long-horizon manipulation planning task. The template task begins with an initial state where all blocks are on the left table without any blocks on top and concludes with the task goal state, where blocks are expected to be stacked in a given sequence as in B1.



 §.§ Disturbances

To comprehensively evaluate the generalization capability and reactivity of Logic-DMP across all benchmarks, we apply variants on initial states or disturbances at any states at different levels.


    
  L1 Motion-level Variant/Disturbance: Blocks are subjected to disturbances in different positions, while logical states remain consistent with the original or expected conditions;
    
  L2 Slight Task-level Variant/Disturbance: Blocks undergo disturbances, resulting in a different logical state. This logical state aligns with that seen in the demonstration for the template task;
    
  L3 Severe Task-level Variant/Disturbance: Blocks are disturbed to a novel and previously unseen logical state in the demonstration;
    
  L4 Extreme Task-level Variant/Disturbance: A new block is introduced into the scene that significantly influences the execution of the initial plan to achieve the target goal.




 §.§ Comparison between LQT-CP and DMP


In this section, we demonstrate the flexible motion modulation capabilities of LQT-CP, addressing two crucial sub-tasks in B2. The experimental results are depicted in Figure <ref>. In the initial picking sub-task, the robot arm is tasked with picking up a hook potentially placed at three distinct positions. Both LQT-CP and DMP exhibit comparable performance, effectively generalizing the reference trajectory to new goal positions, as showcased in Figure <ref>-Top.

In the subsequent pulling sub-task, the robot arm is required to pull cube A using the hook to three different goal positions represented by red, green, and blue cubes in Figure <ref>-Bottom. This task demands not only the generalization of the reference pulling trajectory to new goal positions but also the precise control of the hook passing through two crucial via-points (the top and the left-down corner of cube A) for successful pulling. Consequently, it necessitates more flexible motion modulation concerning via-points tracking and generalization. Figure <ref>-Bottom illustrates the actual position of the hook when expected to be at the via-points. Notably, LQT-CP adeptly generalizes the reference trajectory and via-points, successfully pulling cube A to the goal positions, while standard DMP encounters challenges, which highlights the flexibility of the proposed LQT-CP formulation of DMP for motion modulation.











 §.§ Generalization Ability of Logic-DMP
        




In this section, we assess the generalization ability of Logic-DMP in comparison to two baseline methods: Linear and PDDLStream. Here, Linear refers to the linear execution of a fixed sequence of DMPs without adapting the action sequence when addressing new tasks. For a comprehensive evaluation, each benchmark comprises tasks that share the same task goal but vary in their initial states from the template task. For B1, we solved a four-block tower construction problem, while tasks are simplified by utilizing only two blocks in B2 and B3. Moreover, LQT-CP serves as the motion modulation method for all baselines to ensure a fair comparison. We analyzed the overall success rate and computation time of the three methods in solving 100 tasks with random initial states for each benchmark across various levels (L1-L4). The summarized experimental results are presented in Table <ref>.

In summary, both Logic-DMP and PDDLStream successfully solved all the validation tasks, while the linear execution of DMPs achieves success in only approximately 25% of the tasks, primarily those featuring motion-level variants in the initial states. The results show that Logic-DMP can enhance the generalization ability beyond linear execution. Moreover, Logic-DMP exhibits faster planning capabilities than PDDLStream across all benchmarks, with a 30% to 40% improvement in B2 and B3, and a remarkable 70% improvement in B1. The substantial acceleration observed in  B1 can be attributed to the inherently longer action sequence in the task, with which the planning time of N/A increases significantly. Since Logic-DMP only needs to partially address the TAMP problem with a shorter action skeleton, it notably decreases the whole planning time, showing significant acceleration in the experimental results. This reinforces the efficacy of Logic-DMP in handling long-horizon manipulation tasks and motivates us to formulate the reactive TAMP framework in Algorithm <ref> by employing Logic-DMP in a closed-loop manner.



 §.§ Reactivity of TAMP with Logic-DMP

In this section, we conduct a comparative analysis of the reactivity exhibited by the proposed closed-loop Logic-DMP framework and two baseline approaches: Linear and RLDS. The Linear baseline involves the linear execution of a predetermined sequence, without closed-loop feedbacks. RLDS is a reproduction of the algorithm outlined in <cit.>, with the incorporation of the proposed LQT-CP for motion modulation. The experimental setup aligns with that detailed in Section <ref>, ensuring consistency across benchmarks. We assess the performance of each method in handling disturbances at different levels (L1-L4) and report the average execution times based on 10 trials for each disturbed task in Table <ref>.

Our analysis reveals that the proposed closed-loop Logic-DMP and RLDS demonstrate comparable reactivity in the presence of motion-level and slight task-level disturbances. Specifically, under L1 and L2 disturbances, both closed-loop Logic-DMP and RLDS effectively respond to the disturbances, and achieves task goals within comparable execution times. However, RLDS relies on backward checking of the planned action sequence without plan modification, leading to challenges in handling disturbances at L3 and L4 across all benchmarks. In contrast, Logic-DMP exhibits the ability to generate a new feasible action plan when faced with those disturbances, enabling the TAMP approach to accomplish tasks even under severe or extreme disturbances. Notably, Logic-DMP proves to be faster in deriving the new action plan compared to the straightforward application of TAMP solvers, as demonstrated in the comparison results between PDDLStream and Logic-DMP in Section <ref>, showing the significant reactivity inherent to the proposed closed-loop Logic-DMP approach.



 §.§ Real-world Experiments

We assessed the reactivity of the proposed Logic-DMP in closed-loop scenarios for handling various task-level disturbances within the four-block Tower Construction scenario, as shown in Figure<ref>. All experiments were conducted using a 7-axis Franka Emika robot arm equipped with a RealSense D435 camera for object localization. The entire planning algorithm demonstrates the capability to respond to severe or extreme task-level disturbances at the frequency of 1Hz, providing corresponding actions and generalized motion sequences for rearranging the objects into the desired goal configuration. Additional demonstrations of the reactive behaviors under various task-level disturbances can be found in the supplementary video of the project webpage.
















































§ CONCLUSION

In summary, we introduced Logic-DMP, an Learning from Demonstration (LfD) approach tailored for long-horizon manipulation tasks in dynamic environments. We leveraged an optimal control formulation of Dynamical Movement Primitive (DMP), Linear Quadratic Tracking with Control Primitives (LQT-CP), which naturally extends DMP to incorporate via-point specifications, proving beneficial for handling contact-rich manipulation sub-tasks. We demonstrated that Logic-DMP shows superior generalization ability than DMP and superior efficiency than TAMP solver in handling task variants during long-horizon planning. Consequently, we further extended this framework into a reactive TAMP system to quickly react to disturbances in dynamic environments. We validated the proposed methods through various simulation and real-world experiments, affirming the performances in term of skill transfer, generalization ability, and reactiveness to disturbances in long-horizon manipulation tasks.  


In this paper, we integrated DMP with TAMP solver because of its exceptional extrapolation generalization ability with one single demonstration. Notably, this integration is not confined to DMP alone. It can be extended to other LfD algorithms. An interesting avenue involves further extending Logic-DMP by integrating with diffusion models <cit.> to tackle more complex long-horizon contact-rich manipulation tasks. Moreover, we aim to extend the proposed method to other real-world scenarios characterized by partially observable environments, which necessitate rapid replanning based on new observations.

One drawback of Logic-DMP is its potential to find slightly longer solutions when generalizing to new tasks compared to directly applying TAMP solver. This is attributed to the consideration of goals in multi-goal specifications with the same priority. When multiple feasible solutions exist, Logic-DMP selects the one reached in the fastest way, but it doesn't guarantee that the chosen goal is the closest to the task goal state among all feasible solutions. Future work should explore strategies for assigning priorities to goals within the multi-goal specifications and the integration with optimization-based TAMP solvers <cit.> to solve long-horizon manipulation tasks where the optimality of metrics (e.g. time and energy efficiency) are important. 





IEEEtran

