Whole-Body Neural Policy for Zero-Shot Cross-Embodiment Motion Planning

Authors: prabin  kumar
nakul  gopalan
Venue: roboticsfoundation_org_RSS_2024_Workshop_EARL
Published: 2024-06-15 01:51:16

Abstract: Classical manipulator motion planners work across different robot embodiments. However they plan on a pre-specified static environment representation, and are not scalable to unseen dynamic environments. Neural Motion Planners (NMPs) are an appealing alternative to conventional planners as they incorporate different environmental constraints to learn motion policies directly from raw sensor observations. Contemporary state-of-the-art NMPs can successfully plan across different environments.  However none of the existing NMPs generalize across robot embodiments. In this paper we propose Cross-Embodiment Motion Policy (XMoP), a neural policy for learning to plan over a distribution of manipulators. XMoP implicitly learns to satisfy kinematic constraints for a distribution of robots and $\textit{zero-shot}$ transfers the planning behavior to unseen robotic manipulators within this distribution. We trained XMoP on planning demonstrations from over three million procedurally sampled robotic manipulators in different simulated environments. Despite being completely trained on synthetic embodiments and environments, our policy exhibits strong sim-to-real generalization across manipulators with different kinematic variations and degrees of freedom with the same set of frozen policy parameters. We show sim-to-real demonstrations on two unseen manipulators solving novel planning problems in different real-world environments even with dynamic obstacles. Videos are available at https://sites.google.com/view/xmop.

