Joint Inference of Kinematic and Force Trajectories with Visuo-Tactile Sensing

TL;DR

This paper presents a probabilistic inference framework for joint estimation of kinematic and force trajectories during planar pushing tasks, integrating visuo-tactile data to improve robustness and accuracy in state estimation.

Contribution

It extends previous methods by fusing visual and tactile sensor data for joint state and force estimation using factor graphs, applicable in both batch and incremental settings.

Findings

Improved state estimation accuracy with multi-modal sensor fusion.

Performance degradation under sensor occlusion and noise analyzed.

Outperforms prior methods on multiple datasets.

Abstract

To perform complex tasks, robots must be able to interact with and manipulate their surroundings. One of the key challenges in accomplishing this is robust state estimation during physical interactions, where the state involves not only the robot and the object being manipulated, but also the state of the contact itself. In this work, within the context of planar pushing, we extend previous inference-based approaches to state estimation in several ways. We estimate the robot, object, and the contact state on multiple manipulation platforms configured with a vision-based articulated model tracker, and either a biomimetic tactile sensor or a force-torque sensor. We show how to fuse raw measurements from the tracker and tactile sensors to jointly estimate the trajectory of the kinematic states and the forces in the system via probabilistic inference on factor graphs, in both batch and incremental settings. We perform several benchmarks with our framework and show how performance is affected by incorporating various geometric and physics based constraints, occluding vision sensors, or injecting noise in tactile sensors. We also compare with prior work on multiple datasets and demonstrate that our approach can effectively optimize over multi-modal sensor data and reduce uncertainty to find better state estimates.

Tables

Table 1. TABLE I : RMS and Covariance values on the MIT Dataset.

Object	$\text{RMS}\left({𝐱}_{\mathrm{trans}}\right)$	$\text{RMS}\left({𝐱}_{\mathrm{rot}}\right)$	$\Sigma \left({𝐱}_{\mathrm{trans}}\right)$	$\Sigma \left({𝐱}_{\mathrm{rot}}\right)$
ellip1	0.0262	0.283	2.723e-4	4.171e-10
rect1	0.0253	3.471-5	2.931e-4	4.19e-10
rect3	0.0182	1.672e-5	2.563e-4	4.18e-10
Object	$\text{RMS}\left({𝐞}_{\mathrm{trans}}\right)$	$\text{RMS}\left({𝐞}_{\mathrm{rot}}\right)$	$\Sigma \left({𝐞}_{\mathrm{trans}}\right)$	$\Sigma \left({𝐞}_{\mathrm{rot}}\right)$
ellip1	7.73e-2	9.47e-2	4.74e-3	7.11e-3
rect1	8.59e-2	9.18e-2	5.89e-3	6.01e-3
rect3	0.372	0.376	0.148	0.154
Object	$\text{RMS}\left(\parallel 𝐟\parallel \right)$	$\text{RMS}\left({𝐟}_{\mathrm{rot}}\right)$	$\Sigma \left(\parallel 𝐟\parallel \right)$	$\Sigma \left({𝐟}_{\mathrm{rot}}\right)$
ellip1	0.118	9.543e-2	9.827e-3	1.635e-4
rect1	0.145	9.683e-2	9.862e-3	1.823e-4
rect3	0.113	9.754e-2	9.145e-3	1.856e-4
Object	$\text{RMS}\left({𝐩}_{\mathrm{trans}}\right)$	—	$\Sigma \left({𝐩}_{\mathrm{trans}}\right)$	—
ellip1	3.42e-2	—	2.54e-3	—
rect1	4.52e-2	—	6.21e-3	—
rect3	3.26e-2	—	3.41e-3	—

Table 2. TABLE II : Error Results for Force and Contact Recovery

Component	RMSE	MAE	$\sigma$
Force magnitude (N)	0.352	0.195	0.043
Force direction (deg.)	3.15	2.54	0.78
Contact location (cm)	0.32	0.14	0.18

Equations

$$f+f_{\mu }=m{\ddot{x}}_{CM},\tau +{\tau }_{\mu }=I_{CM}\omega$$

