Learning-based Manipulation Planning in Dynamic Environments Using GNNs and Temporal Encoding
Anonymous Author(s)
AffiliationAddress email
Abstract
Learning-based approaches have shown promising performance for improving theefficiency of motion planning in robot manipulation problems, but mostly in thesetting of static environments.
For the more challenging problem of manipulationplanning in dynamic environments, such as for multi-arm assembly tasks or humanrobot interaction, motion planners need to consider the trajectories of the dynamicobstacles, and reason about the temporal-spatial interactions between the ego-armand the other objects.
We propose a GNN-based neural architecture that involvestemporal encoding, and use imitation learning with data aggregation proceduresfor learning both the embedding and edge prioritization policies.
Experimentsshow that the learning-based approach can significantly accelerate online planningin comparison to state-of-the-art complete dynamic planning algorithms.
Theproposed methods can reduce costly collision checking operations by more than1000x, thus reducing the online planning time by over 95%, while also achievinghigh success rate on hard instances.
Introduction
Motion planning for manipulation tasks has been a longstanding challenge in robotics [2, 10].
Learning-based approaches can exploit patterns in the configuration space to accelerate planningand have shown promising results [29, 5, 3].
Existing learning-based approaches typically combinereinforcement learning (RL) and imitation learning (IL) to learn policies for sampling or rankingthe options at each step of the planning process [34, 45, 4].
Graph Neural Networks (GNNs) are apopular choice of representation for motion planning problems, because of their capability to capturegeometric information and are invariant to the permutations of the sampled graph [19, 22, 23, 44].
Manipulation planning in dynamic environments, such as for multi-arm assembly tasks or safemanipulation around humans is significantly more challenging than in static environments.
Althoughlearning-based approaches have shown success in static environments, it remains open whether theycan also accelerate manipulation planning in dynamic environments.
In dynamic environments, eachdynamic obstacle produces trajectories in the temporal-spatial space, and the motion planner for theego-arm needs to consider global geometric constraints in the configuration space at each time step(Figure 1).
This dynamic nature of the environment generates a larger state space of sequences ofgraphs, which is also very sensitive to the change in one single dimension: time.
A small change inthe timing of the ego-robot or the obstacles, in two spatially similar patterns, may result in completelydifferent planning problems.
For instance, the dynamic obstacle may create a small time window forthe ego-robot to pass through, and if that window is missed then the topology of configuration spacemay change globally.
Consequently, we need to design special architectures that can both encode the graph structures well, and also be able to robustly infer temporal information.
Indeed, completesearch algorithms for dynamic manipulation planning, such as variations of the leading method of
Safe Interval Path Planning (SIPP) [32, 24], focus on reasoning about the temporal intervals that aresafe for the ego-robot.
The complete algorithms require significantly more computation and collisionchecking operations compared to the static setting, often making it impractical to consider dynamicmanipulation planning problems in practice.
We propose a novel Graph Neural Network (GNN) architecture manipulation planning in dynamicenvironments.
We follow the sampling-based motion planning methods [18, 17], where path planningis performed on a sampled graph from the configuration space.
The architecture takes into accountof the overall sampled graph in the configuration space, the obstacle’s trajectory in the work space,and the current state of the ego-robot.
The output is a vector of priority values on the candidateedges at the current state of the robot.
The encoding is performed in two stages.
In the first stage,we encode the graph structure information into the sampled graph using attention mechanisms [41],and also design a temporal encoding method for the obstacle trajectories.
The temporal encodinguses the idea of positional encoding from the Transformer and Nerf [41, 26], which encourages theneural network to learn the temporal pattern from high-frequency input signals.
In the second stage ofencoding, we take into account of the ego-robot’s current vertex in the configuration space, the localgraph structure, and the current time-shifted trajectories of the obstacles.
This two-stage structureis a significant extension of the previous use of GNNs in static environments [22, 23, 44] and it isimportant for making high-quality predictions on the priority values.
The entire GNN of both stageswill be trained simultaneously in an end-to-end fashion.
Because of the complexity of the GNN architecture, we found that RL-based methods can not traingeneralizable models based on the architecture, and imitation learning with data aggregation (DAgger)is the key.
We utilize the complete dynamic planning algorithm SIPP [32] as the expert to first performbehavior cloning as a warm-up training, and then allow the ego-robot to self-explore and learn fromthe expert following the DAgger method [36, 11].
We evaluate the proposed approach in various challenging manipulation environments ranging from2-DoF arms to 7-DoF KUKA arms.
Experiments show that our method significantly reduces collisionchecking by more than 1000x compared to the complete algorithm SIPP, which leads to reducing theonline computation time by over 95%, and also achieves high success rate compared to other learningand heuristic baselines.  Related Work
Manipulation Planning in Dynamic Environments.
The planning in dynamic environment hasproven to be difficult [35].
The work of [32] and its variations [9, 28, 24] are the leading frameworkfor manipulation planning in dynamic environments.
The method significantly reduces the temporalspatial search space by introducing a new data structure by grouping safe configurations over a periodof time safe intervals , [32].
Based on this data structure, Safe Interval Path Planning (SIPP) can findthe optimal paths on a configuration graph.
Further improvements are made to improve the efficiencyof SIPP by leveraging state dominance [9] or render the any-time search feature [28].
Recently, [24]shows SIPP is also powerful in planning the paths of high-dimensional manipulators on configurationroadmaps [18] with the presence of dynamic obstacles, which is the problem of interests in this paper.
Many other algorithms consider path planning in dynamic environments for mobile robots [12, 31, 6].
Such problems are typically more efficiently solvable because of the low dimensionality and the useof more conservative planning methods.
It is common to bound the robot to avoid the occupied spaceof moving obstacles for the entire or receding horizons.
Learning-based Motion Planning.
Learning-based approaches typically consider motion planningas a sequential decision-making problem, which could be naturally tackled with reinforcementlearning or imitation learning.
With model-based reinforcement learning, DDPG-MP [16] integratesthe known dynamic of robots and trains a policy network.
[38] improves the obstacle encoding withthe position and normal vectors.
OracleNet [1] learns via oracle imitation and encodes the trajectoryhistory by an LSTM [13].
Other than greedily predicting nodes that sequentially form a trajectory,various approaches have been designed to first learn to sample vertices, then apply search algorithmsto the sampled graphs [14].
[15, 34, 45] learn sampling distributions to generate critical verticesfrom configuration space.
Inductive bias is also leveraged by several works, including [25, 4, 21],which design neural networks that take structured inputs.
Existing learning-based approaches havealso considered improving collision detection [5, 3, 44] and learning to explore edges given fixedgraphs [39, 21].
For non-static environments, most works focus on multi-agent scenarios [30, 11],where there do not exist non-cooperative dynamic obstacles.
Graph Neural Networks for Motion Planning.
Graph neural networks are permutation invariantto the orders of nodes on graph, which become a natural choice for learning patterns in graphproblems.
For motion planning, [19] utilizes GNN to identify critical samples.
[22, 23, 7] predictsthe action for each agent in the grid-world environment using graph neural networks.
[40, 20] learnsthe control policy for each robot in a large-scale swarm.
[46] learns the submodular aciton selectionfor continuous space.
[44] learns a GNN-based heuristic function for search in static environmentwith completeness guarantee.
Preliminaries
Sampling-based Motion Planning with Dynamic Obstacles.
We focus on the sampling-basedmotion planning, in which we sample from the configuration space C ⊆ R n where n is the numberof degree-of-freedom for the ego-robot.
All the sampled vertices form a graph G = ⟨ V, E ⟩ , withstart vertex v s and goal vertex v g .
The connectivity in the graph is determined by r-disc or k-nearestneighbor (k-NN) methods[8, 43].
In the setting of manipulation planning, we assume access tothe full trajectories of the dynamic obstacles, which are often the other robot arms.
We representthe trajectory of dynamic obstacles in the workspace as the vector of all the joint positions in timewindow T ⊆ R ≥ 0 .
The goal of the manipulation planning problem is to find a path from v s to v g inthe sampled graph that is free of collision with the dynamic obstacles at all time steps in T .
Graph Neural Networks (GNNs).
GNNs can efficiently learn the representation of each vertex andedge on a graph G = ⟨ V, E ⟩ by message passing from neighbors.
With MLP networks f and g , GNNencodes the representation h ( i k + 1) of vertex v i after k aggregations as
[Equation]
where the initial data of vertex v i is x i and h (1) i
= x i .
⊕ is the permutation-invariant aggregationfunction on sets, such as mean, max or sum.
Attention Mechanisms.
For the purpose of introducing the dynamic obstacles in our network, whilereasoning about the temporal-spatial interactions between the ego-arm and the obstacles, we useattention mechanism to encode the obstacle features.
In the general form of attention mechanism,there are n keys each with dimension d q : K ∈ R n × d k , each key has to a value V ∈ R n × d v .
Givenm query vectors Q ∈ R m × d k , we use a typical attention function
Att ( K, Q, V ) for each query as Att ( K, Q, V ) =
softmax ( QK T / √ d k ) V [41].
Imitation Learning.
Imitation learning aims to provide guidance to train policy without explicitly designing reward functions.
Our model also benefits from this technique which follows thedemonstration by the oracle and learns mappings from the observations to actions.
Specifically, inthe task of sampling-based motion planning, we follow the oracle by predicting the priority valueof subsequent edges and prioritize the one given by the oracle.
However, imitation learning canresult in poor performance in practice, in the cases that the ego-arm can stray from the right pathand cannot recover due to the lack of enough guidance from the oracle.
To this end, imitation withdata aggregation (DAgger) [36] provides an iterative algorithm to generate new data by querying theoracle after executing the current policy, then trains a new policy after aggregating the dataset.
GNN-TE: Dynamic Motion Planning with GNNs and Temporal Encoding
4.1 Overall Architecture
Our dynamic motion planning network GNN-TE leverages the graph and temporal nature of thedynamic motion planning task.
It consists of two stages.
The first stage is the global GNN encoder
N G which encodes the global information about the ego-arm and obstacles in the environment.
Thesecond stage is the local planner N p which executes motion planning utilizing the encoding output ofthe first stage.
Fig 2 shows the two-stage architecture of our network.
First-stage global GNN Encoder N G The GNN encoder N G takes in a sampled random geometricgraph G = ⟨ V, E ⟩ , V = { v s , v g , v } .
For an n -dimensional configuration space, each vertexv i ∈ R n +1 contains an n -dimensional configuration component and a 1 -dimensional one-hot labelindicating if it is the special goal vertex.
The vertices and the edges are first encoded into a latent space with x ∈ R | V |× d h , y ∈ R | E |× d h ,where d h is the size of the encoding.
Specifically, to encode the graph while informing the networkof the task specific goal, we choose x = h x ( v, v g , v − v g , || v − v g || 22 )
y
= h y ( v i , v j , v j − v i ) , whereh x and h y are two different two-layer MLPs.
The L2 distance to the goal || v − v g || 22 serves as theheuristic information for N G .
The dynamic obstacles O form barriers on the top of the graph G , and we incorporate the trajectoriesof them to N G to leverage the global environment and assist the planning of N P .
Additionally, toinform the networks about the relative time over the horizon, we incorporate temporal encodingwith the obstacles, i.e. O = O + TE , which we will discuss in the later section.
With the sequentialproperty of the trajectory, we use the attention mechanism to model the temporal-spatial interactionsbetween the ego-arm and the obstacles.
Concretely, the obstacles are encoded into the vertex andedge of G as:
[Equation]
Taking the vertex and edge encoding x, y , the GNN N G aggregates the local information for eachvertex and edge from the neighbors, by performing the following operation with 2 two-layer MLPsf x and f y :
[Equation]
Note that here we use max as the aggregation operator to gather the local geometric information,due to its empirical robustness to achieve the order invariance [33].
The edge information is alsoincorporated into the vertex by adding y l as the input to f x .
Also because the Equation 4 is ahomogeneous function that updates on the x and y in a self-iterating way, we can update without introducing redundant layers loops
After the Second-stage local planner N P After the GNN N G encodes the information of the configurationgraph and obstacles, the second-stage local planner N P utilizes the encoding and performs motionplanning.
Specifically, when arriving at a vertex v i at time t i , N P predicts the priority value η j of allthe subsequent edges e j ∈ E j with the expression η j = g ( y l , O t
i − w , O t i − w +1 , ...,
O t i + w − 1 , O t i + w ) ,where g is a multi-layer MLP.
Note that apart from the encoding of the subsequent edge, we alsoinput the local obstacle encoding within a time window w of the current arrival time t i .
This servesas the local-level information for the network N P to plan towards the goal vertex while consideringthe barriers of dynamic obstacles to avoid collisions.
In the inference time, we use N P to choose theedge with the highest priority value while keeping track of the current time.
4.2 Temporal Encoding
Spatial encoding is a crucial design in the Transformer architecture [42] for making use of the order ofthe sequence.
Dynamic motion planning requires the models to infer the relative position of obstaclesand how they interact with the ego-arm at each time step.
So along with the positions of the obstaclesin the workspace, we add temporal encoding TE ∈ R T × d TE at time t with k -th dimension TE ( t, 2 k ) = sin( ω − 2 k/d TE t ) and TE ( t, 2 k + 1) =
cos( ω − 2 k/d TE t ) (5) where ω ∈ Z + is a fixed frequency.
We choose the temporal encoding to have the same dimensionas our obstacle encoding and add them together before input into the networks N G and N P as theobstacle encoding.
We illustrate the overall encoding procedure on the left of Figure 3.
4.3 Training and Inference Procedures
In each training problem, along with the dynamic obstacles O , start vertex v s and goal vertex v g ,we sample a k-NN graph G = ⟨ V, E ⟩ , V = { v s , v g , v } , where v is the vertices sampled from theconfiguration space of the ego-arm.
At the first stage, we use the global GNN encoder N G to encodethe graph and dynamic obstacles, and the local planner N p in the second stage uses the encoding asthe input to predict the priority value η of the subsequent edges.
Imitation from SIPP with Data Aggregation We train our two-stage network N G and N p in anend-to-end manner by imitating an oracle.
Specifically, we use Safe Interval Path Planning (SIPP)[32] to compute the shortest non-collision motion path and use it as the oracle.
In the first stage, N G will process the graph and the obstacle trajectories and output the encodedfeatures of each vertex and edge.
Then in the second stage, we train the networks to imitate theoracle SIPP along the optimal path.
Concretely, starting from the v s , SIPP provides the optimal pathπ ∗ =
{ ( v ∗ i , t ∗ i ) } i ∈ [0 ,n ] with the vertex v ∗ i and the corresponding arrival time t ∗ i .
When arriving atthe vertex v ∗ i at time t ∗ i , our local planner N p will input the edge feature y l along with the obstacleencoding in the time window [ t ∗ i − w , t ∗ i + w ] to predict the priority value of all the subsequent edges
E i .
Then it prioritizes the next edge on the optimal path e ∗ i : ⟨ ( v ∗ i , t ∗ i ) , ( v ∗ i +1 , t ∗ i +1 ) ⟩ among E i .
Wemaximize the priority value η e ∗ i of e ∗ i over all other e
i ∈ E i \ { e ∗ i } with the standard cross entropyloss L i = − log(exp ( η e ∗ i ) /
(Σ e i ∈ E i exp( η e i ))) .
Since SIPP only provides the guidance on the optimal path, when the planned path of N p deviatesfrom the optimal path, our network cannot sufficiently imitate the oracle.
To this end, we use
DAgger [36] to encourage the network to learn from these sub-optimal paths.
We first train ournetwork for k iterations with pure demonstrations from SIPP.
Then we explore a path π k on the graphusing the priority value predicted by the current network, which may not reach the goal vertex v g orbe optimal.
We randomly stop at the vertex v ki at time t ki and query the oracle.
SIPP treats v ki and t kias the start vertex and the start time respectively, along with the obstacles trajectory starting at t ki ,calculates the optimal path.
The new demonstrations are aggregated to the previous dataset to keeptraining the network.
Inference with Time Tracking.
Given a graph G = ⟨ V, E ⟩ , V = { v s , v g , v } , with the trajectoriesof obstacles O , N G will first encode the graph and obstacles.
Next, N P executes motion planningby predicting the priority values and follow the edge with the maximum one, while keeping trackof the arrival time at each vertex, e π i = argmax e i ∈ E i N P ( η i | V, E, O, N G , t i ) .
Note that e π i is also conditioned on the current arrival time t i .
The priority value η i is the output of N P when we arriveat vertex v i at time t i .
After the edge e π i is proposed by the network, we check the collision on e π istarted at t i .
If there is no collision, we add ( e π i , t i ) into the current path π , otherwise, we query N Pfor another proposed edge with the next greater priority value.
The planning will end if we succeedin finding a path π from v s to v g or fail when stopping at a vertex with all the subsequent edges withcollisions.
Optionally, when the latter one happens, we can backtrack to the last step and query forthe top-5 greatest priorities in turn.
Further discussions are covered in the experiments section.
Experiments
Our approach significantly reduces collision checking from 3000x to 4000x compared to the complete algorithm SIPP, improves the overall planning efficiency, and achieves high success rate
Experiment Setup.
We evaluate the proposed methods on various multi-arm assembly tasks wherewe manipulate the ego-arm and avoid the collisions with other obstacles, including moving arms andstatic obstacles.
Specifically, the environments cover arms of 2 to 7 Degree of Freedom and 1 to 3moving obstacle arms, including (i) 2Arms: 1 obstacle arm with 2 DoF. (ii) Kuka-4DoF: 1 obstaclearm with 4 DoF. (iii) Kuka-5DoF: 1 obstacle arm with 5 DoF. (iv) Kuka-7DoF: 1 obstacle arm with
DoF. (v) 3Arms: 2 obstacle arms with 2 DoF. (vi) Kuka3Arms: 2 obstacle arms with 7 DoF. Withoutloss of generality, we assume all the robot arms in an environment are of the same DoF and move atthe same and constant speed.
The static obstacles are represented as cuboids in the experiments.
Baselines.
We compare our method GNN-TE with the lazy sample-based dynamic motion planningmethod Dijkstra-Heuristic ( Dijkstra-H ).
It prioritizes the edges based on the shortest distance to thegoal on the graph at each time step.
We also compare our method with the oracle SIPP [32], which isa search algorithm that generates optimal paths using safe intervals.
Datasets.
We randomly generate 2000 problems for training and 1000 problems for testing.
Ineach problem, we randomly generate fixed-length trajectories of the moving obstacle arms and 1000samples in the configuration space of the ego-arm that are collision-free with the static obstacles andprovide them to the oracle to generate demonstration trajectories.
In order to further investigate theperformance of algorithms in the challenging environments, we also generate 1000 hard problems,where Dijkstra-H fails to find feasible paths.
Training and Testing Details.
We first train the GNN-TE on all the training problems for 200epochs.
Afterward, we generate 1000 new training data with DAgger, and trained for another 100epochs.
We test all the algorithms on the provided graphs.
We randomly split the test problems into 5groups, and calculate the variance of the performance.
Evaluation Metrics.
We measure the average collision checking of the common success casesto evaluate the effectiveness of algorithms in avoiding obstacles.
Collision checking is the moreexpensive computation in motion planning, and reducing it can lead to a significant acceleration ofonline computation.
We also report the average success rate over all the testing problems for all thealgorithms.
A trajectory is successful, only if it does not collide with any obstacles, and eventuallyreaches the goal given a limited time horizon.
We also measure the optimality of the methods bycomparing the path time ratio compared to the optimal paths found by SIPP on each success case.
Note that the path time metric only measures the cost of the planned path, and does not include theonline computation time needed.
5.1 Overall Performance
[Figure]
Figure 4 shows the overall performance of thealgorithms in various environments, includingall the baselines and GNN-TE without DAgger,i.e. pure imitation learning from the oracle.
In all the environments, SIPP gives the optimal complete non-collision path, but it suffersfrom the excessive amount of collision checking.
GNN-TE significantly reduces the collision checking by more than 1000x, which corresponds to reducing online computation timeby over 95%.
At the same time, the methodshave high success rates and better path time ratiocompared to using simpler heuristics.
Fig.
5 shows the performance snapshots of thealgorithms on a 2Arms and a Kuka-7DoF testcase.
In both cases, our method successes inplanning a near-optimal path compared to theoracle SIPP whereas Dijkstra-H fails.
Performance on randomly generated test problems.
As shown in Fig.4, our methods significantlyreduces the collision checking by over 1000x compared to SIPP , i.e. 60k, 97k, 171k, 197k, 269k,269k to 17.24, 33.08, 37.36, 56.89, 61.99, 72.63 on 2Arms, 3Arms, Kuka-4DoF, Kuka-5DoF, Kuka7DoF, Kuka3Arms respectively.
Because SIPP needs to search in both space and time dimensions, itsuffers from a large number of collision checking.
Our approach benefits from learning and achievesa minimal amount of collision checking even in the dynamic setting while not compromising muchof the success rate, in which case our method outperforms Dijkstra-H on all the environments withhigher success rates.
Note that as a crucial part of the training procedures, DAgger also improves theperformance in all environments.
Performance on hard test problems.
On the hard test problems, Dijkstra-H fails to find feasiblepaths in the dynamic environment.
Comparatively, our GNN-TE can successfully find solutions tothese problems with considerable success rate and acceptable path time ratio.
It is also worth notingthat DAgger can better assist to improve the performance in the more challenging scenarios comparedto the random generated problems.
Optional backtracking search.
In the Appendix, we also report the result of GNN-TE and DijkstraH and them with backtracking GNN-TE w. BT and Dijkstra-H w. BT on 2Arms.
On the samealgorithm, the optional backtracking search will only result in a higher success rate while not affectingpath time ratio and collision checking on the common success cases.
The result of Dijkstra-H w. BTshows that although it improves the success rate significantly but sacrificing a tremendous numberof collision checking.
Nevertheless, our method outperforms the heuristic method on both with orwithout the backtracking search.
We have demonstrated that
See Section
Comparison with End-to-End RL.
We also compare our approach with RL approaches, including
DQN [27] and PPO [37].
The neural network architectures for these two baselines are implementedwith the same GNN architectures as ours.
We observe that even in 2Arms, the average success rate of
DQN on the training set is only around 54%, while PPO only has a success rate of around 20%.
Theyfail to find plans in randomly sampled test cases.
We provide more details in the Appendix.
5.2 Ablation Study
We perform ablation studies on 2Arms of the different encoding in our model, including global andlocal obstacle encoding and temporal encoding.
The results are shown in Fig.
Global Obstacle Encoding.
At stage 1, we both encode the configuration graph and the globalobstacle trajectories with GNN-based global encoder.
To investigate the effectiveness of leveragingglobal obstacle information, we conduct the experiment that we input the sampled configurations atstage 1 and only introduce the obstacle information at stage 2.
As we can observe in the figure thatalthough the path time ratio on the random problems slightly increases, the performance of path timeratio, success rate and collision checking improves in other scenarios.
This results from the modelreceiving the overall trajectories of the obstacles, and the encoding helps to reduce collision checkingand improves success rate especially on the hard problems.
Local Obstacle Encoding.
We compare our model with the one omitting the local obstacle encodingat stage 2.
It only inputs the temporal encoding corresponding to the arrival time as the time indicatorfor planning.
The result has shown that the local obstacle encoding within a time window directlyhelps the local planner perform better.
Temporal Encoding.
We analyze the importance of temporal encoding of time, where we remove itfrom both two stages and only input the trajectory of obstacles in the models.
The results also showthat temporal information helps GNN-TE making use of the order of the trajectory sequence on bothrandom and hard problems.
Discussion and Conclusion
We proposed a GNN-based neural architecture GNN-TE for motion planning in dynamic environments.
Temporal encoding is an important part of the GNN.
We use imitation learning with DAggerfor learning both the embedding and edge prioritization policies.
We evaluate the proposed approachon various environments, ranging from 2-DoF arms to 7-DoF KUKA arms.
Experiments show thatthe proposed approach can reduce costly collision checking operations by more than 1000x andreduces online computation time by over 95%.
Future steps in this direction can involve using ideasfrom the complete planning algorithms, such as incorporating safe intervals, to improve the successrate on hard instances, as well as more compact architectures for further reducing online computation.