Main paper: 
 i. introduction: 
 Motion planning for robotic manipulators is the task of finding a sequence of robot configurations connecting a start joint state to the goal joint state while respecting joint limits of the robot and avoiding obstacles. Even after decades of research in this domain, real-time motion planning in complex unseen environments is still a challenging problem [29, 21, 24] .Classical motion planners either use random sampling to explore the configuration-space [18, 17, 16, 28] or employ gradient-based optimization methods [23, 15, 26, 8] to search for a valid plan. While these algorithms generalize across embodiments, they often demand a non real-time computation budget for generating desired motion behaviors in geometrically complex environments [21, 28] . Furthermore, these algorithms conventionally assume the availability of a precomputed geometric representation of the robot's workspace for state validation, thus making them unscalable in unseen environments with novel types of obstacles. To overcome these limitations, Neural Motion Planners (NMP) learn to generate trajectories directly from visual observations [22, 12, 14, 33, 5, 9] . However, these policies are individually trained on data from a single manipulator, trading-off the cross-embodiment flexibility offered by classical planners that are agnostic to a robot's morphology.We identify two fundamental problems that have deterred learning cross-embodiment motion planning. First, different manipulators have varying kinematic properties such as link lengths and joint limits, as well as diverse morphologies characterized by their degrees of freedom. Each manipulator operates within a particular configuration-subspace bounded by its joint limits. Thus, training a single neural policy to generate actions spanning multiple bounded subspaces renders cross-embodiment motion planning a challenging task to learn. Second, data for training cross-embodiment policies is difficult to gather as there are only a limited number of embodiments available commercially, which do not fully capture the distribution of possible kinematic variations.To address the above challenges, we present Cross-Embodiment Motion Policy (XMoP), a framework of datadriven methods to learn neural policies for cross-embodiment motion planning. Our contributions are outlined as follows:• Our novel control policy uses the robot's physical description, i.e., URDF [30] to operate on link-wise SE(3) observations and plans for task-space end-effector targets across a distribution of 6 and 7 DoF manipulators, enabling zero-shot configuration-space plan generalization. • We propose a 3D semantic segmentation-based model for perceptual cross-embodiment collision detection that shows a 98% recall and zero-shot transfers to real-world unseen environments. • Finally, we combine the control policy with the collision model under a model-predictive framework, achieving 70% success rate for motion planning with unseen robotic manipulators. To the best of our knowledge, XMoP is the first neural policy for configuration-space planning that zero-shot transfers to unseen robotic manipulators. We demonstrate sim-to-real transfer on Franka FR3 and Sawyer robots solving novel planning problems in unstructured real-world environments.Prior methods for neural motion planning directly predict configuration-space actions that do not generalize across embodiments [22, 12, 14, 33, 5, 9] Fig. 1 : XMoP perceives the embodiment state as a sequence of whole-body SE(3) link poses pt and predicts link-wise pose transformations Tt+1:t+H over a horizon H to move the end-effector towards the goal pose gt. We use a Transformer [31] base policy architecture, that operates on an input pose sequence (pt, gt) and uses self-attention to convert the query tokens into a sequence of link-wise relative pose transformations. Contextual information is provided to the Transformer using three types of position embeddings: (1) LPE is a fixed set of sinusoidal position embeddings that are repeated over the horizon providing kinematic chain awareness for every horizon step; (2) HPE and (3) CPE, are learned position embeddings providing awareness for horizon and input, respectively. Additionally, we use novel attention masking strategies within the Transformer for cross-embodiment adaptation. The predicted link-wise transformations are multiplied with the instantaneous link poses to reconstruct the future whole-body pose of the manipulator, which is reached by using a whole-body IK procedure to retrieve the configuration-space joint positions.provides the future states of manipulator over a horizon of H steps. The instantaneous state observation for the manipulator is represented as a sequence of rigid-body SE(3) link poses with respect to the robot's base i.e., p t ∈ R D×4×4 , where D is the manipulator's degrees of freedom. The goal g t ∈ R 4×4 represents the end-effector SE(3) target for motion planning. Our policy π(a t |p t , g t ) as shown in Fig. 1 , learns to predict link-wise relative SE(3) transformations a t = T t+1:t+H for reconstructing the whole-body manipulator pose in future time steps. The formulation for the transformation target T t+k ∈ R D×4×4 for k ∈ {1, ..., H} is shown in eq. 1.EQUATIONwhere link poses p t are obtained using the manipulator's forward kinematics function ϕ(j t ), with j t ∈ R D as the instantaneous configuration-space observation. The configurationspace action in future time step j t+k is retrieved from the predicted whole-body pose pt+k = Tt+k p t by solving for whole-body IK using the following constrained optimization procedure:min j t+k ∥p t+k -ϕ(j t+k )∥, s.t. j L < j t+k < j U (2)where j L and j U are the lower and upper joint limits of the manipulator. The above optimization objective is non-convex, and hence a close initial guess is required for convergence. We address this issue by collecting dense planning demonstrations with a maximum per-joint deviation of 0.05 rad. Thus, making the instantaneous observation j t to be an initial guess that lies within the close neighbourhood of j t+k . In practice, we employ additional stochasticity to handle redundancy and singularities in manipulators.We formulate the whole-body transformation policy π θ (a t |p t , g t ) as a stochastic diffusion policy [7] parameterized by θ that predicts a batch of possible future trajectories for model predictive control. While training, the noise prediction model ϵ θ takes the noisy sample a τ t , which is obtained by applying the forward diffusion process to a 0 t = T t+1:t+H , where τ is the diffusion step. Additionally, we also pass c = (p t , g t ) for observation and goal conditioning. For step conditioning, we follow the adaptive layer normalization strategy proposed in Diffusion Transformers [20] . We train the noise prediction model using mean square error loss as shown in eq. 3 which minimizes the variational lower bound of the KL-divergence between the original data distribution p(a 0 t ) and the DDPM [11] q(a 0 t ) distribution.EQUATIONOur policy utilizes the Transformer [31] model as the underlying backbone which expects input in a sequence format. We emphasize on four key design decisions in our policy: 1) SE(3) Proprioception: The embodiment state is provided to and queried from the policy as a sequence of SE(3) pose-tokens, allowing the policy to learn motion synergies between rigid-body links. We use the noisy sample a τ t as transform query, which is passed to the Transformer as pose-tokens, as shown in Fig. 1 .2) Kinematic Masking: We introduce an inductive bias for kinematics by restricting attention to parent or ancestor links at the current horizon step, and to the same link at both the current and previous horizon steps. 3 ) Morphology Adaptation: To enable learning over different morphologies, we fix the pose-token for the end- effector and mask out the pose-tokens of unavailable links. 4) Link-Horizon Position Embedding: Contextual information is provided to the Transformer using position embeddings for query and input pose-tokens. Fig. 1 shows the position embedding scheme used in XMoP. Prior works have used link-wise tokens [10] , temporal state inputs [13, 25] , action diffusion [7, 13, 5] , and SE(3) pose observations [19, 27] for learning and planning applications. We combine these ideas with our whole-body pose transformation method to learn neural policy for cross-embodiment motion planning.We formulate collision detection as a semantic segmentation problem for identifying the links of the robot that are in collision given a pointcloud observation of the workspace. Our semantic collision detection model XCoD : R 4 → R 2 takes segmented pointcloud of the workspace as input where each point consists of 3D spatial coordinates, and a semantic label. These points are uniformly sampled from the surface of individual links of the manipulator and scene obstacles for the future time steps as predicted by our planning policy π θ . We assign unique semantic labels to every link corresponding to its degree of freedom, while using a reserved label for all obstacles. For training the collision model, we utilize point-wise binary label y, where collision-free link points are assigned a training label of 0, whereas link points in collision are assigned a training label of 1. Fig. 2 shows planning scenes and corresponding colorized pointclouds highlighting pointwise training labels. We utilize a Point Transformer V3 [32] model for the semantic segmentation task and train it using cross-entropy loss along with an additional surrogate Lovász hinge loss, that has shown to improve semantic segmentation performance in prior works [4] .We use XCoD to assign scores for a batch of trajectories predicted by XMoP and choose the least collision future trajectory for locally reactive planning. eq. 4 shows the formulation of the proposed Model Predictive Control (MPC) method where N is the number of surface points sampled from the manipulator, B is the prediction batch size, and ŷ is the perpoint collision logit from XCoD.a * t = a (q)t , q = arg min[s 1 , s 2 , . . . , s B ],EQUATIONSimilar to diffusion Policy [7] , we predict for H p steps while executing only H a steps on the manipulator. XMoP avoids geometric biases from manipulator design, by conditioning on task-space observations (SE(3) poses and pointclouds) that implicitly promote cross-embodiment and sim-to-real generalization.Kinematic Templates. Synthetic manipulators are represented with open kinematic chains connecting a series of rigid-body links. We design these links using axis-aligned cylinders forming a rigid-body template. Each link template is parameterized with the following information: (1) length of the cylinders, (2) radius of the cylinders, and (3) constraints for the joint that connects the link to the preceding link. We follow the design pattern of two commercially available robots: (1) 6-DoF UR [2] (2) 7-DoF Sawyer [1] . Fig. 1 (left) shows a composed manipulator sampled from our synthetic embodiment distribution by randomizing the parameters for constituent link templates. We adopt the 3.27 million synthetic planning problems from the MπNets dataset [9] and generate demonstration data by sampling a unique embodiment for each problem and solving it using the AIT * [28] motion planner.Data Augmentation. During training, we randomize the position and orientation of the link frames for pose computation by uniformly choosing cylinders from the constituent kinematic templates. With this frame augmentation, the number of possible sequences for a single manipulator is 3 DoF which promotes cross-embodiment generalization during training.We evaluate XMoP on 7 different robotic manipulators from 5 commercial manufacturers: Franka Panda, Rethink Sawyer, Kuka IIWA, Kinova Gen3 6-DoF, Kinova Gen3 7-DoF, Universal Robots UR5, and Universal Robots UR10. For each manipulator we use a set of 500 novel problems from the MπNets [9] test distribution, ensuring that valid collision-free IK solutions exist for both start and goal end-effector targets. It should be noted that none of the robots in our benchmark experiments or real-world demos were part of the training dataset. All of our results are zero-shot evaluations with single pre-trained checkpoints.We evaluate XMoP on benchmark problems for each commercial manipulator. Policy rollouts are terminated if the manipulator's end-effector reaches the goal or a maximum of 200 rollout steps are exhausted. We consider a goal to be reached when the L2 norm of the SE(3) pose difference between end-effector and goal is less than a pre-specified threshold of 0.01. Baseline Planners We compare our policy against the upper performance threshold of AIT * [28] planner that has access to an oracle collision checker from PyBullet. We know that our performance will be worse than AIT * which has access to oracle environment state, however it is the best option available to us for cross-embodiment evaluation as there is no neural motion planner baseline that works across unseen robot embodiments. Thus, we do not have a neural motion planning baseline in this work. We also evaluate generalization capabilities of our learned collision model XCoD by combining it with the AIT * baseline. This hybrid planner utilizes the XCoD model for collision queries, where a state is considered invalid if the ratio of detected link points in collision to the total number of manipulator points is greater than 0.001.Embodiment XMoP+XCoD AIT * +XCoD AIT * +PyBullet SR[%] ↑ PL ↓ ST[s] ↓ SR[%] ↑ PL ↓ ST[s] ↓ SR[%] ↑ PL ↓ ST[We utilize the following quantitative metrics to evaluate the planning performance: (1) Success Rate (SR) -A trajectory is successful if the final end-effector position is within 1 cm and orientation is within 5 • of the goal, with no collisions or joint limit violations. (2) Path Length (PL): Sum of L2 norm between consecutive configuration-space way points. (3) Solution Time (ST): Total time elapsed to generate a successful trajectory. Table I shows the benchmark results.Plan Optimality. Table I shows that the oracle AIT * baseline generates approximately 50% more optimal plans and is 10 times faster compared to XMoP and XCoD MPC policy. However, this comes with the assumption of privileged obstacle information being available to PyBullet for collision detection, thus making it infeasible for deployment in unstructured real-world environments.Zero-shot Generalization. Table I demonstrates XMoP's ability to generalize to manipulators with novel designs that were unseen during training, as our training data includes only the design patterns of Sawyer and UR robots. XMoP exploits the fact that motion behavior is characterized by the whole-body pose of the embodiment [3, 6] . For similar wholebody poses, different manipulators might have contrasting joint configurations, but the link poses are relatively closer in SE(3). Similarly, configuration-space actions for different manipulators are dependent on their morphology, but link pose transformations are similar for individual manipulator links.Kinematic Constraints Satisfaction. Our policy predicts link-wise transformations that obey different kinematics constraints across manipulators from a single pose sequence observation. We will investigate these properties of XMoP in future work.Collision Detection. As shown in Table I , the hybrid planner achieves an average 82.4% success rate, which is within 10% of the oracle baseline, thus highlighting the effectiveness of our collision detection model. Although XCoD is trained on complete scene point clouds in simulation, it shows strong generalization to unseen partial point clouds captured from depth cameras in the real world.Sim-to-Real Experiments. We deployed XMoP on two commercial manipulators, Franka FR3 and Sawyer, in different unseen real-world environments. Appendix Fig. 3 shows intermediate snapshots of our policy rollouts. We used monocolor obstacles and segmented them from a calibrated depth camera to extract the obstacle point cloud.Limitation. As with any behavior cloning algorithm, our policy performance is limited by the quality of the synthetic demonstration data available for training and struggles to plan for out-of-distribution goal poses and environment setups.Conclusion. In this paper we presented XMoP, a novel configuration-space neural motion policy for re-targeting planning behavior zero-shot to unseen robotic manipulators. We formulated motion planning as a link-wise SE(3) pose transformation method, showcasing its scalability for data-driven policy learning. We used fully synthetic data for training models for motion planning and collision detection demonstrating strong sim-to-real generalization to real robotic manipulators. XMoP is trained using a behavior cloning approach and is capable of planning the motion for a distribution of robots, thereby unlocking a class of neural policies for learning crossembodiment behaviors. 
 
 Appendix:  



