Where To Start? Transferring Simple Skills to Complex Environments
Abstract:
Robot learning provides a number of ways to teach robots simple skills, such as grasping.
However, these skills are usually trained in open, clutter-free environments, and therefore would likely cause undesirable collisions in more complex, cluttered environments.
In this work, we introduce an affordance model based on a graph representation of an environment, which is optimised during deployment to ﬁnd suitable robot conﬁgurations to start a skill from, such that the skill can be executed without any collisions.
We demonstrate that our method can generalise a priori acquired skills to previously unseen cluttered and constrained environments, in simulation and in the real world, for both a grasping and a placing task.
Videos are available on our project webpage at https://www.robot-learning.uk/where-to-start .
Introduction
There are many methods for acquiring simple robot manipulation skills, such as grasping, placing or insertion.
Such skills can be learnt through methods such as imitation learning [1] or reinforce- ment learning [2].
However, since these methods are data-driven, they would require training across a huge range of environments if the task is to execute the skills whilst avoiding collisions with the environment.
And since this is typically impractical, these skills are typically trained in open, unconstrained environments, without enabling transfer of the skill to more complex, cluttered environments.
This then raises the following question: how do we maintain the high performance of a skill that was trained in obstacle-free environments, whilst ensuring that it does not collide with obstacles in new cluttered environments?
In complex environments, reactively avoiding obstacles by adding small control adjustments to a learned skill might not be enough, resulting in a need to completely alter the skill and re-learn how to complete the task in the new environment.
Therefore, in this paper, we propose to address this by training a separate model to ﬁnd the optimal robot conﬁguration from which executing the a priori acquired skill would complete the task, whilst avoiding collisions with the environment.
The model we propose is a novel heterogeneous graph-based affordance model [3], which allows us to represent the target object, the obstacles, and the robot, all in the same Cartesian space, and thus exploit the underlying structure of the learning problem.
During deployment, given an initial observation of the environment, we ﬁrst maximise the predicted affordance score with respect to the robot conﬁguration using gradient-based optimisation, to ﬁnd the optimal robot conﬁguration from which to start a skill (step 1, Figure 1).
The robot can then reach this conﬁguration using motion planning (step 2), and ﬁnally execute the skill to complete the task (step 3).
We evaluate our proposed three-step approach on both a grasping and a placing task, in challenging, cluttered and constrained environments, both in simulation and in the real world.
We compare it with a single-step end-to-end approach, where skills are trained in cluttered environments directly, and we also study alternative ways to ﬁnd the optimal starting conﬁguration.
We also carry out an ablation study to evaluate the beneﬁts of using a heterogeneous graph as the input of our affordance model.
Overall, our experiments validate our method as an effective way to transfer simple a priori skills to complex environments with previously unseen clutter.
Related Work
Robotic Manipulation in Cluttered Environments .
Methods that address robotic manipulation in environments where collisions with the scene are undesired have primarily focused on the grasping task [4, 5, 6, 7, 8].
Most works try to predict a stable grasping pose for which the end-effector would not collide with the surrounding objects without taking the complete conﬁguration of the robot into account.
[9] learns latent plans and when to switch to a pre-trained target-driven policy.
These plans can be sampled, scored and decoded into end-effector policy actions that avoid objects on the tabletop and grasps the target.
On the other hand, we consider the complete conﬁguration of the robot and reach an optimal starting conﬁguration using kinematic planning.
Optimising Learnt Models at Test Time .
Recently, many works have focused on optimising learnt models with respect to their inputs at inference to perform various robotic manipulation tasks [10, 11].
[12] learns an energy function from demonstrations that, when minimised at inference, can produce actions for a closed-loop policy.
In [13], Cartesian robot representation and joint angles are used as an input, letting the model decide the most useful representation.
[14, 15] tackles the grasp synthesis problem by optimising the learnt success probability model at inference.
[16] has used learnt models as constraints on the key conﬁguration in a nonlinear trajectory optimisation framework.
We take a similar approach but use a learnt affordance model to ﬁnd a robot conﬁguration from which a speciﬁc skill is likely to succeed.
Representation Learning for Robot Manipulation .
A large amount of work has focused on ﬁnding helpful robot and scene representations.
Semantic keypoints representing objects proved to be beneﬁcial in various manipulation tasks [17, 18, 19].
[20, 21] learns dense object descriptors that map raw input images to pixel-wise object representations.
Recently, representing the scene using graphs and utilising graph neural networks has been extensively explored [22, 23, 24].
On the other hand, complex systems with various actors have been successfully modelled using heterogeneous graphs exploiting their underlying structure [25, 26].
Examples include academic graphs [27, 28], recommendation systems [29, 30], and LinkedIn economic graphs [31].
In this work, we also try to exploit the problem’s underlying structure using heterogeneous graphs.
Method 3.1 Overview
We address robot manipulation in cluttered and constrained environments using the three-step approach shown in Figure 2 (left).
We assume access to a skill (e.g. grasping, placing, pushing) which has already been acquired (e.g. through imitation learning or reinforcement learning), and during deployment, we assume a speciﬁcation of which object to execute the skill on (e.g. through a highlevel planner).
Throughout this paper, we call this object the target .
In our experiments we test our method on two skills, grasping and placing, which we train in simulation using behavioural cloning.
However, our method can work with any a priori acquired skill and is agnostic to how that skill was acquired.
Therefore, we now use the general notation π ( . ) to represent a function that executes a given skill.
Our work aims to position a robot in such a way that, even in cluttered and constrained environments, these a priori acquired skills can be successfully executed without colliding with the surrounding environment.
We do so by learning an affordance model, for each individual skill.
Given an initial observation of the scene and a robot conﬁguration, this model predicts whether the skill would be successful if started from this conﬁguration.
By maximising the predicted affordance score with respect to the robot’s joint angles during deployment, a starting conﬁguration for a particular skill can be found and reached using motion planning.
3.2 Affordance Model
We assume that even in a cluttered environment, there exists a distribution of robot conﬁgurations from which an a priori learnt skill would be successfully executed, i.e. Y = 1 .
To ﬁnd such conﬁgurations, we introduce an affordance model p θ ( Y = 1 |P target , P obs , q ) for each skill π ( . ) .
Given an initial observation of the target P target and its surrounding obstacles P obs , and the robot conﬁguration q , the model predicts the probability of the robot successfully executing the skill from this conﬁguration without colliding with any obstacle.
We use segmented point clouds to represent
P target and P obs , and assume we know which policy to execute and which object in the scene is the target.
Note that considering only the pose of the end-effector in most cases is not sufﬁcient for this problem, and we need to consider the complete robot conﬁguration (i.e. joint angles).
3.2.1 Structured Graph Representation
To accurately learn p θ ( . ) , the model needs to reason about the target, obstacles, robot’s conﬁguration, and the relative position of each of these.
It also needs to capture the capabilities of skills and any low-level controllers used to execute them.
While a priori acquired skills can be designed to work on different robots, the affordance model is dependent on the speciﬁc robot and its kinematics as it needs to reason about the robot’s geometry and its movement during skill execution.
Instead of compressing the scene observation into a single vector and relying on a neural network to interpret the vector correctly, we want to exploit the underlying structure of this problem.
We do so by jointly representing the target, obstacles and the robot as a heterogeneous graph.
Heterogeneous graphs allow to attach relevant information to different types of nodes and deﬁne structured relations between them.
This allows to systematically capture numerous semantic relationships across multiple types of objects.
More formally, a heterogeneous graph can be represented as a tuple G =
( V , E , F ) , where V is a node set, E is an edge set, and F is a node feature set.
It also has the associated node type mapping function φ : V → A and the edge type mapping function ψ : E → R , |A| + |R| > 2 .
Here A and R are node and edge types, respectively.
For more detailed explanation of heterogeneous graphs and graph neural networks we refer the reader to [32, 33, 34].
While the target and the obstacles in the environment can be represented in Cartesian space as point clouds, the robot conﬁguration q inherently lies in a different (joint angle) space.
The highly non-linear mapping between the two spaces can make capturing the desired distribution extremely difﬁcult.
We, therefore, represent the complete robot conﬁguration not as a list of joint angles but rather as a set of positions of predeﬁned key points on the robot that can be calculated using fully differentiable Forward Kinematics.
These robot key points can, for example, be the centre of mass for each link of the robot.
To differentiate between robot key points in an unordered set, each has a feature vector containing a one-hot encoding to identify it.
Using the entire point clouds in the heterogeneous graph would result in a massive structure hindering the learning process.
We, therefore, use Pointnet++ set abstraction layers [35] to coarsen the point cloud by sampling a fraction of points and locally encoding their neighbourhood.
Together with the robot’s key points, we now have three sets of points together with their local feature vectors ( { pos t , F t } , { pos o , F o } , { pos r , F r } ) , representing the target, obstacles and the robot, respectively.
We use these to create a fully connected heterogeneous graph that jointly represents the robot, the target object and the obstacles in the environment (see Figure 2 right).
We use edge features to represent the relative transformations between node pairs.
This way, the network never sees any absolute position, which allows for an easy generalisation through the task space.
These relative transformations are differentiable, which still allows us to take gradients with respect to the absolute positions of the robot key points (Figure 3 c ).
The gradient of each key point shows in which direction it should be moved to increase the affordance prediction.
Using kinematic Jacobian and aggregating gradients from the key points, we can compute the gradient with respect to the robot’s joint angles (magenta dashed line in Figure 2) and use it in a gradient based-optimisation framework.
3.2.2 Training
We use a graph neural network to learn p θ ( . ) from the structured input presented in Section 3.2.1.
The whole neural network architecture we use to learn p θ ( . ) can be seen in Figure 2 (right) and is trained end-to-end.
More details on the architecture are presented in the supplementary material.
To train this network, we use a dataset composed of positive and negative samples created by rolling out the learnt skills from the different conﬁgurations and recording the outcome.
We do these rollouts in different types of procedurally generated simulated scenes with varying amounts of clutter (see Figure 4).
One roll-out creates one sample, consisting of the starting robot conﬁguration q i , initial scene observation O i , and the outcome y i : D rollouts = { q i , O i , y i , } Ni =1 .
We choose where to start the a priori acquired skill at random such that the wrist camera is pointing toward the object, and the robot is not colliding with the environment.
We validate the task’s success using privileged information available in simulation and assign a label y to the roll-out accordingly ( 0 or 1 ).
Any rollout leading to a collision is treated as a failure and assigned the label 0 .
In our implementation, the initial scene observation O i is composed of segmented point clouds ( P target and P obs ) aggregated from two-depth cameras mounted on the robot’s head and wrist (see Figure 1).
Using multiple cameras allows for better scene coverage, which can be crucial when estimating affordance scores from initial scene observation.
Figure 3 ( a and b ) shows examples of robot conﬁgurations in a cluttered environment used as negative and positive samples, respectively.
We add noise to all the depth images used to obtain point clouds according to [36] and distort the ground truth segmentation maps from simulation to better mimic a real-world setting.
Having both negative and positive examples, we use a Binary Cross-Entropy loss function to train the network as an affordance classiﬁer.
Additional details regarding the affordance model and its training are presented in the supplementary material.
3.3 Deployment
The ﬁrst step of our method during deployment is to ﬁnd an optimal robot conﬁguration from which to start an a priori acquired skill using initial scene observation.
We do so by optimising the learnt affordance model with respect to the robot’s conﬁguration to maximise the probability of a particular skill being successful.
Optimising the learnt model at inference allows us to include any other task or safety-relevant analytical constraint.
We utilise this and add joint limit and additional collision avoidance ( g coll ( . ) )
constraints between each link ( l ) in a set of links comprising the robot ( L ) and known obstacles ( obs j ) in the environment ( E ).
This results in a constrained, non-convex optimisation problem (Equation 1) which we solve using a gradient-based IPOPT optimiser [37].
[Equation]
In our implementation, known obstacles ( E ) include the table and bounding boxes of segmented objects as cuboid obstacles.
This is a coarse estimation of the environment later used by a motion planner.
Thus, including g coll ( . ) ensures that the solution is not considered to be in a collision by the motion planner, which would make ﬁnding a collision-free path to this conﬁguration infeasible.
In our experiments, solving Equation 1 took on average 3 . 2 seconds and had to be done only once using the initial scene observation.
Once the joint angles from which the low-level skill should start are found, we use an RRT-Connect [38] motion planner to ﬁnd a collision-free path to reach that conﬁguration (second step of our framework).
Finally, the third step is to execute the a priori acquired skill completing the task.
Experiments 4.1 Experimental Setup
We evaluated our method on two manipulation tasks in scenes with varying amounts of clutter: 1) grasping novel objects and 2) placing a held object inside novel containers.
For the grasping task, the robot aims to grasp the target object stably.
For the placing task, the robot starts with a small object inside its gripper, and the goal is to place it inside a target container (e.g. a bowl).
In both cases, collision with obstacles is considered a failure, making the tasks extremely challenging.
We acquire the skill to solve each of these two tasks respectively using Behaviour Cloning (BC) in simulation only.
We use various everyday objects from ShapeNet [39] and expert trajectories created by an oracle planner.
The skills output 6D end-effector velocities and use observations from the wrist-mounted depth camera in terms of the segmented point cloud of the target object.
Our BC policy networks consist of PointNet++ [35] followed by an LSTM cell (see Figure 2, bottom left).
Our closed-loop skills are trained in a tabletop environment with no obstacles.
The supplementary material provides a more detailed description of our BC skills and how we train them.
In simulation, we procedurally generate six types of randomised environments with inherently different structures containing the target object and surrounding obstacles (see Figure 4), using the
CoppeliaSim [40] simulator with PyRep [41].
Env 1 is the easiest with no obstacles and serves as a baseline showcasing the abilities of learnt skills in isolation.
The following environments keep increasing in complexity.
Env 2 includes obstacles (primitive shapes and objects from ShapeNet) surrounding the target object.
Env 3 and 5 have structured obstacles representing drawers and shelves, while environments 4 and 6 add to complexity of 3 and 5 respectively with additional obstacles.
We create D rollouts for each type of environment individually by rolling out skills from 10 different conﬁgurations per randomised scene, totalling 500 samples per type of environment.
For a fair comparison, all evaluated methods use the same observation space (segmented point clouds) processed by PointNet++.
Moreover, we use the same number of training examples to train all the methods and roughly match the number of trainable parameters.
Baselines trained end-to-end in cluttered scenes use additional data matching the number of training samples used to acquire our BC skills.
In all environments, we randomise obstacles, their shapes and the overall arrangement.
In simulation, we evaluate all method on 500 randomised environments of each type, where we randomly choose a target object from 100 objects from ShapeNet [39] that have not been seen during training.
All evaluation environments are also validated using an oracle planner to ensure there is at least one collision-free way to complete the task.
We run all experiments with different random seeds six times and present the mean and standard deviation of the success rate.
In the supplementary material, we provide results in the graph form showing the trends more clearly.
4.2 Comparison Against Learning a Single End-to-End Policy
Our ﬁrst set of experiments aims to answer the following question: in what type of environments does our three-step approach lead to the most noticeable performance gains over learning a single end-to-end policy directly in cluttered environments?
To answer this, we compare the performance of our method with two fully end-to-end Behaviour
Cloning baselines: BC(eef) and BC(joint) .
Contrary to the skills described in Section 4.1, these policies are trained using demonstrations from scenes with obstacles and need to learn both to avoid obstacles and to complete the task.
Generally, this requires controlling the full conﬁguration of the robot.
We therefore concatenate current robot joint angles to every point in the point cloud.
BC(eef) operates in a 6D end-effector action space, whilst BC(joint) uses a joint angle action space.
In this set of experiments, all methods are trained using data from each type of environment separately [Table] and evaluated on unseen conﬁgurations of these environment types.
The results are shown in Table
[Table]
trained end-to-end in constrained environments directly.
BC(joint) , which uses a joint-angle action space, does not yield good results even in environments without obstacles.
This is expected due to highly non-linear mapping between the joint angle space and Cartesian space, in which point clouds are represented.
BC(eef) performs better but struggles in more cluttered environments.
Our method, on the other hand, performs well in all environments, showing only a minor performance decrease with increasing cluttering.
Therefore, to answer our initial question, the more complex the environment the more noticeable the performance gains of our method over learning a single end-to-end policy.
4.3 Different Ways of Finding Starting Conﬁgurations
In this second set of experiments, we aim to answer the following question: is optimising the learnt affordance model a better way to ﬁnd the policy’s starting conﬁguration than alternatives?
The alternatives (or baselines)
we test include 1) a Naive approach, where a random conﬁguration with the wrist camera pointing at the target object is found and reached using a motion planner, 2) a Generative approach, which generates a set of joint angles using a trained CVAE [42], conditioned on a segmented point cloud, 3-4) two-stage Behaviour Cloning ( BC2(eef) and BC2(joint) )
approaches using different action spaces, where a closed-loop policy is learnt from demonstrations to reach the closest validated starting conﬁguration and execute the skill.
The naive baseline serves to validate the need to learn these starting conﬁgurations.
The generative baseline allows us to validate our way of obtaining them (regressing joint angles vs optimising affordance score).
Finally, the BC2 baselines aim to validate the added beneﬁt of the second step in our framework - reaching a starting conﬁguration using a motion planner.
To better showcase the generalisation across unseen clutter, we train all methods using data from 5 of the 6 environment types and evaluate them on the remaining type.
The results can be seen in Table 2.
[Table]
As expected
[Table]
the Naive approach performs well in an uncluttered environment, but unlike our method, its performance signiﬁcantly decreases with increasing clutter.
Other baselines also struggle with cluttered environments, especially the ones operating directly in the joint angle space. They often start the skill execution when the wrist camera cannot see the target.
Together, these results provide evidence of the added value of our method compared to the baselines [Table]
4.4 Ablations
Our third set of experiments investigates how important the structured input representation we use is for the affordance model’s performance.
To do this, we train two affordance models that take as input a simple concatenation of the segmented point cloud representing the environment (obtained using PointNet++) and the robot’s conﬁguration, in terms of joint angles ( PointNet(joint) ) and Cartesian key points ( PointNet(Cartesian) ) respectively.
We then use this joint representation to predict the affordance score.
Additionally, to validate our claim that it is crucial to consider the full conﬁguration of the robot, we train a version of our proposed network that uses only key points on the end-effector to represent the robot’s conﬁguration ( Ours (eef only) ).
We evaluate these methods the same way as in Section 4.3.
Table 3 shows the results of this set of experiments.
PointNet(joint) and PointNet(Cartesian) did not perform well, showing that a structure input representation is really important.
Using only key points representing the robot’s end-effector instead of the whole robot, on the other hand, yielded better results in Env 1 .
However, this approach did not perform well in more cluttered and constrained environments, where it is crucial to consider the whole conﬁguration of the robot to avoid collisions and complete the task.
[Table]
[Table]
Although our method is trained entirely in simulated environments, it uses noisy observations and distorted segmentations and should be able to transfer to the real world.
To validate this, we conduct real-world experiments involving a
Sawyer robot equipped with a Robotiq F2gripper and head and wrist-mounted Intel Realsense D435 depth cameras (see Figure 1).
We evaluate our method in three types of environments with different levels of clutter and ﬁve objects unseen during training for both grasping and placing tasks (see Figure 5).
We run three evaluations per object for each type of environment, randomising the scene’s conﬁguration.
For segmentation, we use a method for unknown object segmentation [43] and manually specify which object is to be manipulated.
The results of the real-world experiments can be seen in Table 4.
Our method, trained solely in simulated environments, was able to directly adapt to all types of challenging realworld environments without any major decrease in performance.
In comparison, and consistently with the results of our second experiment, the Naive baseline struggled to deal with more complex environments (see Section 4.3).
Limitations, Failure Modes and Future Work
Although our method shows promising results in generalising learned skills to unseen cluttered environments, it has its limitations.
First, for skills trained in reality rather than simulation, collecting data for the affordance model could be cumbersome.
Second, our learnt affordance model would need to be re-trained every time a skill changes.
Third, relying on perception pipelines for segmentation can lead to failures.
Fourth, optimisation of the learnt affordance model is quite sensitive to the initialisation provided and can get stuck in bad local minima.
And ﬁnally, our method may fail in cases where executing the skill without contact with the environment is impossible, such as with long-horizon skills that require signiﬁcant movement of the whole robot.
However, the majority of manipulation skills acquired using methods such as Imitation Learning or Reinforcement learning are simple and short-horizon.
In future work, we plan to explore and address these limitations, study the applicability to other tasks beyond grasping and placing, and extend our approach to longer-horizon tasks by chaining together multiple skills, aiming towards a full pick-and-place system.
Conclusions
In this paper, we proposed a method for the robotic manipulation of objects in cluttered and constrained scenes, where collision with the environment is undesired.
We use an approach that combines a priori acquired skills with the use of an affordance model of our own design.
Optimising this affordance model during deployment allows us to ﬁnd suitable conﬁgurations from which starting a given robotic manipulation skill would lead to its successful completion without collision with the environment.
To efﬁciently learn this affordance model, we introduced a novel, heterogenous graphbased representation that jointly captures information about the target object, scene obstacles, and the robot itself, in a structured way.
We showed that our method outperforms various baselines, can generalise to unseen cluttered and constrained scenes, and can transfer from simulation to reality.