SE(2)-Equivariant Pushing Dynamics Models for Tabletop Object Manipulations
Abstract: For tabletop object manipulation tasks, learning an accurate pushing dynamics model, which predicts the objects’ motions when a robot pushes an object, is very important.
In this work, we claim that an ideal pushing dynamics model should have the SE(2) - equivariance property, i.e., if tabletop objects’ poses and pushing action are transformed by some same planar rigid-body transformation, then the resulting motion should also be the result of the same transformation.
Existing state-of-the-art data-driven approaches do not have this equivariance property, resulting in less-than-desirable learning performances.
In this paper, we propose a new neural network architecture that by construction has the above equivariance property.
Through extensive empirical validations, we show that the proposed model shows signiﬁcantly improved learning performances over the existing methods.
Also, we verify that our pushing dynamics model can be used for various downstream pushing manipulation tasks such as the object moving, singulation, and grasping in both simulation and real robot experiments. Code is available at https://github.com/seungyeon-k/SQPDNet-public.
Introduction
Robotic visual pushing manipulation – by visual manipulation, we mean that only visual observations (e.g., depth camera) are available – in cluttered environments including unseen objects is an important yet challenging manipulation skill that allows a robot to interact with and change its environment to be suitable for performing downstream tasks.
For example, pushing manipulation techniques have been used to move tabletop objects graspable [1, 2, 3, 4], rearrange multiple objects for sorting [5, 6, 7, 8] and ﬁnd a target object occluded by the other objects [9, 10].
We consider model-based approaches for the pushing manipulations that consist of the following two components: (i) to construct a pushing dynamics model which predicts the motions of the objects after a robot performs a pushing action to the environment and (ii) to ﬁnd an optimal sequence of pushing actions that achieves the goal given a predesigned task criteria [11, 12].
Our primary focus is the ﬁrst step which is to develop an accurate visual pushing dynamics model that takes a visual observation as an input.
Analytic approaches that precisely model the physical interactions [13, 14, 15, 16] cannot be used since we are given unseen objects with only vision data.
Recently, there has been considerable interest in data-driven methods for learning pushing dynamics models [17, 18, 19, 20, 21, 22], but their generalization performances are still far-less-thansatisfying.
We claim that one of the important reasons behind this is that neural network models used in existing approaches lack considering the symmetry of the physical systems, and more precisely, equivariance .
For example, suppose a model is trained with an experience where a robot pushes a box object into a red arrow direction as shown in Figure 1 ( Scene 1 ).
And consider a new situation where the same box object is located at a different pose and the robot pushes the object in the same relative direction as shown in Figure 1 ( Scene 2 ).
At an intuitive level, a good model should be able to easily generalize to this type of new situation, where tabletop objects are only translated or rotated along the z -axis.
In more technical terms, the pushing dynamics model needs to be equivariant to the SE(2) transformation.
In this paper, we deﬁne the SE(2) -equivariant pushing dynamics model and deliberately design a neural network architecture that by construction has the equivariance property.
The core idea to make the model equivariant is to properly transform the coordinates of the pushing action and the objects’ poses as needed; details are elaborated in Section 2.
This construction naturally captures the symmetry of the physical systems and signiﬁcantly improves the generalization performances.
To employ the proposed equivariant pushing dynamics model in environments with only vision data and unseen objects, we need an additional module that can recognize the objects’ shapes and poses.
In this work, we represent 3d objects’ shapes by using the shape class called the superquadrics , which can express diverse shapes ranging from boxes, cylinders, and ellipsoids to other complex symmetric shapes.
We train the recognition network that predicts the objects’ shapes with superquadrics by adopting an idea from [23].
We call our superquadric object representation-based pushing dynamics model a SuperQuadric Pushing Dynamics Network (SQPD-Net) .
Experiments and benchmark comparisons against the existing state-of-the-art methods conﬁrm that our dynamics model achieves the highest performance in predicting objects’ motions after pushing action.
In addition, we validate the effectiveness of our model by using it for model-based optimal controls for various pushing manipulation tasks in both simulation and real-world experiments.
SE(2)-Equivariant Pushing Dynamics Models
In this section, we develop a neural network architecture specialized to learn a SE(2) -equivariant pushing dynamics model.
We assume that multiple rigid-body objects are placed on the table whose surface is assumed to be ﬂat and orthogonal to the gravity direction, and the robot interacts with the objects by pushing manipulations.
Each object is represented by a pose parameter T ∈ SE(3) ( 4 ×matrix representation) and shape parameter q , where the pose parameter is described with respect to some global ﬁxed frame and the shape parameter is a vector.
And the pushing action is deﬁned as a tuple ( p , v ) where the tip of the end-effector moves from the position p ∈ R 3 to p + v ∈ R 3 .
As the tip of the end-effector moves, the robot can have contact with environments, pushes objects, and changes the poses of the objects.
Further, we assume there are maximally M rigid-body objects on the table that have the parameters { ( T i , q i ) }
Ni =1 for N ≤ M .
We consider a discrete-time pushing dynamics model f that outputs the object’s transformed poses { T (cid:48) i } Ni =1 when a pushing action ( p , v ) is applied, i.e., { T (cid:48) i } Ni =
= f
( { ( T i , q i ) } N i =1 , ( p , v )) , where N can vary as long as N ≤ M .
Assuming the gravity direction is the z -axis, we ﬁrst give a precise deﬁnition of the SE(2) -equivariant pushing dynamics model:
Deﬁnition 1 A pushing dynamics model f is SE(2) -equivariant
if
[Equation]
for all object numbers N ≤ M and rigid-body transformations C that have the following form
[Equation]
where Rot ( ˆz , θ ) is a 3 × 3 rotation matrix for rotations around z -axis and t xy =
( t x , t y , 0) ∈ R 3 .
To build a SE(2) -equivariant neural network architecture, we ﬁrst introduce an object pose decomposition method that decomposes an object pose T i ∈ SE(3) to a pose projected to the table surface denoted by C i ∈ SE(3) and the relative rigid-body transformation U i ∈
SE(3) such that
T i
=
C i
U i .
Object Pose Decomposition.
Given an object pose T ∈ SE(3) , we decompose it to two 4 ×matrices C , U ∈ SE(3) as visualized in Figure 3.
First, C is deﬁned by projecting T to the table surface, which has the form in equation (2).
And secondly, U is deﬁned as C − 1 T .
More details are in Appendix B.
Now, we explain our network architecture for the pushing dynamics model f ; overall architecture is described in Figure 2.
The model f is deﬁned as { f i } Ni =1 where each f i outputs the i -th object’s transformed pose, i.e., T (cid:48)
i =
f i ( { ( T j , q j ) } Nj =1 , ( p , v )) .
For f i , we ﬁrst decompose the i -th object pose T
i =
C i
U i and transform the other objects’ poses (including itself) and pushing action as follows: (i) T j (cid:55)→
C − 1 i T j for j = 1 , · · · , N and (ii) ( p , v ) (cid:55)→ C − 1 i ( p , v ) :
= ( R Ti p − R Ti t i , R Ti v ) where R i and t i are rotation matrix and translation vector parts of C i .
Then, three different multi-layer perceptron (MLP) networks are used to extract SE(2) -invariant feature vectors: (i) the MLP 1 takes the transformed action C − 1 i ( p , v ) and outputs a feature vector a i , (ii) the MLP 2 takes the i -th object’s parameter ( U i , q i ) and outputs a feature vector b i , and (iii) the MLP 3 takes the transformed object’s parameters ( C − 1 i T j , q j ) and outputs a feature vector c ji for all j = 1 , · · · , N and then these output vectors pass through some permutation invariant function h as c i
= h ( c 1 i , · · · , c Ni ) such as the element-wise max pooling.
These feature vectors are concatenated as y i =
( a i , b i , c i ) , and we have the last layer MLP 4 that takes y i and outputs δ T i ∈ SE(3) .
We note that these MLP layers are shared across all i = 1 , · · · , N .
Then, the transformed poses are deﬁned as T (cid:48)
i = T i δ T i for all i = 1 , · · · , N .
As a result, this dynamics model is SE(2) -equivariant by construction; the proof is in Appendix B.
Training.
Denote by s = { ( T i , q i ) } Ni =1 for some N ≤ M and a = ( p , v ) .
In this paper, we train the pushing dynamics model given a set of 3-tuples { ( s , a , { T (cid:48) i } Ni =1 ) k } Kk =1 where T (cid:48)
i is the next pose of the i -th object.
The loss function L is deﬁned by comparing the ground-truth next poses { T (cid:48) i } Ni =1 and the predicted poses { ˆT (cid:48) i } Ni =
= f
( s , a ) as follows:
[Equation]
where d SO(3) is a distance measure between two rotation matrices, I 3 is 3 × 3 identity matrix,
R (cid:48) i , ˆR (cid:48)
i and t (cid:48) i , ˆt (cid:48) i are rotation matrices and translation vectors parts of T (cid:48) i , ˆT (cid:48) i , respectively, and α is a weighting parameter (for our later experiments we set α to 0.1).
The details about the used distance measure are in Appendix C.
Object Recognition-based Pushing Manipulations
If we have known objects and can easily estimate the poses of the objects, then it is straightforward to use the learned pushing dynamics model for pushing manipulations.
However, for unseen objects, we ﬁrst need to recognize the objects’ shapes and poses.
Therefore, our overall framework consists of the following two steps: (i) to recognize objects’ shapes and poses and (ii) to push objects by using the learned pushing dynamics model and pre-designed task criteria, of which details are explained in the following subsections.
3.1 Object Shape and Pose Recognition via Superquadrics
We propose to use implicit functions to represent 3d objects’ shapes.
In general, an implicit object surface representation is deﬁned by a level set of a function S ( x, y, z ; q , T ) = 0 , where q is a shape parameter and T ∈ SE(3) is a pose parameter.
In our framework, any implicit function approximation model S ( x, y, z ; q , T ) can be used.
In this work, we employ the shape class called the superquadrics, a family of geometric shapes that resemble ellipsoids and other quadrics, which can be used to represent diverse shapes ranging from boxes, cylinders, and ellipsoids to bi-cones, octahedra, and other complex symmetric shapes.
The implicit equation for a superquadric surface at T
= I 4 ( I 4 is 4 × 4 identity matrix) has the following form:
[Equation]
[Figure] [Equation] [Equation] where q = ( a 1 , a 2 , a 3 , e 1 , e 2 ) ∈ R 5 is the shape parameter.
In particular, a 1 , a 2 , a 3 controls the sizes and e 1 , e 2 controls the geometric shapes.
Some examples are shown in Figure 4.
At T (cid:54) =
I 4 , the equation S ( x, y, z ; q , T ) can be written with the passive coordinate transformation of ( x, y, z ) by T , i.e., S ( x, y, z ; q , T ) = S ( T − 1 ( x, y, z ); q , I 4 ) ; see Appendix C for details.
The object recognition problem that we address in this paper can then be posed as follows: given a visual input obtained from a depth camera that typically contains partial views of the objects, we need to predict the superquadric parameters ( q , T ) for each object.
The predicted object represented by ( q , T ) should ﬁt the full object, although only a partial view of the object is given as input.
This problem has been recently tackled by [23], where two neural network models that take point cloud data as inputs are employed: (i) object segmentation network [24] and (ii) object full shape and pose recognition network [23].
We include details about the network architectures and training methods of these networks in Appendix C.
We call our SE(2) -equivariant pushing dynamics model that uses the superquadric representation a
SuperQuadric Pushing Dynamics Network (SQPD-Net) .
3.2 Model-based Pushing Manipulations
Given a visual observation of tabletop objects as a point cloud which we denote by o , our goal is to ﬁnd a sequence of robot pushing actions ( a 1 , a 2 , · · · , a T ) that changes the environment for some given task.
In this section, we assume that we are given (i) a recognition module R that outputs the objects’ poses and shapes, i.e. R ( o t ) =
s t (throughout, we denote by s t
= { ( T t,i , q t,i ) } Ni =1 ), and (ii) a pushing dynamics model s t +1 = f
( s t , a t ) .
Given a task-speciﬁc objective function J , we solve the following optimal control problem:
[Equation]
For tasks we focus in this paper, we set r ( s t , a t ) = 0 and only use a terminal cost function q ( s T +1 ) .
We use the sampling-based MPCs [12] (implementation details are in Appendix D).
Below, we introduce three terminal cost functions for the following pushing manipulation tasks: (i) moving, (ii) singulation, and (iii) grasping.
We denote the translation vector and rotation matrix parts of the transformation matrix T ( · ) as t ( · ) , R ( · ) , respectively.
Moving is a task to move objects to their desired poses.
The desired poses are given as { T d,i } Ni =1 , then we deﬁne a terminal cost function as
[Equation]
Singulation is a task to separate objects by more than a certain distance τ .
We deﬁne a terminal cost function as
[Equation]
[Equation]
Grasping is a task to make a target object graspable.
Given a target object index i , we generate candidate grasp poses for the recognized target object as shown in Figure 5 and check collisions with the environment and the other recognized objects; green grasp poses are collision-free and red poses are not.
The terminal cost q ( s T +1 ) is deﬁned to be 0 if at least one collision-free grasp pose exists and 1 otherwise.
Further details are provided in Appendix
C.
Experiments
In this section, we empirically show that (i)
our proposed pushing dynamics model, the SQPDNet, outperforms the existing state-of-the-art data-driven pushing dynamics models, and (ii)
our
SQPD-Net can be used for various downstream pushing manipulation tasks, e.g., object moving, singulation, and grasping.
Environment.
We use the 7-dof Franka Emika Panda robot with a parallel-jaw gripper and an Azure
Kinect DK camera sensor mounted on the gripper.
The raw input visual observation is a depth image, which is then pre-processed to other 3d representations (e.g., point cloud) as needed.
Pushing Manipulation Dataset.
To train pushing dynamics models, we generate a pushing manipulation dataset in simulation (Pybullet).
Throughout our experiments, we use cylindershaped and cube-shaped objects of various sizes, and one scene contains less than 5 objects.
To execute an action ( p , v ) ∈ R 6 , (i) the robot ﬁrst moves so that the gripper’s tip is placed at p and its orientation is set as visualized in Figure 6, and then (ii) the robot moves in a way that the gripper’s tip moves to p + v with a ﬁxed orientation.
We generate the pushing manipulation dataset as follows: (i) we place random objects at random poses in the workspace, (ii) we sample an action ( p , v ) ∈ R 6 , where p is sampled near one randomly selected object and v directs the center of the object, and (iii) we execute the robot pushing action.
In this process, we note that the gripper’s other parts than the tip can also make contact with the environment.
More details are included in Appendix D.
Baseline Methods.
We compare our SQPD-Net with the following baseline methods: 2DFlow and
SE3-Net adopted from [17], SE3Pose-Net adopted from [18], and 3DFlow and DSR-Net adopted from [20].
The 2DFlow, SE3-Net, and SE3Pose-Net take an organized point cloud as a visual input and predict the ﬂow vectors of the points.
The 3DFlow and DSR-Net take a voxelized truncated signed distance ﬁeld (TSDF) as a visual input and predict the voxel ﬂow.
Our SQPD-Net takes the estimated objects’ poses and superquadric shape parameters as input and predicts the objects’ next poses.
While, in the existing approaches, the models directly predict motions from the pre-processed raw visual observations, our model consists of two modules: (i) a pre-trained recognition network
R that predicts objects’ poses and shape parameters and (ii) the SQPD-Net that predicts the objects’ next poses.
We denote these two networks together by R-SQPD-Net .
For the comparison purpose, we also test the case where the ground-truth objects’ poses and shape parameters are used as an input for the SQPD-Net and denote it by GT-SQPD-Net .
Evaluation Metrics.
Throughout, we use two types of evaluation metrics for the learned pushing dynamics models: (i) ﬂow error (the lower the better) and (ii) mask intersection over union (mask
IoU, the higher the better).
First of all, we consider the visible and full ﬂow errors.
The visible ﬂow error is the root mean squared error (RMSE) between the ground-truth ﬂows and predicted ﬂows of the points on the visible surface of the objects, while the full ﬂow error is the RMSE computed with all points from the objects’ volumes.
Second, we consider the 2D and 3D mask IoUs.
The 2D mask IoU is computed by using the depth images and thus only visible surfaces are taken into consideration.
On the other hand, the 3D mask IoU is computed with the complete 3D occupancy grid.
The full ﬂow error and mask IoU cannot be computed in 2DFlow, SE3-Net, and SE3Pose-Net, because they do not estimate the complete objects’ shapes as an intermediate step of the prediction of the pushing dynamics.
4.1 Pushing Dynamics Learning
In this section, we ﬁrst empirically verify the equivariance property of our method and show the performance advantages of ours over the existing methods. METHOD
[Table]
Equivariance Study.
For the purpose of testing the equivariance of the models, we design the following experiment: we train the models with only one pushing manipulation data – a 3-tuple { o , a , o (cid:48) } where o and o (cid:48) are current and next observations respectively and a is pushing action – so that the models overﬁt the given data.
Then, we compare the models’ generalization capabilities with test data that are generated by applying random SE(2) - transformation to the data.
An ideal equivariant model should produce almost zero error in the test data.
Table 1 shows average visible ﬂow errors of the baseline methods and SQPD-Nets, obtained by running the above experiment multiple times with different training data (details are in Appendix D).
The 3DFlow and DSR-Net are omitted in this experiment because they cannot make estimations if the transformed actions do not belong to the pre-deﬁned discrete set of actions.
The GT-SQPD-Net produces almost zero error as expected while the R-SQPDNet produces a little error originating from the recognition error.
Our SQPD-Nets are much more SE(2) -equivariant compared to the existing works.
Figure 7 shows an example prediction result from the SE3Pose-Net and RSQPD-Net; the blue bounding box represents the ground-truth next pose of the object.
For the test data, the SE3Pose-Net predicts a completely wrong motion.
More example ﬁgures of experimental results are provided in Appendix D.
Pushing Dynamics Learning.
We compare the learning performances of the SQPD-Nets and the baseline methods with a large-scale pushing dataset where the training/validation/test data consist of 12000, 1200, and 1200 numbers of 3-tuples ( { o , a , o (cid:48) } ) , respectively.
Figure 8 shows the predicted depth images and 3D masks for an example pushing data in the test dataset.
As shown in the ground-truth motions (left of Figure 8), the red, green, and gray objects are in contact with each other and these three objects move together when the red object is pushed.
In this case, our RSQPD-Net only successfully predicts the complex interactive motions of the objects. Table 2 shows [Table] Unknown
[Table] METHOD visible full 2D 3D visible full 2D 3D 2DFlow [17] 2. SE3-Net [17] 1. SE3Pose-Net [18] 1.
- - - 3DFlow [20] 1.
DSR-
[Table]
R-
[Table]
-Net (ours) 0.
GT-SQPD-Net
[Table]
ours) 0.
the evaluation metrics computed within the test data and shows that our SQPD-Nets outperform the other baseline methods by signiﬁcant margins.
Further experimental results with more example ﬁgures are provided in Appendix D.
4.2 Pushing Manipulation using R-SQPD-
[Figure] In this section, we use the R-SQPD-Net trained in Section 4.1 and conduct the pushing manipulation tasks introduced in Section 3.2 (moving, singulation, and grasping) in both simulation and real-world.
For the real-world experimental setup, we use various box- or cylinder-like objects as shown in Figure 9; the same objects are used in simulation experiments.
Since we directly apply the R-SQPD-Net trained in simulation to the real physical environment, it is reasonable to ask about the sim-to-real transfer issue.
In our experiments, we use slow pushing motions to generate quasi-static movements of the objects and thus minimize the sim-to-real gap (for quasi-static object movements, the dynamical properties of the objects and environment, e.g., mass, friction coefﬁcient, become less affective [25]).
Figure 10 shows some real-world manipulation results for various tasks.
For the moving task ( ﬁrst row ), we set the desired positions t d,i as (0 . 3 , t 0 ,i,y , t 0 ,i,z ) and β = 0 in equation (6).
For the singulation task ( second row ), we set τ = 20 (cm) in equation (7).
For the grasping tasks ( third and fourth row ), we sample about 15 to 30 candidate grasp poses for the target recognized objects.
For all three examples, our approach can ﬁnd a series of pushing actions that successfully perform the desired tasks.
Notably, for the grasping tasks, without using ad hoc objective functions, the robot realizes how to re-conﬁgure the objects so that feasible grasp poses can be found for the target objects: (i) the robot pushes the large and ﬂat object to the edge of the table and (ii) the robot pushes the surrounding objects to make the surrounded target object graspable.
TASK
Simulation
Real
Moving 9/ 8/
Singulation 9/ 8/
Grasping clutter 4/ 4/
Grasping large 4/ 3/
Failure Cases.
Table 3 shows the manipulation success rates in simulation and real-world experiments.
We design 10 test scenarios for each task, of which object conﬁgurations are in Appendix D.
A few failure cases occur, whose underlying reasons we observe can be roughly categorized as (i) a failure of shape recognition (simulation, real) and (ii) a sim-to-real transfer issue (real).
Details can be found in Appendix D.
4.3 Limitations and Future Directions
First, since SQPD-Net considers single superquadric-shaped objects, it is not trivial to apply it directly to more complex or non-convex shapes.
As the researches on representing objects in multiple superquadrics progress [26, 27], extending our approach to multiple superquadric-shaped objects remains a future work.
Second, the dynamics prediction task becomes challenging when pushing an object with a non-uniform mass distribution since different mass distributions will lead to different motions.
In this case, if we can consistently predict the reference poses of the objects (e.g., prespeciﬁed poses in CAD models), our SE(2) -equivariant model is applicable regardless of the mass distribution.
Since predicting reference poses is not easy with only depth images, this is a limitation of our approach. As one possible solution, additional information such as RGB images should be utilized [28, 29]. Lastly, there could be some situations where the SE (2) -equivariance does not apply; in this case, our approach can be detrimental. One example is that the friction coefﬁcients are different in different regions of the table.
As a research direction to overcome this, a locally SE(2) - equivariant model – SE(2) space is divided into several subspaces and the model is equivariant only within each subspace – can be considered.
Conclusion
This paper has proposed a SE(2) -equivariant pushing dynamics model.
Using the superquadric representations of object shapes, we have proposed a SuperQuadric Pushing Dynamics Network (SQPD-Net).
Through extensive empirical validations, we conﬁrm that the SQPD-Net signiﬁcantly outperforms the existing state-of-the-art visual pushing dynamics models.
Moreover, we have veriﬁed that the SQPD-Net can be used for various pushing manipulation tasks.
Acknowledgments
This work was supported in part by SRRC NRF grant 2016R1A5A1938472, IITP-MSIT grant 20210-02068 (SNU AI Innovation Hub), IITP-MSIT grant 2022-0-00480 (Training and Inference Methods for Goal-Oriented AI Agents), SNU-AIIS, SNU-IAMD, SNU BK21+ Program in Mechanical Engineering, SNU Institute for Engineering Research, Samsung Research, and Samsung Electronics
Co.,Ltd.