Reviews: review: title: title: Review
main_review: main_review: This paper proposes a solution to a very interesting topic of learning how to plan for a range of robot embodiment. The presented approach is interesting, especially the way of the collision assessment. However, the way of handling the kinematic chain constraints is not well described and the numerical results are quite far from being convincing.

Pros:
- interesting approach to handle different robot embodiment
- the way of detecting collisions that works on the raw data 


Cons:
- the description of handling the kinematic chain constraints is not clear enough
- the obtained results suggests that the proposed method introduces quite a big computational overhead over the classical planning approaches, which makes the use of learning questionable (typically we use learning to minimize the plan computation time)

Comments and questions:
1. "Furthermore, these algorithms conventionally assume the availability of a precomputed geometric representation of the robot’s workspace
for state validation, thus making them unusable in unseen
environments with novel types of obstacle" - I agree that typically planers require a map, but it is not making classical motion planning approaches unusable if a "novel type" of obstacle is present. Maybe this point refers to some trajectory optimization techniques, but this is not true for a wide range of planners.

2. How the kinematic chain constraints are enforced? Are they just ignored and resolved by the IK, which minimizes the error but never reaches values close to 0? Authors wrote "Our policy predicts link-wise transformations that obey different kinematics constraints across manipulators from a single pose sequence observation" but it is very unclear why and to what extent this is true.