$$f_{\mu }=-{\mu }_s{\int }_A\frac{v(r)}{|v(r)|}P(r)dA,{\tau }_{\mu }=-{\mu }_s{\int }_{A}r\times \frac{v(r)}{|v(r)|}P(r)dA$$

$${\left(\frac{f_x}{f_{max}}\right)}^2+{\left(\frac{f_y}{f_{max}}\right)}^2+{\left(\frac{\tau }{{\tau }_{max}}\right)}^2=1$$

$$\frac{v_x}{\omega }=c^2\frac{f_x}{\tau },\frac{v_y}{\omega }=c^2\frac{f_y}{\tau },\text{and}c=\frac{{\tau }_{max}}{f_{max}}$$

$${\tilde{\mathbf{p}}}_t=\left[\begin{array}{c}{\mathbf{p}}_t\\ {\mathbf{f}}_t\end{array}\right],{\tilde{\mathbf{w}}}_t=\left[\begin{array}{c}{\mathbf{w}}_t\\ {\alpha }_t\end{array}\right]$$

$$\mathrm{C}(\xi ,{\mathbf{p}}_t)$$

$$\mathrm{S}(\mathbf{x},\mathbf{e})=\{\begin{array}{ll}{G}_{\delta }-\delta ,& \text{if intersecting}\\ 0,& \mathrm{otherwise}\end{array}$$

$$V({\mathbf{x}}_{t-1},{\mathbf{x}}_t,{\mathbf{x}}_{t+1})=\frac{{\mathbf{x}}_t-{\mathbf{x}}_{t-1}}{{\Delta }_t}-\frac{{\mathbf{x}}_{t+1}-{\mathbf{x}}_t}{{\Delta }_{t+1}}$$

$$D({\mathbf{x}}_{t-1},{\mathbf{x}}_t,{\tilde{\mathbf{p}}}_t)=\frac{{\mathbf{v}}_t}{{\omega }_t}-c^2\frac{{\mathbf{f}}_t}{{\tau }_t}$$

$$\small\begin{split}\Phi^{*}&=\underset{\Phi}{\operatorname{arg\,min}}\sum^{T}_{t=1}\bigg{\{}\left\lVert\mathrm{D}(\mathbf{x}_{t-1},\mathbf{x}_{t},\tilde{\mathbf{p}}_{t}))\right\rVert_{\Sigma_{\mathrm{D}}}^{2}+\left\lVert\mathrm{V}(\mathbf{x}_{t-1},\mathbf{x}_{t},\mathbf{x}_{t+1})\right\rVert_{\Sigma_{\mathrm{V}}}^{2}\\ &+\left\lVert\mathrm{V}(\mathbf{e}_{t-1},\mathbf{e}_{t},\mathbf{e}_{t+1})\right\rVert_{\Sigma_{\mathrm{V}}}^{2}+\left\lVert\mathrm{C}(\mathbf{x}_{t},\mathbf{e}_{t})\right\rVert_{\Sigma_{C}}^{2}+\left\lVert C(\tilde{\mathbf{p}}_{t},\mathbf{x}_{t})\right\rVert_{\Sigma_{C}}^{2}\\ &+\left\lVert C(\tilde{\mathbf{p}}_{t},\mathbf{e}_{t})\right\rVert_{\Sigma_{C}}^{2}+\left\lVert S(\mathbf{x}_{t},\mathbf{e}_{t})\right\rVert_{\Sigma_{S}}^{2}+\left\lVert M(\mathbf{x}_{t},\mathbf{y}_{t})\right\rVert_{\Sigma_{M}}^{2}\\ &+\left\lVert M(\mathbf{e}_{t},\mathbf{z}_{t})\right\rVert_{\Sigma_{M}}^{2}+\left\lVert M(\tilde{\mathbf{p}}_{t},\tilde{\mathbf{w}}_{t})\right\rVert_{\Sigma_{M}}^{2}\bigg{\}}\end{split}$$

Full text

I Introduction & Related Work

Manipulation is a difficult problem, complicated by the challenge of robustly estimating the state of the robot’s interaction with the environment. Parameters such as the contact point and the force vector applied at that point, can be very hard to robustly estimate. These parameters are generally partially observable and must be inferred from noisy information obtained via coarse visual or depth sensors and highly sensitive but difficult to interpret tactile sensors.

