CLASP: Constrained Latent Shape Projection for Reﬁning Object Shape from Robot Contact
Anonymous Author(s)
AfﬁliationAddress email
Abstract: Robots need both visual and contact sensing to effectively estimatethe state of their environment.
Camera RGBD data provides rich information ofthe objects surrounding the robot, and shape priors can help correct noise andﬁll in gaps and occluded regions.
However, when the robot senses unexpectedcontact, the estimate should be updated to explain the contact.
To address thisneed, we propose CLASP:
Constrained Latent Shape Projection.
This approachconsists of a shape completion network that generates a prior from RGBD dataand a procedure to generate shapes consistent with both the network prior androbot contact observations.
We ﬁnd CLASP consistently decreases the Chamfer Distance between the predicted and ground truth scenes, while other approachesdo not beneﬁt from contact information.
Introduction
You look into a cabinet and see a box of crackers.
You reach in and attempt to grab the box fromthe side, but your ﬁngers hit something.
Perhaps this box is larger than you thought?
Your mentalmodel of the box updates, you try a wider grasp, and you successfully retrieve your snack.
Robots arecurrently not so adept.
While they can estimate the pose of known shapes [1] or estimate parametersof objects [2], they cannot yet fuse this visual and contact information to draw from the wide rangeof shape priors in the world.
A robot could try to learn its next action directly from vision and forcefeedback instead [3], but this approach lacks the logic to generalize to scenarios not seen in training.
We propose a method that allows robots to mimic the process of updating object shape from contactinformation.
A shape completion neural network ﬁrst generates beliefs over possible object shapesbased on visual RGBD data.
The belief updates the object shape to be consistent with contactinformation gathered by a robot moving in the scene.
We make the realistic assumption that
the
RGBD camera perceiving the scene suffers from sensor noise and occlusion.
We assume the robotcan sense if it collides with an object, but not where the contact was made (i.e., no sensorized skin).
Many of the “cobot” platforms available today utilize this contact model to detect collision and stopbefore harming a person.
Formally, this type of contact creates a contact manifold, a thin space of shapes with a boundarybordering the robot.
Past work has projected shapes onto the contact manifold in object pose space[4] and robot conﬁguration space [1], but both require known shape geometry.
Our objective is toupdate the unknown shape geometry to satisfy contact constraints.
Returning to the cracker boxexample, the robot will be unsure if the contact occurred at the top ﬁnger, the bottom ﬁnger, orperhaps the back of the hand or the elbow.
Filling in all possible contact points would lead to absurdscenes with robot shells protruding from the cracker box.
However, ignoring this contact informationleaves the robot with the original belief of the thinner cracker box and no explanation of why theattempted grasp failed.
Our shape completion network generates a prior in latent shape space whichcan be decoded into shapes in workspace; however, shapes generated directly from this latent priorare unlikely to satisfy contact constraints.
Our key insight is that latent samples from our neural network can be projected onto the contactmanifold in the latent shape space using iterative gradient descent, creating shapes both likely under ambiguity ﬁnal shape sensor noise and occlusion, which we store as a set of sampled scenes in a particle
Contact information (pink) reduces ambiguity, and using CLASP the particles converge to the true shape.
the visual prior and consistent with the contact information.
We further expect these projected shapesto be closer to ground truth than direct samples not considering contact.
We accomplish this with our proposed Constrained LAtent Shape Projection (CLASP), which storesa belief over shapes in a particle ﬁlter.
Each particle represents a collection of latent object shapeswhich can be decoded into a scene.
Every new robot measurement of contact and freespace triggersan update on all particles.
During each particle update, gradient steps are taken to increase theoccupancy likelihood of the most likely point(s) explaining the contact(s), decrease the occupancylikelihood of the free points, and increase the latent likelihood under the shape prior.
We test this method both in simulation and on a live robot by constructing scenes of objects on atabletop, generating robot motions that generate freespace and contact observations (Fig. 1), updating the belief using CLASP as well as baselines, and comparing the set of sampled shapes to theground truth scene.
We ﬁnd CLASP outperforms both ablations of CLASP, as well as the approachesof Rejection Sampling and directly updating the input to the shape completion network.
We alsoﬁnd that CLASP produces more accurate scenes than a VAE GAN shape completion network.
Related Work
Shape Completion from Vision: The goal of Shape Completion is to predict a full shape from asingle partial input.
Recently, neural networks have become a popular method of shape completion.
A common network architecture learns an encoder to a feature space followed by a decoder to theshape output [6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17].
Scene Completion networks are trainedon larger spatial volumes of occupied points and use similar architectures with adaptations to joininformation from multiple scales [18].
While most methods predict the single best estimated shape,we build off work that uses a variational autoencoder architecture to produce plausible and diverseshapes [5].
We draw from the vast work on shape and scene completion and contribute a methodthat improves scene estimates using contact information from a robot.
Shape Completion from Touch: In different works, “touch” can refer to a single known contactpoint, a contact conﬁguration, force-torque measurements, or a rich tactile sensor.
Work using thedeﬁnition of contact point or contact conﬁguration typically uses touch to reduce the version spaceof shape possibilities [19], but such approaches cannot tractably capture the diversity of all shapes.
Alternatively, some situations model known shapes with unknown poses [2].
Both classical Iterative
Closest Point [20, 21] and neural networks [22, 23, 24] have been used to predict valid poses.
Togenerate samples consistent with contact information, the Implicit Manifold Particle Filter projectssampled poses onto the contact manifold using an iterative approach [4].
Analogously, we projectsampled particles onto the contact manifold in the latent shape space of our neural network.
Touch can also refer to the rich tactile sensors such as GelSight [25] or soft-bubble grippers
[26],with input more analogous to images.
Tactile patches can be directly mapped to visual features [27].
Alternatively, neural networks have used these sensors for material classiﬁcation [28] and graspedpose estimation [29].
We do not assume our contact sensing has such rich information.
Combining Vision and Touch: A neural network can combine vision and touch (force + torque[3], or GelSight [30]) using separate encoders to a latent space for each sensing modality alongside adecoder to a variety of spaces.
We considered a similar encoder structure with a decoder to producecompleted shapes, but this would require a large dataset of (Shapes × Contact + Freespace) measurements and the resulting network would be only applicable to the robot used for training.
The lossof a neural network can be tweaked during training to bias towards priors similar to contact, such asconnectivity and stability [31].
A neural network can output a downstream objective, such as graspsuccess probability, instead of shape reconstruction, and thus may be successful for a diverse arrayof objects where accurate reconstruction is not available [32].
Insteaed of a neural network, surfacereconstruction has be done using a Gaussian Proccess (GP) prior ﬁt to tactile measurements [33, 34].
Our method is most similar to the work of Wang et al.
[35], which uses gradient descent on thelatent space of a shape completion network to enforce touch constraints.
Where that work uses ahigh-resolution GelSight tactile sensor to reﬁne shape details previously reconstructed from vision,our work focuses on ambiguous shapes (e.g. a box with unknown depth, or novel shapes not in thetraining data) and the lower information measurement of contact detection.
We accomplish muchlarger shape updates by using a diverse set of predictions and a novel projection loss function.
Problem Formulation
Consider a robot R observing a static scene composed of speciﬁc objects o j sampled from somedistribution of objects O .
The objects divide the workspace into occupied space W occ and freespace W free =
W \ W occ The robot has access to a training subset of O beforehand, but does notknow the speciﬁc objects o j in the current scene.
The robot observes the scene with two distinct sensing modalities.
In the visual modality, the robotviews the scene from a stationary RGBD camera receiving color depth images Im .
Due to sensornoise and occlusion these depth images offer an incomplete and noisy measurement on the fullregion of W occ occupied by the obstacles.
From the camera image we assume the scene can besegmented into distinct objects from O .
For the tactile modality, consider a robot that is able to sense if it has made contact with any object,but not where along the robot surface the contact was made.
We assume the contact does not movethe objects.
For a conﬁguration in conﬁguration space q ∈ C , let R ( q ) ⊂
W denote the region ofworkspace occupied by the robot.
A robot that has visited conﬁgurations { q 1 , q 2 , ... } = Q free ⊂ Cwithout observing contact can carve out regions of known free space:
[Equation]
For each conﬁguration q contact ∈ Q contact where contact is observed, there must be at least oneobject point in collision with the robot (and not in known freespace).
[Equation]
Using existing nomenclature, each such region is called a Collision Hypothesis Set (CHS) [36].
Our objective is to model the conditional occupancy p
( W occ |O , Im, Q free , Q contact ) .
Speciﬁcally,we desire a stochastic function g
( O , Im, Q free , Q contact ) which generates sample W occ as similaras possible to the true conditional distribution.
Since the true conditional distribution is unknown,in practice we seek to minimize the distance of drawn samples to the ground truth scene.
Method
Our approach is to use a particle ﬁlter storing a collection of latent shapes.
We ﬁrst segment thescene into distinct objects, then use an existing shape completion neural network to draw latentshape samples ψ j for each object from p ( ψ j |O , Im ) , initializing the particle ﬁlter.
Each particlecan be decoded into the objects in a scene, thus the collection of particles represents the beliefp ( W occ |O , Im ) .
We propose Constrained LAtent Shape Projection (CLASP) as the measurementupdate, projecting these samples onto the constraints imposed by Q free and Q contact . Our methodis shown in Fig.
2, where the trapezoids f enc and f dec are the encoders and decoders of PSSNet [5].
4.1 Initial Belief
The RGBD camera images are passed to a segmentation algorithm, which yields distinct pixel regions in the image corresponding to different objects o j .
For each object o j the correspondingportion of the depth image is converted ﬁrst to a point cloud, then voxelgrids of known-occupiedand known-free space centered around the visible object points with a transform T j mapping thevoxelgrid to the workspace coordinates.
For each object o j we use the Plausible Shape Sampling Network (PSSNet) [5] f to generate possibleshape completions.
PSSNet is structured as a variational autoencoder.
An encoder f enc maps theknown-free and known-occupied voxelgrids to a mean and variance in latent space.
A latent vectorψ can be sampled and passed to the decoder f dec , which outputs a probability of occupancy for eachvoxel.
Thresholding (e.g. p > 0 .
5 for each voxel) yields a completed shape.
An object o j that is representable by f can be stored compactly as ψ j such that f dec ( ψ j ) =
o j .
Thetransform T j maps the completed shape into the workspace frame.
A world is composed of staticobjects { o 1 , o 2 , ... } ∈ O .
A particle φ stores a speciﬁc world as a sequence of latent-space vectors{ ψ 1 , ψ 2 , ... } .
We sample worlds conditioned on only the depth-image observation by independentlysampling latent vectors of objects.
The initial belief is a set of particles { φ 1 , φ 2 ,
... } ∈ Φ generatedfrom the information from the depth camera before any robot motion.
4.2 Projecting a single object
Sampling particles using only camera information may yield worlds that are inconsistent with therobot contact information.
For example, PSSNet may predict objects that extend far into occludedspace that intersect regions the robot has moved through.
Alternatively, PSSNet may predict objectsthat do not extend into occluded space, and so the robot may observe contact with no object toexplain the collision.
Predicting shapes from vision and robot contact in a single pass would requirea dataset speciﬁc to each robot and a speciﬁc set of motions.
To resolve these inconsistencies, sampled particles are projected onto the constraints in the latentspace of the shape completion network, shown in Fig.
For sample i of object j , ψ ij induces aworkspace occupancy.
Our constraints lie in the workspace, but we wish to project the latent spacevector.
Therefore, the projection is accomplished by optimizing a loss via gradient updates on ψ ijwhile holding f dec ﬁxed, mirroring the process of training a neural network but optimizing the inputinstead of the network weights.
Consider the unthresholded voxelgrid with values between 0 and 1produced by the decoder: f dec ( ψ ij )
=
W ij .
We optimize the loss: L all =
L free + L occ + L
prior
The ﬁrst term L free penalizes all voxels predicted above a threshold δ that are known to be free.
[Equation]
otherwise (3)
The second term L occ penalizes unexplained contact.
Each contact q contact must be caused by someobject s.t.
R ( q contact ) ∩ W occ (cid:54)
= ∅ , however it is not obvious which object is responsible for thecontact, or which voxel of the object contacted the robot.
During optimization we consider a speciﬁcassignment of Q jcontact to object o j .
We deﬁne the assignment process in Section 4.3.
Because asingle occupied voxel is enough to explain a contact, at each iteration the loss is optimized based on the maximum prediction of occupancy overlapping with the collision hypothesis set.
[Equation]
The ﬁnal term L prior penalizes deviation of ψ from the original distribution predicted by the encoder.
Without this constraint, ψ can deviate arbitrarily, losing all dependence on the depth image andeven leaving the training domain of f dec .
This would produce completions that no longer look likeobjects.
L prior is weighted by α to maintain a similar magnitude of gradients to L free and L occ .
[Equation]
Sampling the occupancy for a speciﬁc object o j given Im, Q free and Q jcontact is thus accomplishedby sampling a ψ j and optimizing until the constraints are satisﬁed.
Projection can fail if an iterationlimit, set to 100 steps, is reached without satisfying the constraints.
For practical efﬁciency thisfailure can sometimes be detected early when gradient updates no longer change the loss and theconstraints are not satisﬁed.
We use Adam [37] for optimization with a learning rate of 0.01.
4.3 Multi-object completion
CLASP stores an assignment of each q contact to a particular object
o j for each full-scene particlei .
When a measurement contains a new contact q contact , it is assigned to a speciﬁc object for eachsampled particle i as follows.
First, the output of our shape completer is a ﬁnite-sized voxelgrid,typically smaller than the full scene.
The new q contact cannot be assigned to any object j where
R ( q contact ) lies entirely outside the output region of the decoder f dec ( ψ j ) .
Next, for each remainingj , a projection is attempted for each ψ ij to satisfy the new q contact .
If all attempts fail, we assumethis new q contact was not caused by object j .
For each full-scene particle i , a speciﬁc assignment ofq contact is randomly and uniformly selected from the remaining objects j that could possibly explainthe contact.
Experiments
We evaluated scenarios of different objects to determine if CLASP improves the estimate of thescene using robot contact information.
We tested ablations of CLASP to evaluate the importanceof the latent prior and constraint satisfaction.
We also tested alternative approaches to CLASP thatdid not rely on projection.
Finally, we compared CLASP on two different network architectures andtrained on multiple datasets.
We trained separate instances of PSSNet [5] on Axis-Aligned Boxes(AAB), YCB [38], and ShapeNet mugs [39] (training details in Section A.1).
5.1 Robot Contacts
Simulation: To generate contact measurements we moved the right arm of a robot composed of two
Kuka iiwa arms with Robotiq 3-ﬁnger grippers.
We generated scenes by manually placing simulatedobjects from AAB, YCB, or ShapeNet on a virtual table at about camera height.
The known voxelswere passed to our trained PSSNet to generate a set of possible completions.
We generated robot motions to gather information by moving near and sometimes contacting theobjects using the procedure described in the appendix A.2.
The ﬁrst motion typically sweeps knownfree space rather than making contact.
The second or third motion intentionally makes contact withthe object.
Live Robot: The physical kinematics of our robot matched the simulated robot.
A Kinect depthcamera mounted at the “head” position generated the RGBD images.
A calibrated motion capturesystem provided transforms between the Kinect and robot frames.
We segmented the RGB imageusing the CSAIL semantic-segmentation-pytorch library [40] which we retrained on YCB objects.
Each segmentation was converted into a voxelgrid and fed to PSSNet as in simulation to generatesample worlds.
The same procedure was used to generate robot motions as in simulation.
Sampling and VAE GAN occasionally produced no valid shapes
On the live robot, contact was determined at each conﬁguration by checking if the measured externaltorque exceeded a threshold of 2 Nm per joint.
This threshold was large enough to avoid generatingfalse positives while remaining sensitive enough to detect contact with secured objects.
5.2 Scenes
We tested four scenes in simulation and two on the live robot.
Each scene consisted of a singleobject secured to the table in front of the robot.
We also tested a scene of multiple YCB objects.
Contacts occurred with occluded sections of the objects, with examples shown in Fig.
In bothsimulation and the live robot, table occupancy was not considered when evaluating the quality of thecompletions.
Simulated Scenes: The ﬁrst pair of scenarios used a single YCB Cheezit box (Shallow) and a stackof three Cheezit boxes (Deep).
These setups generated similar depth images but different groundtruth shapes.
Both used networks trained on the AAB dataset.
The Simulated Pitcher from YCB waspositioned with the handle occluded from view and used networks trained on the full YCB dataset.
The Simulated Mug from ShapeNet also had the handle occluded from view and used networkstrained on all mugs in ShapeNet.
The handles on these objects were localized through contact.
Live Scenes: The Live Cheezit also consisted of a stack of three boxes, and again the Live YCB
Pitcher had the handle occluded.
Both scenes used networks trained on the full YCB dataset.
The
Cheezit boxes were attached together and the pitcher was taped to prevent motion during contact.
Simulated objects were manually aligned to the live scene to approximate the ground truth of liveobjects and were used for evaluation.
5.3 Baselines
We compared our proposed method to the following alternatives.
D IRECT E DIT directly adds orremoves voxels to satisfy the contact information.
R EJECTION S
AMPLING samples latent spacevectors from the distribution predicted by the encoder, then decoded these into 3D objects andrejected any samples not satisfying the contact or free space constraints. S OFT R EJECTION S AMPLING selects
and then Directly Edits the least violating samples in the cases where all samples arerejected.
For OOD (Direct Out-Of-Distribution Prediction), we combined the visual known free andoccupied voxels with the contact known free and occupied voxels as input to PSSNet.
While othermethods did not have access to the true contact point, we allowed this method this advantage andadded the true contact point directly to the known occupied voxels from the depth image.
Finally,while other approaches used the PSSNet shape completion network, we tested using a VAE GAN[17] network.
This network tends to produce better average but less diverse samples.
Section A.3describes these baselines in more detail.
We also tested ablations of our method.
CLASP: IGNORE PRIOR tested removing the loss term
L prior .
CLASP: ACCEPT FAILED PROJECTIONS tested accepting all projections, even those thatdo not satisfy the contact constraints.
To test our contact assignment in the multi-object case (Section 4.3), CLASP: N O CONTACT DISAMBIGUATION determined if a projection of latent ψ j couldsatisfy each new q contact as in CLASP, then assigned each q contact to all feasible objects j .
Thisresulted in scenes explaining a single q contact with multiple objects.100 particles were sampled in each method, with the threshold of L free set at δ = 0 . 4 and theweighting of L prior set at α = 0 .
5.4 Results
Single-object scenes were tested on all baselines and ablations using PSSNet trained with the appropriate dataset.
Fig.
3 compares the Chamfer Distance (CD) [41] of each accepted sample tothe ground truth for selected scenes.
Plots for all scenes are shown in Section A.3.
We consider adifferent analysis in Section A.4.
We ﬁnd that R EJECTION S
AMPLING often performs well duringthe ﬁrst few observations with zero or one contact.
However, R EJECTION S
AMPLING soon failsto return any valid samples with two or more contacts.
We see that OOD produces completionsthat are typically much worse than the original completions from only vision.
The initial estimate(observation 0) from VAE GAN are hit-or-miss.
All networks saw the YCB Pitcher during training,and VAE GAN recalls the pitcher more accurately than PSSNet during testing to the point where contacts are unnecessary.
However, the recall of VAE GAN in the other ambiguous scenarios isworse than PSSNet and projection to the contact constraints often fails, leaving no sampled shapes.
The results justify our choice of PSSNet over VAE GAN for CLASP, as VAE GAN is unable tosufﬁciently adjust to the contact information.
Considering ablations of CLASP, ACCEPT FAILED PROJECTIONS performed as well initially (whenno projections fail) and signiﬁcantly worse as the number of observations increases.
Ignoring the latent prior during projection also performs worse and occasionally produces shapes that qualitativelylook less like objects compared to completions from other methods.
Across all scenes, CLASP performed similarly to the best of all other methods with 0 or 1 contactsand the best with multiple contacts.
CLASP successfully used the robot contact information in allscenarios to reduce the CD between the predicted and ground truth shapes in all scenarios.
Robotmeasurements with a contact typically caused a larger reduction in CD than measurements with onlyfreespace information.
Numerically, the CD reduced most in the Cheezit scenarios, with a reductionof the mean from 0 .
5 cm to 0 .
1 cm for the Shallow and from 0 .
14 cm to 0 .
08 cm for the Deep.
The
CD reduction in the pitcher and mug scenarios was signiﬁcantly smaller, as the general shape ofthe pitcher and mug could be predicted from the image.
The prediction of the occluded handlewas improved with contact.
The trend of improvement in the live scenes matched the simulation.
However, the numeric error of the live scenes was much larger, perhaps caused by imperfect transferof the learned shape completer from training in simulation to prediction on live Kinect data as wellas imperfect alignment of the robot frame to the Kinect frame.
In the multi-object scene (Fig. 4) the VAE GAN method achieves a better completion from the
RGBD data, but our proposed method produces better samples after 2 contacts.
Our proposed contact assignment (Section 4.3) outperforms naively satisfying the contacts whenever possible.
Discussion and Conclusion
While we model CLASP using a particle ﬁlter and would like to have the Bayesian estimate of thescene given all observations, we acknowledge many non-Bayesian approximations.
Particle ﬁltersapproximate Bayes ﬁlters, but the 100 particles we sample may not be a sufﬁcient coverage of thelatent shape space.
CLASP projects samples, which does not preserve Bayesian estimates.
While our shape network uses voxelgrids, implicit representations have recently become popularand offer advantages worth considering.
Currently shape completion networks produce the mostvisually pleasing results when trained on a single object, visually decent results when trained on asingle class of objects, and poor results when trained on large diverse shape datasets.
In order tobe practically applicable to robots, shape completion must handle a wide variety of objects.
Shapecompletion is rarely the end goal, but rather a tool robots can use to aid in tasks.
Choices of correctmetrics and reﬁnements to CLASP ultimately depend on the speciﬁc downstream application.
We demonstrated a method for estimating shape completions initialized with purely RGBD visualdata, then updated from observations of a robot arm moving through unknown regions and sensingcontact.
We stored the belief of the scene as a particle ﬁlter of latent vectors from a shape completion network and used CLASP to enforce shape consistency with the robot observations.
Mostimportantly, we showed that CLASP improves the estimate of object shape using these contact observations.
Our results further showed that CLASP performs better than ablations of CLASP andalternative methods.
We hope CLASP will be used within a larger robotics framework where reasoning over environment uncertainty based on shape priors aids in accomplishing larger goals.