3. Authors emphasize that this planner works in unseen environments, however, it is not clear what is meant by that. Reviewer understands that this refers to the fact that there is no map of the environment accessible. However, authors seems to assume that they have access to complete point clouds of the environment, which can be easily transformed to a map. This creates a confusion.

4. This is especially concerning taking into account the results, which kind of shows that if we can generate a map of the environment in less than 40s then AIT* is more efficient than the proposed planner.

5. The description of the proposed data augmentation technique is hard to understand.
From the number of combinations it seems that for every joint there are 3 choices, however, it is not clear how this emerges from "uniformly choosing link templates". It is also not clear how choosing cylinders randomly allows for satysfying the constraints of the kinematic chain.

6. At one place authors wrote " We consider a goal to
be reached when the L2 norm of the SE(3) pose difference
between end-effector and goal is less than a pre-specified
threshold of 0.01" while in the other place "A trajectory
is successful if the final end-effector position is within 1 cm
and orientation is within 5◦ of the goal, with no collisions
or joint limit violations" which are not equivalent.

7. Authors wrote "XCoD by virtue of its formulation
learns a robust 3D mesh collision classifier conditioned on
link-wise semantic labels specified for the robot" but from the introduction of XCoD it seems that the collision classification is done based on point clouds not meshes.

8. Limitation one can be addressed by learning-to-plan works that does not consider the need for an oracle [1,2] or can improve over the plans generated by an oracle [3].

[1] T. Jurgenson, A. Tamar, "Harnessing Reinforcement Learning for Neural Motion Planning", in Proceedings of Robotics: Science and Systems, 2019, Freiburg im Breisgau, Germany, doi: 10.15607/RSS.2019.XV.026

[2] P. Kicki et al., "Fast Kinodynamic Planning on the Constraint Manifold With Deep Neural Networks," in IEEE Transactions on Robotics, vol. 40, pp. 277-297, 2024, doi: 10.1109/TRO.2023.3326922.

[3] P. Kicki and P. Skrzypczyński, "Speeding up deep neural network-based planning of local car maneuvers via efficient B-spline path construction," 2022 International Conference on Robotics and Automation (ICRA), Philadelphia, PA, USA, 2022, pp. 4422-4428, doi: 10.1109/ICRA46639.2022.9812313

9. " k ∈ {1...H}" missing commas

10. Some of the references are to ArXiv while they are already published works.