For instance, in the case of “in-hand” manipulation problems, where a held object is often partially occluded by an end-effector, tactile sensing offers an additional modality that can be exploited to estimate the pose of the object [1].

Vision and tactile sensors have been used to localize an object within a grasp using a gradient-based optimization approach [2]. This has been extended to incorporate constraints like signed-distance field penalties and kinematic priors [1]. However, the former is deterministic and the latter handles uncertainty only per time-step, which is insufficient since sensors can be highly noisy and sensitive. Particle filtering-based approaches have been proposed that can infer the latent belief state from bi-modal and noisy sensory data, to estimate the object pose for two-dimensional grasps [3] and online localization of a grasped object [4]. These approaches are often limited in scope. For example, [4] uses vision to only initialize the object pose and later relies purely on contact information and dynamics models. In general, particle filtering based methods also suffer from practical limitations like computational complexity, mode collapse, and particle depletions in tightly constrained state spaces.

Beyond manipulation, sate estimation is a classic problem in robotics. For example, Simultaneous Localization and Mapping (SLAM) has been studied for many decades, and many efficient tools have been developed to address noisy multi-modal sensor fusion in these domains [5, 6, 7]. One of the more successful tools, the smoothing and mapping (SAM) framework [7], uses factor graphs to perform inference and exploits the underlying sparsity of the estimation problem to efficiently find locally optimal distributions of latent state variables over temporal sequences. This technique offers the desired combination of being computationally fast while accounting for uncertainty over time, and has been recently incorporated into motion planning [8, 9].

This framework has also been explored for estimation during manipulation [10, 11, 12]. In particular, Yu et al. [11] formulate a factor graph of planar pushing interaction (for non-prehensile and underactuated object manipulation) using a simplified dynamics model, with both visual object-pose and force-torque measurements and show improved pose recovery over trajectory histories compared to single-step filtering techniques. However, the scope of [11] is limited to the use of a purpose-built system, equipped with small-diameter pushing-rods kept at a vertical orientation, allowing for high-fidelity contact-point estimation. A fiducial-based tracking system is also used. Such high precision measurements are impractical in a realistic setting.

In this work, we extend the capabilities of such factor graph inference frameworks in several ways to perform planar pushing tasks in real world settings. We extend the representation to incorporate various geometric and physics-based constraints alongside multi-modal information from vision and tactile sensors. We perform ablation benchmarks to show the benefits of including such constraints, and benchmarks where the vision is occluded or the tactile sensors are very noisy, using data from on our own generalized systems. We conduct our tests on two systems, a dual-arm ABB Yumi manipulator equipped a gel-based Syntouch Biotac tactile sensor [13] and a Barrett WAM arm equipped with a pushing probe end effector mounted with a force torque sensor (see Fig.1). Both of these systems are also set up with a vision-based articulated tracking system that leverages a depth camera, joint encoders, and contact-point estimates [1].

Through inference, we jointly estimate the history of not only object poses, and end-effector poses, but also, contact points, and applied force vectors. Estimating contact points and applied force vectors can be very useful in tractable dynamics models to predict future states and can be beneficial to contact-rich planning and control for manipulation [14].

With our experiments, we show that we can contend with a range of multi-modal noisy sensor data and perform efficient inference in batch and incremental settings to provide high-fidelity and consistent state estimates.

II Dynamics of Planar Pushing

In this section, we review the dynamics model for pushing on planar surfaces. The quasi-static approximation of this model is used in the next section to describe the motion model of the pushed object within the factor graph for estimation.

Given an object of mass $m$ being pushed with an applied force $f$, we can describe the planar dynamics of the rigid body through the primary equations of motion

[TABLE]

where $x_{CM}$ is the object position measured at the center-of-mass (CM), $\omega$ the angular velocity of the object frame, $I_{CM}$ the moment of inertia, and $f_{\mu}$ the linear frictional force. The applied and frictional moments are defined as $\tau={x}_{CM}\times f$ and $\tau_{\mu}={x}_{CM}\times f_{\mu}$ respectively.

We can estimate the frictional loads on the object by considering the contribution of each point on the support area $A$ of the object [10]. The friction force $f_{\mu}$ and corresponding moment $\tau_{\mu}$ is found by integrating Coulomb’s law across the contact region of the object with the surface

