Learning the Dynamics of Compliant Tool-Environment Interaction for Visuo-Tactile Contact Servoing
Abstract: Many manipulation tasks require the robot to control the contact between a grasped compliant tool and the environment, e.g. scraping a frying pan with a spatula.
However, modeling tool-environment interaction is difﬁcult, especially when the tool is compliant, and the robot cannot be expected to have the full geometry and physical properties (e.g., mass, stiffness, and friction) of all the tools it must use.
We propose a framework that learns to predict the effects of a robot’s actions on the contact between the tool and the environment given visuo-tactile perception.
Key to our framework is a novel contact feature representation that consists of a binary contact value, the line of contact, and an end-effector wrench.
We propose a method to learn the dynamics of these contact features from real world data that does not require predicting the geometry of the compliant tool.
We then propose a controller that uses this dynamics model for visuo-tactile contact servoing and show that it is effective at performing scraping tasks with a spatula, even in scenarios where precise contact needs to be made to avoid obstacles.
Introduction
Many manipulation tasks require the robot to control the contact between a grasped tool and the environment.
The ability to reason and control this extrinsic contact is crucial to enabling robots that can scrape a frying pan with a spatula or a surface [1] a onto a bottle [ in assemblies [3, 4], and perform many other tasks.
In this work, we seek to address the problem of controlling the extrinsic contact between a grasped compliant tool (e.g. a spatula) and the environment.
In general, the robot cannot expect to have the full geometry and physical properties (e.g., mass, friction, stiffness) of all the tools it must use or the geometries of the environments it must manipulate in.
Instead, the robot must utilize multimodal sensory observations, such as pointclouds and tactile feedback, to act on the environment.
In recent years, learning-based methods have become increasingly popular to address the complexities of robotic manipulation, including for contact-rich tasks [5].
These methods can be loosely grouped into model-free methods, that directly learn a policy [3, 2, 6], and model-based methods, that learn system dynamics [7, 8, 9].
By focusing on modeling system dynamics, model-based methods can plan to reach new goals without retraining, and are often more data-efﬁcient [9].
Therefore, we propose learning the dynamics of our system to solve the extrinsic contact servoing task.
It is not obvious which representation to use for these dynamics.
Fully recovering tool and environment geometries from visual data [10, 11] and tactile feedback [12] has been widely explored, with recent extensions to compliant geometries [13]; however, even if the system can be fully identiﬁed, contact models to resolve interactions can have limited ﬁdelity [14].
On the other hand, learned dynamics representations can be difﬁcult to interpret and require demonstrations or observations from the desired state to specify goals [7, 15].
Instead, we propose a novel contact feature representation for our learning method that focuses on tool-environment interaction and bypasses explicitly modeling the whole system.
We represent the contact conﬁguration as 1) a binary contact mode (indicating if the system is in contact); 2) a contact geometry (as a line in 3D space); and 3) an end-effector wrench .
We propose a learning architecture to model the dynamics of the proposed contact representation from raw sensory observations over candidate action trajectories.
We propose structuring the model as a latent space dynamics model with a decoder that recovers the contact state.
We also propose an action offset term in the dynamics that allows us to accurately propagate robot poses, despite controller errors (e.g. from robot impedance).
To provide labels to our model, we collect selfsupervised data on a 7DoF Franka Emika Panda, using sensor data to automatically label contact state.
We validate our proposed method by completing various desired contact trajectories on the real robot system.
We ﬁrst show that our method can track diverse desired contact trajectories in the absence of obstacles.
Next, we demonstrate that we can utilize extrinsic contact servoing to scrape a target object from the table, while handling occlusions and avoiding contact with obstacles (Fig. 1).
Related Work
Existing research has investigated the task of recovering contact locations.
Manuelli et al.
[16] localize point contacts on a rigid robot with known geometry by employing a particle ﬁltering approach to update a set of candidate contact locations based on force torque sensing.
Kim et al.
[4] and Ma et al.
[17] model contact between a grasped rigid object and the environment by assuming stationary line contacts and modeling the deformation of a GelSlim gripper.
The estimated line contact is then used in a Reinforcement Learning (RL) policy.
Neither of these methods extends to compliant tools and neither models the dynamics of the contact conﬁguration.
Other works explore tactile servoing methods, where contact at the sensor is driven to a desired conﬁguration.
Li et al.
[18] use a large tactile pad and deﬁne contact conﬁguration features of objects pressed against the sensor.
They manually construct a feedback controller based on these features and use it to drive contacts to desired conﬁgurations.
Sutanto et al.
[19] use a smaller proﬁle tactile sensor and learn the dynamics of a learned latent space.
They then employ a Model Predictive Control (MPC) scheme to drive contacts to desired conﬁgurations on the sensor.
Both of these works assume contact is happening at the sensing location.
We, on the other hand, seek to servo extrinsic contacts, where Other work focuses on maintaining contact between a tool and the environment.
Sakaino [20] uses imitation learning to learn a controller able to maintain contact between a mop and a tabletop.
In contrast, we wish to not only maintain contact but control the extrinsic contact geometry.
Problem Formulation
We parameterize our contact feature as a binary contact indicator c b 2 { 0 , 1 } , used to indicate whether the tool is in contact, a contact line c l 2 R 2 ⇥ 3 representing the contact geometry between the tool and the environment, and an end effector wrench c w 2 R 6 .
The geometry c l is only active when the tool is in contact c b = 1 .
The contact representation allows extrinsic contact goals to be expressed as desired contact trajectories G =
[ g 1 , g 2 , . . .
, g L ] , where each g i 2 R 2 ⇥ 3 is a desired contact line to reach.
We assume that contact should be maintained throughout the task.
We formulate extrinsic contact servoing as a model predictive planning problem, given observations of the current state of the system o 0 .
For a given horizon T , we select the next T desired contact lines [ g i +1 , . . .
, g i + T ] ✓ G to be our current contact goal sequence.
The planning problem is:
[Equation]
Here g is a model describing the contact feature dynamics .
The binary constraint ensures that the tool remains in contact while the cost function d measures the distance between the two contact lines, as the average Euclidean distance between the line endpoints.
Finally, if d ( c l 1 , g i +1 ) < ✏ we increment i , thus moving to the next sequence of desired contact lines for the next round of planning.
Method 4.1 Contact Feature Dynamics Model
To solve our constrained optimization Eq. 1, we require a model g which can map from raw observations o 0 and a proposed action trajectory a 0: T  1 to the resulting contact states { c b 0: T , c l 0: T , c w 0: T } .
We propose modeling the contact feature dynamics as a deep neural network.
Our actions are changes in end effector pose.
We assume access to a pointcloud v 0 and input wrench h 0 measured at the robot’s wrist as our observations, o
= ( v 0 , h 0 ) .
Note that end effector wrench is both an input to our method and part of the contact state; predicting future wrench aids the representation learning and provides expected wrenches for planning.
We perform all learning in the local end effector frame.
We transform the pointcloud to the end effector frame EE 0 v 0 and clip to a 0 .
5 m 3 bounding box region around the end effector that contains the contact event.
We similarly predict our contact lines in the current end effector frame, EE t c lt , 8 t 2 [1 , T ] .
Learning in the end effector frame provides invariance in the visual domain to translations and rotations of the end effector and removes distractors that do not contribute to the contact state, such as the rest of the robot arm or the scene background.
Our contact feature dynamics model (Figure 2) has three components: an encoder e which maps from raw observations to a learned latent space, a decoder d which maps from the latent space to the contact state, and a dynamics model f which captures dynamics in the latent space.
We parameterize the models by a set of learned weights ✓ .
We start by embedding the current observations into the latent space with our encoder ˆ z
=
e ( v 0 , h 0 ) .
We unroll actions in the latent space as the contact state alone has insufﬁcient contextual information (e.g. end-effector pose and local geometry information) to predict the next contact state.
Because we predict the t th contact state in the current end effector frame EE t , an important consideration when designing our dynamics model is being able to accurately recover this frame.
Controller error, e.g., from the impedance of the robot, means the commanded action is not perfectly executed.
To account for this, we predict an additional term from our dynamics model  ˆ a t +1 , which is an SE (3) transformation that predicts the offset between the commanded and realized next end effector pose.
Thus, our dynamics model predicts, ˆ z t +1 ,  ˆ a t
+1 = f
(ˆ z t , a t ) .
This allows us to construct the following recursive estimate of our end effector frame W ˆ T EE t +1 =
W ˆ T EE t T ( a t ) T (  ˆ a t +1 ) , where W ˆ T EE t is the SE (3) transformation describing the pose of the end effector at time t .
We know the initial transform W ˆ T EE 0 from our robot proprioception, T ( a t ) provides the transformation for the action command, and T (  ˆ a t +1 ) for the predicted offset term.
To enforce valid SE (3) predictions we predict rotations in the axis-angle representation [21].
Finally, we recover our contact state estimates with our decoder d given the latent state ˆ z t : ˆ c bt , EE t ˆ c lt , ˆ c wt = d (ˆ z t ) .
We can then recover the predicted contact line in the world frame by composing with the estimate of the end effector frame transformation W ˆ T EE t .
An overview of the model architectures is shown in Fig.
2 and full architecture details can be found in Appendix A.
4.1.1 Training Loss
We train our model on rollouts of the system, where a single example is a sequence [ v t , h t , a t ,  a t , c lt , c wt , c bt ]
Tt =0 .
We deﬁne the loss over the example as:
[Equation]
Here BCE is the Binary Cross Entropy classiﬁcation loss and MSE is the Mean Square Error regression loss.
↵ ,  , ⇢ and  are loss weighting terms.
The ﬁrst four loss terms are prediction losses over the contact mode, contact geometry, end effector wrench, and action offset transformation.
The ﬁnal loss term is a latent consistency loss, which encourages latent rollouts to match the latent state yielded by encoding future observations.
4.2 Extrinsic Contact Servoing Controller
We propose to solve our planning problem using Model Predictive Path Integral (MPPI), which has been shown to be effective for continuous control tasks where sampling is cheap and parallelizable (e.g., neural network representations) [22].
We convert our binary constraint to a penalty, penalizing a trajectory if it yields actions that lead out of contact.
Pairing this with the contact line prediction loss yields the following ﬁnal cost function:
[Equation]
The constraint is violated if the likelihood of binary contact is below the classiﬁcation threshold , in which case we penalize by the distance to the threshold.
weights the penalty against the contact line loss.
With this cost function we apply MPPI to yield the next action and execute it on the robot.
[Figure]
4.3 Extrinsic Contact Dynamics Labeling
Our contact dynamics training loss in Eq.
2 requires ground truth contact state labels ( c l t , c b t , c w t ) at time t .
As accurate simulation of contactful interactions is challenging, we propose a method of data acquisition directly in the real world.
To generate contact line labels, we use a high resolution, low frequency scanner, a Photoneo PhoXi 3D Scanner, to generate high quality scans of the contact interaction.
Using these scans, we generate contact line labels by ﬁltering points just above the table, clipped to the area around the end effector.
We then cluster these points to remove noisy points on the tabletop and generate the contact line c lt by selecting the two furthest points in the cluster.
See
Appendix B for examples of contact labels.
We use a force torque sensor to identify the contact state wrench c wt .
We identify binary contact c bt automatically from whether a line was found in the pointcloud and/or based on force torque sensing.
Results 5.1 Experimental Setup
We test our method on a Franka Emika Panda with rigidly-mounted compliant spatulas at the end effector (Fig. 1).
For our observations o 0 , we use pointclouds v 0 from an Intel Realsense D sensor 1 and mount an ATI Gamma Force/Torque sensor between the end effector and tool.
We use the last four wrench values received after the previous action completed as the tactile input h 0 .
To collect our datasets, we use a random action policy with a heuristic to encourage contact between the tool and the spatula.
No other objects are on the tabletop during data collection to allow proper data supervision, as detailed in Sec. 4.3.
We compare our proposed method to modeling the contact dynamics as a rigid system. We assume the commanded actions are perfectly executed by the robot to recover the future poses of the end effector. We assume access to the tool geometry as a pointcloud in the end effector frame.
This pointcloud is then transformed via the future poses of the end effector to recover where the tool would be, assuming rigid motions.
We further assume that we know the table location and identify any points in the transformed point cloud that penetrate the table surface. If any exists, we set c b = 1 . We then choose the two furthest points in the intersecting set of points as the end points of the contact line c l .
The baseline does not predict the wrench c w , but is enough to solve our planning problem in Eq.
This baseline makes three assumptions our method does not make: 1) it assumes access to a pointcloud of each tool, 2) it assumes knowledge of the current tool being used, 3) it assumes explicit knowledge of the environment.
[Figure]
5.3 Modeling Contact Feature Dynamics
We ﬁrst investigate the ability of our model to capture the contact feature dynamics exhibited in our dataset.
We train three variations on our model.
First is the full model, as described in Sec. 4.1, hereafter called “Full Model.”
Second, to understand the importance of modeling the action offset of the robot, we ablate the offset action prediction, thus we propagate the end effector frame only with the commanded action.
We call this method “No Offset.”
Finally, we investigate our model trained only on visual input data, called “Vision-Only.” We train on a dataset collected from three spatulas (see “Training Tools” in Fig.
5a), collectingtrajectories on each, for a total of 30000 transitions.
We split the data 80/10/10 for train, validation, and test. We train with a rollout horizon of T = 3 .
All methods are trained with the Adam optimizer [23] until convergence on the validation set.
We set ↵ = 100 . 0 ,  =  = ⇢ = 0 .
1 in our loss term in Eq.
2 to balance the scale of the terms.
We compare the prediction performance of the models on the test split of the dataset in Fig.
3 (see Appendix C for torque prediction results, whose results are very similar to force results).
Our full model achieves high accuracy in predicting binary contact ( > 95% ), 3-5mm contact line error, and less than 0.5N force error.
Comparing “Full Model” to “No Offset” shows the importance of modeling action offsets, without which contact line error quickly grows. Comparing all learned models to the Rigid Body Baseline (Sec. 5.2) shows the importance of modeling the compliance of the tool.
The “Vision-Only” model unsurprisingly struggles to recover contact forces. Appendix C show additional results examining generalization of the model to the dynamics of an unseen spatula.
5.4 Extrinsic Contact Servoing
Next, we investigate how our proposed controller performs following speciﬁed contact trajectories.
Obstacle-Free: We start by attempting to servo along four different contact trajectories in the obstacle-free environment.
The desired contact trajectories are shown in Fig.
4, and explore translation of contact as well as cases where the robot must tilt the tool to achieve a contact smaller than the width of the tool.
We use the same labeling technique introduced in Sec.
4.3 to get ground truth contact trajectories executed by the controller.
We run the experiment once on a training spatula (left-most in Fig.
5a) and once on an unseen spatula (right-most in Fig.
5a).
We use the controller described in Sec.
4.2, with = 0 .
45 ,  = 0 .
and compare our “Full Model” learned dynamics (as trained in Sec. 5.3) vs the “Rigid Body Baseline” dynamics.
To investigate the planning performance, we run the controller ﬁve times per trajectory and measure the Intersection over Union (IoU) of the desired and swept contact areas.
We construct the goal contact area by sweeping the space between the speciﬁed goal contact lines and the realized controller swept area by assuming that the space between two consecutive contact states was swept out if the two states were both in contact.
[Figure] We show qualitative examples of the “Full Model” controller realized scrapes on the training spatula compared to the goal scrapes in Fig.
We see that the controller is able to closely match the desired swept areas, including in the difﬁcult tilting problems.
The IoU performance is shown for the training spatula (Fig. 5b) and unseen spatula (Fig. 5c).
Our proposed method outperforms the baseline on nearly all cases, for both the training and unseen tool runs.
Performance drops for all methods on the tilting problems, as it is more difﬁcult to maintain dexterous contact on only a part of the tool.
With Obstacles: We next examine our method’s robustness to visual occlusions and reaction forces arising from contact with a target object to be scraped.
This task is common in construction, cooking, and cleaning.
A deformable and slightly adhesive material (Playdough) is pressed onto the surface and a contact trajectory is speciﬁed through the object.
In one case, the target object is alone on the tabletop (Fig. 6), and thus we specify a contact trajectory using the full width of the tool.
In the second scenario, obstacles are on the table near the target object (Fig. 1), thus we must specify a contact trajectory that avoids them. All experiments are performed on the leftmost spatula in Fig. 5a.
We use our “Full Model”, and train on 22005 sequences collected only with the relevant tool.
Besides running our Full Model in these scenarios, we investigated enhancements of the method to aid its performance in the presence of visual occlusions and object reaction forces.
First, we applied data augmentation to our training dataset, randomly generating ellipsoids in the pointcloud and using a hidden point removal algorithm [24] to provide corresponding occlusions in the original point cloud.
See Appendix B for examples of augmented inputs.
We call this method “Full Model + Aug.”
Second, we investigate using the difference between the predicted and observed wrenches  w =
ˆ c
wt  c wt to derive an action offset to compensate for the extra wrench experienced by the robot.
From  w we derive an action that will counteract this wrench offset ˆ a t =
wk p .
k p are the pose gains of the impedance controller.
The offset action is composed with the original action from the controller.
We call this method “Full Model + Aug + Wrench Offset.”
We use two metrics.
First, we measure the approximate mass of the target object to be scraped before and after scraping and determine the percentage of mass successfully removed.
Second, we compare the 2D footprint of the material before and after scraping and report the percentage of the footprint successfully removed.
The second is a more challenging metric, since even a slightly wrong scrape will leave residue.
The quantitative results over 5 runs of each method in each experiment setup are shown in Fig.
Examples of scrape executions are shown in Fig.
1 and Fig.
See Appendix C for more examples of scrape results.
We see that in each case, all methods were able to remove over 95% of material mass on average and about 40-60% of material’s footprint from the tabletop.
Surprisingly, we don’t see a consistent improvement training our model with visual occlusions or adding action offsets.
The Full Model’s robustness to occlusions here could be due to the fact that we use a single tool in these experiments, and thus it may be sufﬁcient in most cases to capture the location of the table with respect to the end [Figure] effector in order to estimate the contact line.
Even with visual occlusions near the tool contact, it is likely our method can still recover the relative pose of the tabletop from the surrounding points.
The lack of clear improvement from the wrench offset action may be due to the fact that it is sufﬁcient to be able to replan, as we do at every step with our MPPI controller.
Limitations and Conclusion
Limitations:
A common failure mode for our method is in controlling contact when the tool is tilted, where it is more likely for the method to yield actions that take the robot out of contact.
This could, in part, be due to data imbalance.
In future work, we are interested in utilizing online learning [9] or curiosity [25] to more effectively cover the space of contacts in our dataset.
There are cases where tool to environment contact is not represented well as a contact line.
Extending our contact feature dynamics to these tasks will require expanding our representation learning method to consider these more diverse contact speciﬁcations and more complex contact modes.
Finally, our method relies upon supervision.
For future contact-rich tasks of interest, the need for labels could become more costly.
We hope to investigate how we can remove reliance upon supervision by exploring few/zero shot generalization [26, 27] and domain randomization techniques [28].
Conclusion: Our approach simpliﬁes contact rich interactions for compliant tool manipulation, by avoiding the necessity for full system identiﬁcation while maintaining interpretability and accuracy by explicitly modeling the contact state of the system and how it evolves.
In the future, we wish to investigate our method’s applicability to other tasks where full state estimation is difﬁcult, but the contact state is crucial, such as wiping with a cloth.
Additionally, we wish to investigate representations that handle more complex contact geometries. Acknowledgments This material is based upon work supported by the National Science Foundation Graduate Research Fellowship Program under Grant No. 1841052.
Any opinions, ﬁndings, and conclusions or recommendations expressed in this material are those of the authors and do not necessarily reﬂect the views of the National Science Foundation.
This work was supported in part by Toyota Research Institute under the University Research program 2.0.
This work was supported in part by ONR grant N00014-21-1-2118 and NSF grants IIS-1750489 and IIS-2113401.