L EARNING N EURAL I MPLICIT F UNCTIONS AS O BJECT R EPRESENTATIONS FOR R OBOTIC M ANIPULATION
Anonymous authors
Paper under double-blind review
A BSTRACT
Robotic manipulation planning is the problem of ﬁnding a sequence of robot conﬁgurations that involves interactions with objects in the scene, e.g., grasp, placement, tool-use, etc.
To achieve such interactions, traditional approaches require hand-designed features and object representations, and it still remains an open question how to describe such interactions with arbitrary objects in a ﬂexible and efﬁcient way.
Inspired by recent advances in 3D modeling, e.g. NeRF, we propose a method to represent objects as neural implicit functions upon which we can deﬁne and jointly train interaction constraint functions.
The proposed pixelaligned representation is directly inferred from camera images with known camera geometry, naturally acting as a perception component in the whole manipulation pipeline, while at the same time enabling sequential robot manipulation planning.
I NTRODUCTION
Intelligent agents should be able to interact with objects in the environment, such as grasping and placing an object, or more general tool-use, to achieve a certain goal.
In robotics, such instances are formalized as manipulation planning, a type of a motion planning problem that solves not only for the robot’s own movement but also for the objects’ motions subject to interaction constraints.
Traditional approaches represent objects using meshes or combinations of shape primitives and describe interactions as hand-crafted constraints in terms of that representation.
The approach of using such traditional geometric representations has long-standing limitations in terms of their perception and generalizing to large varieties of objects and interaction modes: (i)
The representations have to be inferred from raw sensory inputs like images or point clouds – raising the fundamental problem of perception and shape estimation. However, if the aim is manipulation skills, the hard problem of precise shape estimation might be unnecessary to predict accurate interaction features 1 , and an end-to-end object representation might be more appropriate than a standard perception pipeline.
(ii)
With increasing generality of object shapes and interaction, the complexity of representations grows and hand-engineering of the interaction features becomes inefﬁcient.
What is a good representation of an object ?
Considering the representation will be used to predict interaction features, we expect it to encode primarily task-speciﬁc information rather than only geometric. And some of the information should to be shared across different interaction modes.
In other words, good representations should be task-speciﬁc so that the feature prediction can be simpliﬁed and, at the same time, be task-agnostic to enable synergies between the features. E.g., mug handles are called handles because we can handle the mug through them and also, once we learn the notion of a handle, we can interact with the mug through them in many different ways.
From the perception aspect, good representations should be easy to infer from raw sensory inputs and should be able to trade their accuracy off in favor of the feature prediction.
To this end, we propose a novel data-driven approach to learning interaction features. The proposed feature prediction scheme is illustrated in Fig. The whole pipeline is trained end-to-end directly with the task supervisions
so as to make the representation and perception task-speciﬁc and thus to simplify the interaction prediction. The object representation acts as a bottleneck and is shared across multiple feature predictions so that the task-agnostic representations can emerge.
Particularly, the object representation is a neural implicit function over the 3D space (Park et al., 2019; Mildenhall et al., 2020) upon which equality constraint features are trained. The proposed neural implicit robot frame’s pose  
p 1 . p N   obj images, cam poses, intrinsics ...
PIFO
PIFO   y 1 . y N   shared (Sec.3.1) featurehead h task speciﬁc (Sec.3.2) function is pixel-aligned: The function takes images from multiple cameras as input (e.g. stereo) and, assuming known camera poses and intrinsics, the latent representation at a certain spatial location is directly related to pixels of the images.
Once learned, the interaction features can be used by a typical constrained optimal control framework to plan dexterous object-robot interaction. We adopt Logic-Geometric Programming (LGP) (Toussaint et al., 2018) as an optimization-based manipulation planning framework and show that this learned-feature based planning enables to compute trajectories that involve various types of interaction modes only from images.
Due to the representations’ generalization, the learned features are directly applicable to manipulation tasks involving unseen objects.
To summarize, our main contributions are • To represent objects as neural implicit functions upon which interaction features are trained, • An image-based manipulation planning framework with the learned features as constraints, • Comparison to non pixel-aligned, non implicit function, and geometric representations, • Demonstration in various manipulation scenarios ranging from simple pick-and- hang [videos] to longer-horizon manipulations [videos] and zero-shot imitations [videos].
R ELATED W ORK 2. N EURAL I MPLICIT R EPRESENTATIONS IN 3D M ODELING AND V IEW S YNTHESIS
Neural implicit representations have recently gained increasing attention in 3D modeling.
The core idea is to encode an object or a scene in the weights of a neural network, where the network acts as a direct mapping from 3D spatial location to an implicit representation of the model, such as occupancy measures (Mescheder et al., 2019; Songyou Peng, 2020) or signed distance ﬁelds (Park et al., 2019; Gropp et al., 2020; Atzmon & Lipman, 2020).
In contrast to explicit representations like voxels, meshes or point clouds, the implicit representations don’t require discretization of the 3D space nor ﬁxed shape topology but rather continuously represent the 3D geometry, thereby allowing for capturing complex shape geometry at high resolutions in a memory efﬁcient way.
There have been attempts to associate these 3D representations with 2D images using the principle of camera geometry.
Exploiting the camera geometry in a forward direction, i.e., 2D projection of 3D representations, yields a differentiable image rendering procedure and this idea can be used to get rid of 3D supervisions.
For example, Sitzmann et al.
; Niemeyer et al.
; Yariv et al.
; Mildenhall et al. ; Henzler et al. (2021); Reizenstein et al. (2021) showed that the representation networks can be trained without the 3D supervision by deﬁning a loss function to be difference between the rendered images and the ground-truth.
Another notable application of this idea is view synthesis.
Based on the differentiable rendering, Park et al.
; Chen et al.
(2020); Yen-Chen et al.
addressed unseen object pose estimation problems, where the goal is to ﬁnd object’s pose relative to the camera that produces a rendered image closest to the ground truth.
By conditioning 3D representations on 2D input images, one can expect the amortized encoder network to directly generalize to novel 3D geometries without requiring any test-time optimization.
This can be done by introducing a bottleneck of a ﬁnite-dimensional global latent vector between the images and representations, but these global features often fail to capture ﬁne-grained details of the 3D models (Songyou Peng, 2020).
To address this, the camera geometry can be exploited in the inverse direction to obtain pixel-aligned local representations, i.e., 3D reprojection of 2D image features.
Saito et al.
(2019) and Xu et al.
(2019) showed that the pixel-aligned methods can establish rich latent features because they can easily preserve high-frequency components in the input images.
Also, Yu et al.
(2021) and Trevithick & Yang (2021) incorporated this idea within the view-synthesis framework and showed that their convolutional encoders have strong generalizations.
While the above work investigates neural implicit functions to model shapes or appearances, we train them to model physical interaction feasibility and thereby to provide a differentiable constraint model for robot manipulation planning.
2. O BJECT /S CENE R EPRESENTATIONS FOR R OBOTIC M ANIPULATIONS
Several works have proposed data-driven approaches to learning object representations and/or interaction features which are conditioned on raw sensory inputs, especially for grasping of diverse objects.
One popular approach is to train discriminative models for grasp assessments.
For example, ten Pas et al.
; Mahler et al. (2017); Van der Merwe et al.
(2020) trained a neural network that, for given candidate grasp poses, predicts their grasp qualities from point clouds.
In addition, Breyer et al.
; Jiang et al.
(2021) proposed 3D convolutional networks that take as inputs a truncated SDF and candidate grasp poses and return the grasp affordances.
Similarly, Zeng et al.
(2020b;a) addressed more general manipulation scenarios such as throwing or pick-and-place, where a convolutional network outputs a task score image.
On the other hand, neural networks also have been used as generative models.
For example, Mousavian et al.
(2019) and Murali et al.
(2020) adopted the approach of conditional variational autoencoders to model the feasible grasp pose distribution conditioned on the point cloud.
Sundermeyer et al.
(2021) proposed a somewhat hybrid method, where the network densely generates grasp candidates by assigning grasp scores and orientations to the point cloud.
You et al.
(2021) addressed the object hanging tasks from point clouds where the framework ﬁrst makes dense predictions of the candidate poses among which one is picked and reﬁned.
Compared to these works, our framework takes advantage of a trajectory optimization to jointly optimize an interaction pose sequence instead of relying on exhaustive search or heuristic sampling schemes, thus not suffering from the high dimensionality nor the combinatorial complexity of long-horizon planning problems.
Another important line of research is learning and utilizing keypoint object representations.
Manuelli et al.
; Gao & Tedrake (2021);
Qin et al.
Turpin et al.
(2021) represented objects using a set of 3D semantic keypoints and formulated manipulation problems in terms of such the keypoints.
Similarly, Manuelli et al.
(2020) learned the object dynamics as a function of keypoints upon which a model predictive controller is implemented.
Despite their strong generalizations to unseen objects, the keypoint representations require semantics of the keypoints to be predeﬁned.
The representation part of our framework is closely related to dense object descriptions proposed by Florence et al.
The idea is to train fully-convolutional neural networks that maps a raw input image to pixelwise object representations which directly generalize to unseen objects.
Our proposed framework can be seen as an extension of this pixelwise representation to dense representations over the 3D space which is learned by the task supervisions and can be seamlessly integrated into general sequential manipulation planning problems.
Another recent related work was proposed by Yuan et al. (2021), where the learned object-centric representations are used to predict the symbolic predicates of the scene which in turn enables symbolic-level task planning. In contrast, our framework predicts the task feasibility given a robot conﬁguration and enables trajectory optimization of the lower-level continuous motions.
I NTERACTION F EATURE P REDICTION VIA I MPLICIT R EPRESENTATION
Given N view images with their camera poses/intrinsics, { ( I 1 , T 1 , K 1 ) , ..., ( I N view , T N view , K N view ) } , we deﬁne an interaction feature as a neural implicit function: [Equation] (1) where q ∈ SE (3) is the pose of the robot frame interacting with the object.
As shown in Fig.
1, the feature prediction framework consists of two parts: the representation network, which we call a backbone, and the feature head networks.
The backbone serves as an implicit functional representation of an object, which, conditioned on a set of posed images, outputs d -dimensional representation vectors at queried 3D spatial locations.
The interaction feature predictions are made through the feature heads, where each head is fed on a set of representation vectors obtained by querying the backbone at a set of key interaction points.
While the multiple feature heads separately model different interactions, the backbone is shared across the tasks, making it learn more general object representations.
The rest of this section will be devoted to introduce each module in detail.
BottleneckConv 256 256 256128 128 128 16 64 64  64 localimgfeat 1 3 p 1 3 z  coordfeat ×  local   local projection
3. P IXEL -A LIGNED I MPLICIT F UNCTIONAL O BJECT R EPRESENTATION (PIFO)
Given N view posed images, we deﬁne a functional representation of an object as a mapping:
[Equation]
where p ∈ R 3 and y ∈ R d are a queried 3D position and a representation vector at that point, respectively.
The representation network consists of three parts: image encoder, 3D reprojector, and feature aggregator.
The ﬁrst two modules compute a representation vector for each image, as depicted in Fig. 2, and the last one combines them into one vector.
Image Encoder:
This module takes as input an image and computes a feature image.
In order to capture both local and global information in the image, we adopted the U-net architecture (Ronneberger et al., 2015), especially with ResNet-34 (He et al., 2016) as its downward path and two residual 3 × 3 convolutions followed by up-convolution as the upward path, i.e.,
[Equation]
3D Reprojector:
To endow the network with the multi-view consistency, all the 3D operations are performed in the view space.
The 3D reprojector ﬁrst converts a queried point, p , to the image coordinate including depth, π ( p ; T , K ) = z ∈ R 3 and then uses the projected point to extract the local image feature from the feature image, F , via bilinear interpolation.
Finally, the extracted feature and the coordinate feature, which is necessary to handle out-of-image query points, are passed to a couple of fully connected layers to get a representation vector for a single image, i.e.,
[Equation]
Feature Aggregator:
The feature aggregation from the multiple views should be permutationinvariant.
While there are many options, like summation, averaging, or more sophisticated attention mechanisms, we simply take the averaging operation for it, i.e. y =
[Equation]
3. F EATURE P REDICTION
A feature head network predicts the interaction constraint value of a robot frame ’s pose via the object representation.
We ﬁrst attach a set of keypoints to the robot frame and query the backbone at those keypoint positions, i.e., ∀ i ∈ { 1 , ..., N keypoint } ,
[Equation]
where ˆ p i is i th keypoint’s local coordinate, and R ( q ) and t ( q ) denote the rotation matrix and the translation vector, respectively.
The feature head then takes as input the resulting representation vectors and predicts a feature value through a couple of fully connected layers, i.e.,
[Equation]
Note that when the considered feature is an SDF, only a single point needs to be queried, i.e.,
N keypoint
= 1 , so the whole architecture reduces to the surface reconstruction in Saito et al.
T RAINING
In this paper, we consider manipulation scenarios where a robot arm, Franka Emika Panda, or two have to pick and hang mugs on hooks.
The environment contains mugs having different shapes and hooks on which a mug can be hung.
To formulate this manipulation problem, three interaction features are considered: an SDF feature for collision avoidance and grasping/hanging features.
4. D ATA G ENERATION
We took 131 mesh models of mugs from ShapeNet (Chang et al., 2015) and convex-decomposed those meshes.
The meshes are translated and randomly scaled so that they can ﬁt in a bounding sphere with a radius of 10 ∼ 15 cm at the origin.
For each mug, we created datasets of the posed images and each interaction as follows.
Posed Images: The posed image data consists of 100 images ( 128 × 128 ) with the corresponding camera poses and intrinsic matrices generated by an OpenGL renderer.
Azimuths and elevations of the cameras are sampled such that they can uniformly be distributed on the surface of a sphere, while their distances from the object center are randomly chosen.
The azimuth, elevation and distance fully determine the camera’s positions and making them upright and face the object center gives the camera’s orientations.
For the intrinsics, we used fov = 2 arcsin( d/r ) , where d is the camera distance from the object center and r is the radius of the object’s bounding sphere, so that the entire object appears in the image.
Lighting is also randomized.
SDF: We sampled 12,500 3D positions and computed their signed distance values using the mesh-tosdf library (Kleineberg, 2021).
Following the approach of DeepSDF (Park et al., 2019), we sampled more aggressively near the object surface to foster the learning of the object geometry.
Grasping & Hanging: We obtained 1,000 feasible grasping and hanging poses of the gripper and the hook, respectively.
For grasping, we used an antipodal sampling scheme, similarly to Eppner et al.
(2021), to create candidate gripper poses and checked their feasibility using Bullet (Coumans & Bai, 2016–2021).
For hanging, we randomly sampled collision-free hook poses and checked if it’s kinematically trapped by the mug in the directions perpendicular to the hook’s main axis.
Fig. 8 shows some rough looks of the generated data.
In the end, we have a dataset of:
[Equation]
which we divided into 78 training, 25 validation and 28 test sets.
4. D ATA A UGMENTATION AND L OSS F UNCTION
Data Augmentation: While randomizing the azimuth, elevation and distance of the camera provides all possible appearances of the object, it still cannot account for the roll angles of the camera and off-centered images.
To show the network all possible images that it can encounter when deployed later and to mitigate the size-ambiguity issue, we propose to use a data augmentation technique based on Homography warping:
In each iteration, for a randomly sampled set of images, we artiﬁcially perturb the roll angle of each camera and the estimated object center position (at which the cameras are looking).
Also, fov is modiﬁed as if the radius of the bounding sphere is 15 cm so that smaller objects can appear smaller in the transformed images.
This results in new rotation matrices, ˆ R , and intrinsic matrices, ˆ K , of the cameras.
We then compute the corresponding Homography transformation and warp the images accordingly (details in Appendix A.1): [Equation]
Random cutouts are also applied to address the object occlusion.
Fig.
9 depicts how this image augmentation works.
For grasping and hanging, we generate 6D random poses ˆ q task ∼ P in each iteration 2 and, similarly to Atzmon & Lipman (2020), set the training target to be unsinged distances in SE(3) to the set of the feasible poses: d ( q ; q
[Equation]
N task task
[Equation]
Training: For the overall network training, we ﬁrst choose a minibatch of mugs from which a subset of augmented images with their camera poses and intrinsics, (cid:16) ˆ I 1: N views , ˆ T 1: N views , ˆ K 1: N views (cid:17) , a subset of SDF data, (cid:0) p 1: N SDF , SDF 1: N SDF (cid:1) , and the grasping/hanging data, (cid:16) ˆ q 1: N task task , d 1: N task task (cid:17) , are sampled.
The images are encoded only once per iteration and then the SDF, grasping, hanging features are queried at the sampled points and poses.
The overall loss is given as L total =
L sdf + L grasp + L hang , where a typical L 1 loss is used for SDFs, i.e. L sdf = 1 N SDF (cid:80) N SDF i [-=
-]=1 | φ sdf ( p i ) − SDF
i | , and the sign-agnostic L 1 loss in (Atzmon & Lipman, 2020) for grasping and hanging, i.e., L task = N task (cid:80) N task i =1 (cid:12)(cid:12)(cid:12)(cid:12) φ task (ˆ q i task ) (cid:12)(cid:12) − d i task (cid:12)(cid:12) , ∀ task ∈ { grasp, hang } .
The feature head and backbone are trained end-to-end.
Speciﬁcally, we used N views = 4 , N SDF = 300 , N grasp = 100 , N hang = 100 .
As shown in Fig.
11, the grasp and hang interaction points are deﬁned as (3 × 3 × 3) grid points around the gripper center and as 5 points along the hook’s main axis, respectively.  S EQUENTIAL M ANIPULATION P LANNING WITH L EARNED F EATURES [Table] robot frame’s pose  
p 1 .
p N   raw images &
[Figure] (Sec.5.2) [Figure] interaction featureprediction (Sec.3) h
In order to compute a full trajectory of the robot and the objects that it interacts with, the learned features can be integrated into any constraint-based trajectory optimization and manipulation planning framework, using the features as differentiable interaction constraints.
In this work, we adopt
Logic-Geometric Programming (LGP) (Toussaint et al., 2018) as an optimization-based manipulation planning framework.
In typical manipulation scenes, cameras are equipped such that their views cover a wide range of the environment.
As shown in Fig.
3, we warp raw images of the entire scene to get object-centric images and corresponding camera extrinsics/intrinsics and compute the robot frame’s pose via forward kinematics to feed them into the network.
Section 5.1 presents how the learned constraint functions are integrated into sequential manipulation planning problems and
Section 5.2 discusses the proposed warping procedure.
5. L OGIC -G EOMETRIC P ROGRAMMING FOR M ANIPULATION P LANNING
The core concept of manipulation planning is the rigid transformations of objects.
For an object transformed by δ q ∈ SE (3) , we deﬁne a rigid transformation of the interaction feature as:
[Equation]
(9) which is equivalent to transforming the representation as T ( δ q )
[ ψ ](
[Equation]
(cid:1) .
By composing the forward kinematics with the feature as
[Equation]
(10) we obtain an interaction feature as a function of a robot joint conﬁguration x and object’s rigid transformation that can be equipped into manipulation planning.
Now we are ready to formally formulate manipulation planning problems.
For an n -joint robot and m rigid objects, LGP is a hybrid optimization problem over the number of phases K ∈ N , a sequence of discrete actions a 1: K and sequences of the robot joint conﬁgurations x 1: KT , x ∈ R n and the object’s rigid transformations δ q 1: KT , δ q ∈ SE (3) m .
The trajectory is discretized into T steps per phase.
A discrete action a k describes which interaction should be fulﬁlled at the end of the phase k , i.e., which mug to pick or on which hook to hang a grasped mug, and uniquely determines an interaction mode s k = succ ( s k − 1 , a k ) , i.e., whether each mug is grasped or hung on a particular hook.
Suppose that a discrete action sequence a 1: K and the corresponding modes s 1: K with s K ∈ S goal are proposed by a logic tree search.
We deﬁne the geometric path problem as a 2 nd order Markov optimization (Toussaint, 2017):
[Equation]
where the initial joint states x − 1:0 and objects’ transformations δ q − 1:0 = 0 4 are given.
f is a path cost that penalizes squared accelerations of the robot joints.
H ( s k , a k ) is a set of path constraints the discrete state and action impose on the geometric level at each phase k
( t ) = (cid:98) t/T (cid:99) ; these constraints include physical consistency, collision avoidance, and the learned interaction constraints that ensure the success of the discrete action a k .
Lastly, I H ( s k , a k ) decides the time slice and object index that are subject to the constraint H .
Appendix A.2 introduces the set of imposed constraints in detail.
As all the cost and constraint terms are differentiable and their Jacobians/Hessians are sparse, we can solve this optimization problem efﬁciently using the Gauss-Newton optimization method.
5. M ULTI -V IEW P REPROCESSING
Let M n ∈ { 0 , 1 } W × H be the object masks available along with the raw images I n , ∀ n = 1 , ..., N cam .
We ﬁrst solve the following optimization to ﬁnd a position and radius of the minimal bounding sphere such that the warped images contain all the object pixels in the original images: min p ∈ R 3 ,r ∈ R + r,
s.t. ∀
( u,v ) ∈{ ( u (cid:48) ,v (cid:48) ); M n ( u (cid:48) ,v (cid:48) ) =1 , ∀ n ∈{ 1 ,...,N cam }} : ||W ( ˆ R , ˆ K )( u, v ) || 2 < 1 , (12) where ˆ R can be obtained from the sphere center p and the camera position t , and ˆ K is computed as fov = 2 arcsin( || t − p || 2 /r ) from which the warping W is deﬁned as in A.1 or Equation 8.
After solving the optimization above, we ﬁx the camera orientations ˆ R , change the intrinsics as if the bounding sphere has a radius of 15 cm and ﬁnally warp the raw images accordingly. Fig. 12 shows the raw images from an example environment and the images warped by the multi-view processing.  E XPERIMENTS 6. P ERFORMANCE OF L EARNED F EATURES
Baselines: The key techniques of the proposed framework are threefold: the pixel-aligned local image features, the implicit function over the 3D space as representations and the task guided learning scheme. To examine the beneﬁts from each component, three baselines are considered. (i) Global image features : The ﬁrst baseline still represent an object as an implicit function but the image encoder outputs a global image feature as shown in Fig.
13(b) rather than having the pixel-aligned local feature extraction; we used the ResNet-34 architecture as the image encoder and ﬁxed the other model speciﬁcations. (ii) Vector object representations : The second baseline represents an object as a ﬁnite-dimensional vector instead of an implicit function; as shown in Fig. 13(c),
the representation network ﬁrst computes the image features from the images using ResNet-34 and the camera features from the camera parameters using a couple of fully connected layers. Two features are then passed to another couple of fully connected layers to produce the object representation vector. The feature heads take as input the frame’s pose as well as the object representation vector. (iii) SDF representations : The last baseline uses SDFs as object representations; the network architecture for the SDF feature remains the same, but the grasping and hanging heads take as inputs a set of the keypoints’
SDF values instead of the d -dimensional representation vectors.
The SDF values are detached when passed to the grasp/hang heads so the backbone is only trained by the geometry (SDF) data.
Evaluation Metric: Regarding the shape reconstruction, we report the Volumetric IoU and the Chamfer distance.
To measure these metrics, we ﬁrst randomly sampled 4 images from the dataset and reconstructed the meshes from the learned SDF feature using the marching cube algorithm (See
Fig. 10). The volumetric IoU is the ratio between the intersection and the union of the reconstructed and ground-truth meshes which is (approximately) computed on the 100 3 grid points around the objects.
To compute the Chamfer distance, we sampled 10,000 surface points from each mesh and absolute [Table] averaged the forward and backward closest pair distances.
To evaluate the learned task features, we solved the unconstrained optimization ˆ q ∗ =
arg min q || φ task ( q ) || 2 , task ∈ { grasp, hang } using the Gauss-Newton method.
Starting from this solution, we then solved the second optimization problem by including the collision feature (details in Appendix A.3), q ∗ = arg min q || φ task ( q ) || 2 + w coll
||
φ coll ( q ) || 2 .
Because the local optimization method can be stuck at local optima, we ran the algorithm from 10 random initial guesses in parallel and picked the best one.
The optimized pose is then tested in simulation and the success rates are reported in Table 1.
Result:
Table 1 shows that the SDF representation has the best shape reconstruction performance; PIFO is slightly worse, followed by the other two frameworks.
On the other hand, the task performances of PIFO are signiﬁcantly better than the others.
The SDF representation is especially worse in the hanging task, which implies that SDFs along the line are not sufﬁcient for the feature prediction and our task-guided representation simpliﬁes the feature prediction.
Fig.
4 depicts SDF values of an unseen mug with a complex shape handle predicted by PIFO and the global image feature model; one can observe that the global image feature model reconstructed the handle shape as being more “typical” and the pixel-aligned representation was better able to capture ﬁne-grained details.
PIFO was also tested with the different numbers of input images and it can be seen from the last two rows of the table that the more images we put in, the better performance the network shows.
Tables 2 and 3 report all combinations of the metrics and the number of views.
6. S EQUENTIAL M ANIPULATION P LANNING VIA LGP
As shown in Fig. 5, we considered three manipulation scenarios: Basic pick & hang, long-horizon three-mug hanging, and handover.
In the ﬁrst scenario, the environment contains one robot arm, one hook, one mug and 4 cameras (as in Fig. 12(a)), and the interaction modes are constrained by the discrete action sequence of [(G RASP , gripper, mug), (H ANG , hook, mug)].
10 mugs were picked from each of the training and test data sets and their initial poses are randomized.
5 When executed the optimized trajectory in the Bullet simulation, the success rates on the train and test mugs were 50 % and 40 %, respectively.
If we allow the method to re-plan and execute when it failed, the success rates increased to 90% and 70%, respectively [playlist1].
The three-mug scenario consists of 6 discrete phases with [(G RASP , gripper, mug1), (H ANG , M hook, mug1), (G RASP , gripper, mug2), (H ANG , U hook, mug2), (G RASP , gripper, mug3), (H ANG , L hook, mug3)].
The handover scenario has two arms at different heights and the target hook is placed very high, requiring two arms to coordinate a handover motion; the corresponding discrete actions are [(G RASP , R gripper, mug), (G RASP , L gripper, mug), (H ANG , U hook, mug)].
Fig.
5 shows the last conﬁgurations of the optimized plans; we refer the readers to Figs.
16–17 and the videos [playlist2] for clearer views. Inverse Kinematics with Generative Models: One important attribute of our framework is that, while most existing works train generative models that directly produce the interaction poses, ours models interactions as equality constraints which can jointly be optimized with other planning features. To see the beneﬁts of such joint optimization, we considered the following inverse kinematics problem with a generative model: For the basic pick & hang and handover scenarios, we optimized [Figure]
each interaction pose separately as in Sec.
and checked if these individually optimized poses are kinematically feasible, i.e. whether or not the inverse kinematics problems have a solution. Even though the mug’s initial pose was given such that the ﬁrst gasping is feasible, 53 out of 100 pairs of grasp and hang poses were infeasible for the pick & hang scenario and 86 out of 100 sets for the handover scenario, i.e., many of the sampled poses led to a collision or an infeasible robot conﬁguration for hanging/handover. Some failure cases are depicted in Figs. This will become even worse when the whole trajectory is optimized or the mug’s initial pose is given arbitrarily.
As the sequence length gets longer, not only should an exponentially larger number of planning problems be solved to ﬁnd a set of feasible poses, but also the found poses are not guaranteed to be optimal.
Again, the joint optimization with our constraint models doesn’t raise such issues.
6. 6D P OSE E STIMATION AND Z ERO - SHOT I MITATION
Figs.
14 and 15 visualize some principal components of the image features and of the representation vectors, respectively.
It can be seen that each component represents a certain property of the objects, such as inside vs. outside, handle vs. other parts, or above vs. below. This enables the imagebased pose estimation which we call feature-based closest point (FCP) matching, i.e., the problem of ﬁnding the relative pose of a target mesh w.r.t. a model mesh whose pose is given, without deﬁning any canonical coordinate of the objects. To this end, we ﬁrst queried the backbone at 10and 5 3 grid points around the target and the model, respectively, (as shown in Fig.
20(d))
with their own images. For each model grid point, the corresponding target point is obtained such that their representations are closest. Finally, FCP computes the rigid transformation that minimizes the sum of their Euclidean distances. We compared this to the conventional iterative closest point (ICP) algorithm on point clouds, i.e., the problem of ﬁnding the pose minimizing the Euclidean distance of two sets of point clouds obtained from depth cameras.
Instead of using depth images, the point clouds can be obtained from the reconstructed meshed via the learned SDF features and we call it
ICP2.
The point clouds’ size was 1000.
Fig.
6 shows the position and orientation errors of the 6D pose estimation.
FCP performs much better especially in the orientation because, as already widely known, ICP easily gets stuck at the local optima; ICP2 was similarly as worse as ICP.
Speciﬁcally, a signiﬁcant improvement was observed in F+ICP2 where we used the FCP results as starting points of ICP2 (which is performed without depth images).
Fig.
21 depicts some of the results.
Another observation from the PCAs is that the semantics of the representation are consistent across different objects, e.g. the handle parts of different mugs have similar representations, which implies a pose of one object can be transferred into another through it.
We therefore considered an image-based zero-shot imitation scenario, where the environment contains one robot arm, one target mug (ﬁlled with small balls) and 4 cameras as shown in Fig.
We manually designed the pouring motion and captured the camera images of pre- and post-pouring postures of the mug,
P pre = ( I pre , T pre , K pre ) and P post =
( I post , T post , K post ) , respectively.
For a new mug, we solved the sequential manipulation planning with [(G RASP , gripper, mug), (P OSE FCP, P pre , mug), (P OSE FCP,
P post , mug)], where (P OSE FCP, · , · ) imposes the aforementioned FCP constraint.
That is, the tra- jectory optimizer tries to match each part of the new object to the corresponding part of the target while coordinating the global consistency of the full trajectory, thereby resulting in the imitation of the reference motions only from the posed images.
Fig.
7 shows the optimized post-pouring posture; Figs. 22 – 23 and the videos [playlist3] show the reference and its imitations more clearly.
C ONCLUSION
This work transferred the idea of neural implicit representations to robotic manipulation applications.
The pixel-aligned nature enables the proposed representations to be inferred from posed camera images and easily capture ﬁne-grained details of the objects.
We demonstrated that the learned representations and interaction features allowed for formulating and solving general sequential manipulation problems only from images within the LGP framework.
The learned representations generalize to unseen objects, enabling manipulation planning and zero-shot imitations for them.
A A PPENDIX A. H OMOGRAPHY T RANSFORMATION
The idea of the Homography warping is that two images taken by cameras at the same position but with different orientations and intrinsics can be transformed into each other.
Suppose that we have a source image I with the camera position t , rotation matrix R and projection matrix K and that an object is inside a bounding sphere at p ∈ R 3 with a radius r ∈ R + .
An image focusing on the bounding sphere can be taken from a (synthetic) camera at the same position t with the view direction as t − p and the ﬁeld of view angle as 2 arcsin( || t − p ||
2 /r ) , from which we can compute the new camera rotation matrix ˆ R and the intrinsic ˆ K .
Given ˆ R and ˆ K , the new ﬁeld warped by the corresponding Homography can be obtained as follows:
First, a pixel in the source image, p 1 = ( u, v, 1) , is reprojected into a ray in the 3D space: P 1 =
K − 1 p 1 .
Next, the ray is viewed in the new camera coordinate: P 2 =
ˆ R T R P 1 .
Lastly, this ray is projected back into a pixel in the new camera: p 2 = ˆ K P 2 .
Putting all together, the Homography warping is given as:
[Equation]
where w is the parameter that makes the last element of the output homogeneous coordinate 1 , which results in the warped image ˆ I with its camera pose ˆ T = [Equation] ˆ
[Equation]
and intrinsic matrix ˆ K .
A. M ANIPULATION C ONSTRAINTS
In this work, we consider two discrete actions, (G RASP , gripper, mug) and (H ANG , hook, mug), for grasping and hanging, respectively. Each action imposes three constraints on the path as follows. • The action a k = ( G RASP , gripper , mug )
ﬁrst imposes the learned grasping constraint at the end of its phase, H i
grasp ( x t , δ q it
[Equation]
= 0 , t = kT , i.
[Equation]
where i and j are indices of the mug and the gripper, respectively.
It also imposes the zero-impact switching constraint at t
= kT ,
i
.e.
, ˆ v t =
[Equation]
where ˆ v t is a joint velocity computed from x t − 1 and x t via ﬁnite difference.
Lastly, it introduces an equality constraint on the gripper’s approaching direction for collision-free grasping; more precisely, the constraint is imposed at t ∈ { kT − 2 , kT − 1 , kT } as:
[Equation]
where j ˆ a it is the mug’s acceleration in the gripper’s coordinate computed from j t ( δ q
it − 2 ) , j t ( δ q it − 1 ) and j t ( δ q it ) via ﬁnite difference, and a approach ∈ R + is the predeﬁned approaching acceleration magnitude.
The gripper’s z axis is depicted in Fig.
11(a)
as a blue arrow.
Combined with the above zero-impact constraint, this constraint enforces the gripper to approach the mug in the gripper’s - z axis direction and to stop moving at the end of the phase.
• Similarly, the action a k
= ( H ANG , hook , mug ) consists of the learned hanging constraint, the zero-impact and hanging approaching constraints as
[Equation]
where i and j are indices of the mug and the hook, respectively, and the hook’s z axis is the blue arrow in Fig.
11(b) (or outer product of the red and green arrow).
The discrete actions above affect the consecutive symbolic states.
While s k indicates a mug is grasped by a gripper or hung on a hook at the phase k , we impose the following path constraint:
[Equation]
where i and j are indices of the mug and the gripper/hook, respectively.
Effectively this introduces a static joint between the two frames (Toussaint et al., 2018)
so the mug moves along with its parent frame (the gripper or hook).
The collision constraints are also imposed along the trajectory, where the pair collisions with the mug are computed by the learned SDF feature.
We introduce the collision feature in the following section.
We would like to emphasize that our manipulation planning framework is not limited by the constraints we introduced above, but it can incorporate any existing other constraint models and methods, e.g., (Toussaint et al., 2018;
Ha et al., 2020; Toussaint et al., 2020; Driess et al., 2021).
A. D EFINING P AIR -C OLLISION C ONSTRAINTS WITH SDF S
For manipulation planning problems written only by convex meshes, the distance or penetration of two objects, which we call pair-collision features, are computed with either Gilbert-Johnson-Keerthi (GJK) for non-penetrating objects or Minkowski Portal Reﬁnement (MPR) for penetrating objects.
In this section, we introduce how to deﬁne pair-collision features when one or both objects are given as SDFs.
SDF vs. Sphere: Let δ q i , q j and r j be the rigid transformation of PIFO, the sphere’s pose and radius, respectively.
Then the pair-collision feature is simply given by:
[Equation]
SDF vs. Capsule: Let δ q i , q j , h j and r j be the rigid transformation of PIFO, the capsule’s pose, height and radius, respectively.
The pair-collision feature is given by the solution of the following optimization:
[Equation]
SDF vs. Mesh: Let δ q i and q j be the rigid transformation of PIFO, the mesh’s pose, respectively.
[Equation]
where n 1 is the normal vector of φ SDF at p 1 and d j ( p 2 ) is the signed distance of p 2 to the mesh computed by GJK/MPR.
SDF vs. SDF: Let δ q i and δ q j be the rigid transformations of two PIFOs.
[Equation]
The optimizations in Equation 22–Equation 24 should be run multiple times from different initial guesses because the object shape represented as SDF can be non-convex.
In practice, we found approximating the meshes by a number of spheres and computing the collision feature much more efﬁcient because querying the network φ SDF at multiple points can be done in parallel on GPUs.
A. N ETWORK P ARAMETERS
Image encoder has the U-net architecture (Ronneberger et al., 2015), especially with the headless
ResNet-34 (He et al., 2016) as its downward path and two residual 3 × 3 convolutions followed by up-convolution as the upward path.
The number of output channels is 64 .
3D reprojector computes the coordinate feature as 32-dimensional vector using one linear+ReLU layer and concatenate it with the local image feature.
They are passed to two hidden layers with the width of (256, 128) followed by ReLUs.
Therefore, the dimension of the representation vector is 128.
SDF head takes as input one representation vector and computes the output through one hidden layer with the width of 128 followed by ReLU.
Grasp and hang heads take as input 27 and 5 representation vectors at their interaction points (depicted in Fig. 11) and predict the feature through two hidden layers with the widths of (256 , 128) followed by ReLUs.
As shown in Figure 13, the network structures for comparison in Section 6.1 was kept similar to the above as possible.
Image encoders of the global image feature and vector representation networks are the ResNet-34 returning 64 -dimensional vector.
The feature head structures remain the same, but, because the vector representation scheme doesn’t represent objects as implicit functions, the input of their feature head is the frame
’s pose as 7-dimensional vector (3D translation+ 4D quaternion).
The grasp and hang heads of the SDF representation scheme take as input 27 - and 5 -dimensional vectors of their interaction points SDF values.
[Table] [Table] [Table] [Table]
[Table]
[Table]
[Table]
[Table]
[Table]
[Table]
[Table]
[Table]
[Table]
[Table] [Table] [Table] [Table]
[Table]
[Table]
[Table] [Table] [Table] [Table]
[Figure]
[Figure]
[Figure] [Figure] [Figure] [Figure]
256 256 128 128 64 643 64 localimgfeat 1 3 p 1 3 z  coordfeat ×  local   local projection (a) PIFO 64 6432 12816 256 256 8 512 512  globalimgfeat 1 3 p 1 3 z  coordfeat ×  local
[Figure] [Figure] [Figure] [Figure]
[Figure] [Figure] [Figure]
[Figure] [Figure] [Figure]
[Figure] [Figure] [Figure]