[TABLE]

where $v(r)$ denotes the linear velocity at a point $r$ in area $A$, and $P(r)$ the pressure distribution. The coefficient of friction is assumed to be uniform across the support area.

For pusher trajectories that are executed at near-constant speeds, inertial forces can be considered negligible. The push is then said to be quasi-static, where the applied force is large enough to overcome friction and maintain a velocity, but is insufficient to impart an acceleration [15]. Then, the applied force $f$ must lie on the limit surface. This surface is defined in $(f_{x},f_{y},\tau)$ space and encloses all loads under which the object would remain stationary [16]. It can be approximated as an ellipsoid with principal semi-axes $f_{max}$ and $\tau_{max}$ [17]

[TABLE]

where $f_{max}=\mu_{s}f_{n}$, and $f_{n}$ is the normal force. In order to calculate $\tau_{max}$, we assume a uniform pressure distribution and define $r$ with respect to the center of mass ($r=r_{CM}$): $\tau_{max}=-\mu_{s}\frac{mg}{A}\int_{A}|r_{CM}|dA$. For quasi-static pushing, the velocity is aligned with the frictional load, and therefore must be parallel to the normal of the limit surface. This results in the following constraints on the object motion

[TABLE]

used within our estimation factor graph in the next section.

III State Estimation with Factor Graphs

To solve state estimation during manipulation we formulate a factor graph of belief distributions over any state and force vector trajectory and perform inference over the trajectory given noisy sensor measurements. The graph construction and inference is performed with GTSAM [7, 18] via sparsity exploiting nonlinear least squares optimization to find the maximum a posteriori (MAP) trajectory that satisfies all the constraints and measurements. In the batch setting we use a Gauss-Newton optimizer and in an incremental setting we use iSAM2 that performs incremental inference via Bayes trees [19]. All random variables and measurements are assumed to have a Gaussian distribution. In the remainder of this section, we describe the construction of the relevant factor graphs depicted in Fig. 2.

III-A Model Design

We construct three different factor graphs for state estimation in our pushing task: CP, SDF, and QS (see Fig. 2). All three models include the latent state variables for a given time $t$: the planar object pose $\mathbf{x}_{t}\in{SE(2)}$, the projected end-effector pose $\mathbf{e}_{t}\in{SE(2)}$, and the contact point $\mathbf{p}_{t}\in\mathbb{R}^{2}$.

Measurements: Each of the latent state variable is accompanied by an associated measurement factor $\mathrm{M}$ which projects corresponding measurements from $SE(3)$ into the pushing plane. The object poses are estimated by the visual tracking system with measurements $\mathbf{y}_{t}\in{SE(3)}$. Likewise, the end-effector pose measurements $\mathbf{z}_{t}\in{SE(3)}$ may be provided from robot forward kinematics, or from the tracking system (DART includes a prior on joint measurements). The contact-point measurements $\mathbf{w}_{t}\in{SE(3)}$ are provided by a tactile sensor model. In the QS graph (Fig. 2(c)), we include a new state variable for the applied planar contact force $\mathbf{f}_{t}\in\mathbb{R}^{2}$ with corresponding measurements $\mathbf{\alpha}_{t}\in\mathbb{R}^{3}$. For simplicity of graphical representation, we combine the contact point and force variables:

[TABLE]

Geometric Constraints: We assume constant point-contact between the end-effector and the object. We include the factor $\mathrm{C}$ which incurs a cost on the difference between the contact point $\mathbf{p}_{t}$ and the closest point to a surface ($\xi$) :

[TABLE]

where $G\left(\xi,~{}\mathbf{p}_{t}\right)$ is the projection of $\mathbf{p}_{t}$ onto $\xi$, and $\xi=\xi\left(\cdot\right)$ returns the surface geometry of a body with a given pose: $\xi=\xi\left(\mathbf{x}_{t}\right)$ for the object, and $\xi=\xi\left(\mathbf{e}_{t}\right)$ for the end-effector. Additionally, the object and the end-effector must be prevented from occupying the same region in space. Such a constraint is necessary in practice where contact-point estimation is often noisy. Therefore, we introduce a factor $\mathrm{S}$ to penalize intersecting geometries with a signed distance field. Let the point on the end-effector furthest into the object be denoted by $\mathbf{\delta}\in\mathbb{R}^{2}$, where $\mathbf{\delta}=\mathbf{\delta}\left(\mathbf{x},\xi\left(\mathbf{e}\right)\right)$. The projection of $\delta$ onto $\xi\left(\mathbf{x}\right)$ (the surface of the object) is then defined by $G_{\delta}=G\left(\mathbf{\xi\left(x\right)},\delta\right)$, and we can apply a penalty

[TABLE]

Dynamics: We add a constant velocity prior $V$ to impose smoothness on state transitions. For example, for finite-difference velocities of object poses we have

[TABLE]

where $\Delta_{t}$ and $\Delta_{t+1}$ denote the timestep sizes at $t$ and $t+1$. Similar to [11], we introduce an additional factor $D$ to condition object state transitions on quasi-static pushing. The corresponding graphical model is denoted by QS and is shown in Fig. 2(c). From Eq. 4 we get

[TABLE]

where $\mathbf{v}_{t}=({\mathbf{x}_{\mathrm{trans},t}-\mathbf{x}_{\mathrm{trans},t-1})/\Delta_{t}}$ and $\omega_{t}=({\mathbf{x}_{\mathrm{rot},t}-\mathbf{x}_{\mathrm{rot},t-1})/\Delta_{t-1}}$ are the finite-difference linear and angular velocity, respectively. The final cost function is optimized with respect to the set of variables $\Phi=\{(\mathbf{x},\mathbf{e},\tilde{\mathbf{p}})\}_{t=1}^{t=T}$ over a trajectory of length $T$ :

[TABLE]

The above equation provides the locally optimal i.e. MAP solution of the estimation problem.

IV Baseline Comparison

In order to first ascertain the general performance of our approach, we evaluate the QS-graph on the MIT planar pushing dataset [20] using batch optimization. This data contains a variety of pushing trajectories for a single-point robotic pushing system. The object poses were tracked with a motion capture system, and contact forces were measured with a pushing probe mounted on a force-torque sensor. We use this data as ground truth, since it is considered to be sufficiently reliable. We restrict our experiments to a subset of this data, using trajectories with zero pushing acceleration and velocities under 10 cm/s in order to maintain approximately quasi-static conditions. Additionally, we only consider trajectories on the ABS surface, but examine different object types (ellip1, rect1, rect3) with approximately 100 trajectories per object and measurements provided at 100Hz. Gaussian noise is artificially added to the measurements prior to inference, with the following sigma values: $\sigma_{\mathbf{x}_{\mathrm{trans}}}=0.5\mathrm{cm},\;\sigma_{\mathbf{x}_{\mathrm{rot}}}=0.5\mathrm{rad},\;\sigma_{\mathbf{e}_{\mathrm{trans}}}=0.5\mathrm{cm},\;\sigma_{\mathbf{e}_{\mathrm{rot}}}=0.5\mathrm{rad},\;\sigma_{\mathbf{p}}=0.5\mathrm{cm},\;\sigma_{\mathbf{f}}=0.5\mathrm{N}$.

The resulting RMS and covariance values post-optimization are shown in Table I. The optimized values exhibit marked reductions in error compared to the sigma values of the initial measurements. Note that, for object poses we only include values in which the object is in motion, in order to exclude trivial stationary estimates. All position-related values are in cm, with angular values in radians, and forces in Newtons. An example of an optimized trajectory is shown in Fig. 3. Although the observation noise is artificial, these results indicate that latent state estimates may still be successfully recovered with the addition of geometric and physics-based priors, and without over-constraining the optimization.

V State Estimation in Open and Cluttered scenes

We first perform pushing experiments with the Barrett WAM manipulator acting on a laminated box as shown in Fig. 4. The system is observed by a stationary PrimeSense depth camera located 2.0m away from the starting push position of the end-effector. Vision-based tracking measurements of the object pose are provided by DART, configured with contact-based priors and joint estimates [1]. The robot is equipped with a Force-Torque sensor and a rigid end-effector mounted with a spherical hard-plastic pushing probe. The contact forces are measured by the F/T sensor, with contact point measurements provided through optimization in DART. Ground-truth poses are provided via a motion-capture system. The table is mounted with a smooth delrin sheet to provide approximately uniform friction across the pushing area.

We performed 100 pushing trials with varying initial end-effector and object poses. The end-effector trajectories were varied in curvature and maintained a translational speed close to 6 cm/s to approximate quasi-static conditions. Object pose-tracking measurements were provided at roughly 25Hz, with end-effector poses and force/contact measurements published at 250Hz. Incremental inference of the factor graph is performed after 5 object pose measurements.

In order to provide real-time performance, DART maintains a belief distribution over state at a single timestep. However, this can make tracking susceptible to unreliable measurements that may arise from state-dependent uncertainty or partial observability. As such, we purposely include trajectories in which the object orientation changes significantly with respect to the camera orientation, causing large variations in pointcloud association. In addition, the pushing trajectories were also performed in cluttered scenes, as depicted in Fig. 4, with 85% occlusion of the pushing object occurring in the middle of the trajectory.

Examples of measured and estimated state trajectories are shown in Fig. 6. In the fully-observable (unocccluded) setting, distinct improvement of the object pose can be seen with both SDF and QS models. Under heavy occlusion, the visual tracking system loses the object and is unable to regain the trajectory state. However, the addition of both geometric and physics based priors to the factor graph result in realignment of the tracked object. Fig. 5 shows the tracking performance for fully observable trajectories using the CP, SDF, and QS factor graphs. The results are compared to the model proposed by Yu et al. [11], which includes quasi-static dynamics factors with contact and zero-velocity priors.

In addition to improving inference on kinematic trajectories, the QS graph can be used to improve contact point and force estimates. To demonstrate this, we artificially add non-Gaussian noise (bi-modal mixture of two triangular distributions) to contact points and force measurements on the ground-truth data. The resulting estimation errors after optimization are shown in Table II, and indicate that our approach manages to recover true contact points and pushing forces. An an example of force-trajectory optimization is illustrated in Fig. 7.

VI Force Estimation for Tactile Sensing

We further demonstrate inference on force trajectories using realistic (noisy) tactile data. The Biotac sensor comprises of a solid core encased in an elastomeric skin and is filled with weakly-conductive gel [13]. The core surface is populated by an array of 19 electrodes, each measuring impedance as the thickness of the fluid between the electrode and the skin changes. A transducer provides static pressure readings which consist of a single scalar value per time-step. This sensor is also equipped with a thermistor for measuring fluid temperature. Although the device does not directly provide a force distribution or contact point measurements, an analytical method for estimating these values is described in [13].

Using an ABB YUMI robot with a mounted Biotac sensor, we generated randomized linear trajectories of the end effector pushing a 0.65 kg box across a laminated surface (see Fig. 1) starting from a number of different poses. We used the DART tracking system [1] to obtain object and end-effector pose measurements, along with approximate contact points. The analytical force sensor model [13], was used to provide initial force measurements.

Examples of initial and optimized trajectories are shown in Fig. 8-9. The presence of the contact surface factor shrinks the contact point covariance in the direction of push, as is expected. The covariances for finger and object pose estimates are drastically reduced, exhibiting the benefits of joint-inference across trajectory histories. Also, the dynamics factor aligns the force vector in the direction of motion of the object. This is further clarified in Fig. 9, where force vectors are correctly aligned with the object center-of-mass for linear trajectories, and provide a moment arm during angular displacement. This demonstrates the importance of contact and geometric factors in aligning the surface tangents of the finger and the object at the point of contact.

VII Conclusion

We proposed a factor graph-based inference framework to solve estimation problems for robotic manipulation in batch and incremental settings. Our approach can leverage geometric and physics-based constraints along with vision and tactile based multi-modal sensor information to jointly estimate the history of robot and objects poses along with contact locations and force vectors. We perform several benchmarks on various datasets with multiple manipulators in real environments and show that our framework can contend with sensitive, noisy sensor data and occlusions in vision to efficiently solve for locally optimal state estimates that closely match ground truth. Future work will include incorporating the approach within a motion planning context [9], combining vision and tactile modalities in learning predictive sensor models [22, 23], and the possibility of integration into a hierarchical task-planning framework.