Contact Detection from Joint Torque Measurements
Pang, joint work with Jack
Motivation
 Robots can and do make contact with the environment when they are working, e.g. picking up plates.
 We want the robot to keep working despite contacts.
 To accomplish that goal, an important component is knowing where the contact is.
 Tactile skins are nice, but can be fragile and cumbersome.
 Joint torque sensors are readily available and more robust.
Three ways to detect contact from joint torques
 Method by Haddadin
 Samplingbased methods, e.g. Contact Particle Filter.
 "Gradient descent" (our work)
Method by Haddadin
 Identify the link in contact as the last link with nonzero torque measurement.
$$\bm{\tau}_\text{ext} = \left[ \tau_1, \tau_2, \dots, \tau_{\textcolor{blue}{i_c}}, 0, 0, 0\right]$$
 Solve for \( \bm{m}_{i_c}, \bm{f}_{i_c} \) from
 Assume that \( \bm{m}_\text{ext} = 0 \), and solve for the line of action of \( \bm{f}_\text{ext} \) from
 Intersect the line with \(\mathcal{S}_{i_c}\), the surface of link \(i_c\).
Haddadin, Sami, Alessandro De Luca, and Alin AlbuSchäffer. "Robot collisions: A survey on detection, isolation, and identification." IEEE Transactions on Robotics 33.6 (2017): 12921312.
geometric Jacobian of frame \(L_{i_c}\)
Pitfalls
 \(\bm{\tau}_\text{ext}\) is almost always not zero.
 Biased due to imperfect calibration of EE inertia.
 Noisy due to robot motion.
 Instead of \(\tau_i = 0 \), Haddadin proposed checking \(\tau_i \leq \epsilon \).
 The contact detector makes a mistake when \(\bm{f}\) is on link \(i_c\) but \(\tau_{i_c} \leq \epsilon \).
$$\bm{q}_1$$
$$\bm{q}_2$$
Moving...
Thresholding joint torque
 Using link 6 of the IIWA robot as an example.
 Only the \(z\) component of \(\bm{m} = \bm{p} \times \bm{f}\) is sensed by the joint torque sensor.
 Thresholding \( \tau_6 = m_z = p_y f_x + p_x f_y\leq \epsilon \) : a linear constraint on the projection of \(\bm{f}\) onto the \(xy\) plane.
Link 6 of IIWA in its local coordinate frame.
Computing the probability of \(\tau_i \leq \epsilon\)
For a sampled point \( \bm{p} \) on the surface of the link:
 Sample forces of magnitude \(1\text{N}\) inside the friction cone at \( \bm{p} \).
 Project force samples onto the \(xy\) plane.
 Count number of samples inside the "band of small torque".
 Error probability = (# small torque samples) / (# all samples)
 Force samples on friction cone.
 Projected samples.
 Smalltorque samples.
Space of contact force \(\bm{f}\)
Results
Space of contact force \(\bm{f}\)
$$\mu=1, \; \epsilon / \\bm{f}\ = 0.01 \; \left(\text{N} \cdot \text{m} / \text{N}\right)$$
Probability: 0
1
Thresholding joint torque
 Instead of \(\tau_i = 0 \), Haddadin proposed checking \(\tau_i \leq \epsilon \).
 Only the \(z\) component of \(\bm{m} = \bm{p} \times \bm{f}\) is sensed by the joint torque sensor.
 Thresholding \( \tau_6 = m_z = p_y f_x + p_x f_y\leq \epsilon \) : a linear constraint on the projection of \(\bm{f}\) onto the \(xy\) plane.
Link 6 of IIWA in its local coordinate frame.
Contact Particle Filter
 For a given joint configuration \( \bm{q} \) and a joint torque measurement \(\bm{\tau}_\text{ext} \), if \(\bm{\tau}_\text{ext} \) is generated by a single external contact, there exists \( \bm{p} \in \mathcal{S} \) (robot surface) s.t. \( \ \bm{J}(\bm{q}, \bm{p})^{\intercal} \bm{v_f}  \bm{\tau}_\text{ext} \^2 = 0, \bm{v_f} \geq 0\).
 To find \(\bm{p}\), we would like to solve
 But the constraint that \( \bm{p} \) stays on the surface of the robot, \( \bm{p} \in \mathcal{S} \), is difficult to deal with.
 Instead, samples are drawn from \( \mathcal{S} \) so that \( \bm{p} \) is fixed for each sample.
 By replacing the friction cone with a polyhedral cone, the optimization above becomes a QP:
 The samples start uniformly distributed on \(\mathcal{S}\). Resampling is weighted by
 Unlike the Haddadin method, CPF always converge to a point on the robot.
Manuelli, L. and Tedrake, R., 2016, October. Localizing external contact using proprioceptive sensors: The contact particle filter. In 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (pp. 50625069). IEEE
\(l = 0\)
\(l \) large
10000 samples / link
But there could be more than one \( \bm{p} \) where \( l(\bm{\tau_\text{ext}, p, q}) = 0 \)
Some contacts can be explained equally well by multiple contact locations.
Out of 10000 randomly generated configuration and contacts, how often does contact particle filter fail to identify the true contact location?
25%
CPF failure modes
Multiple Local Minima of \(l\)
\(l = 0\)
\(l \) large
Where on \(\mathcal{S}\) is \( \ \bm{J}(\textcolor{blue}{\bm{q}}, \bm{p})^{\intercal} \bm{v_f}  \textcolor{blue}{\bm{\tau}_\text{ext}} \^2 = 0\)?
 \(\mathcal{S}\) is a triangle mesh: \( \textcolor{green}{\bm{v}(\bm{p})} \) is a piecewise constant function of \(\bm{p}\).
 Even on a single piece, the objective function is nonlinear in the decision variables:
 And there are tens of thousands of pieces!
Constants
Alternative strategy
 Describing the set \( P_0 \coloneqq \{\bm{p} \in \mathcal{S} : l( \textcolor{blue}{\bm{\tau_\text{ext}}, \bm{q}}, \bm{p}) = 0\} \) is hard.
 Contact estimation from a single \(\bm{\tau}_\text{ext}\) can be ambiguous.
 But sampling from \( P_\epsilon \coloneqq \{\bm{p} \in \mathcal{S} : l( \textcolor{blue}{\bm{\tau_\text{ext}}, \bm{q}}, \bm{p}) \leq \epsilon\} \) is easier.
 New strategy:
 Find a set of samples: \( \overline{P}_{\epsilon} \subset P_{\epsilon} \) using rejection sampling.
 Run gradient descent on all \( \bm{p} \in \overline{P}_{\epsilon} \), creating a new set of converged samples: \( \overline{P}_{\epsilon}^* \)
 Ask the robot to collect more data to find the true contact from \( \overline{P}_{\epsilon}^* \) (TODO).
10000 samples / link
10000 samples / link
1000 samples / link
\( \overline{P}_\epsilon^* \) : white points.
Gradient descent
\( P_\epsilon \): points in white box
Gradient descent on \( \mathcal{S}\)
while \( \ \nabla_\bm{p} l \ \geq \epsilon \):
 project \( \nabla_\bm{p} l \) to the plane of the triangle of \( \bm{p} \).
 descend along the projected gradient to a new point \( \bm{p}^+ \) using line search.
 project \( \bm{p}^+ \) back to the mesh.
test idx: 1343
link 6
How fast do they converge?
(Show this in meshcat)
How well does this work?
No. Tests  Gradient Descent successes*  Success%  GD w/ Rejection successes 
Success%  

CPF succeeds  7551  7326  97%  
CPF no detection  916  874  95%  
CPF too far away  1365  1112  81%  1255  92% 
 * Success means the true contact location is within 1cm of at least 1 of the samples gradient descent converges to.
 50 gradient descents are run on each link, with number of iterations capped at 100.
Failure modes

Many of the failures are due to anomalies in the mesh: "grooves (test 9701) and ledges".

Improve mesh quality.


Some are due to failures in proximity queries

Use signed distance field instead of bounding volume hierarchy?

Flipped normal
test id 4838
Proximity query returns a point "offmesh".
test id 510
True contact on a "ledge", not covered by samples. Therefore the local minimum doesn't exist on the robot surface.
test id 868
What does \( \overline{P}_\epsilon \) look like?
 White squares are members of \( \overline{P}_\epsilon \).
 Generated from 100 samples (50 / link).
 Number of converged samples: 23.
Alternative strategy
 Describing the set \( P_0 \coloneqq \{\bm{p} \in \mathcal{S} : l( \textcolor{blue}{\bm{\tau_\text{ext}}, \bm{q}}, \bm{p}) = 0\} \) is hard.
 But sampling members of \(P^* \coloneqq \{\bm{p} \in \mathcal{S} : \nabla_\bm{p} l ( \textcolor{blue}{\bm{\tau_\text{ext}}, \bm{q}}, \bm{p}) = 0\}\) is easier.
 \( P_0 \subset P^{*} \) (?)
 Members of \( P^* \) can be found by gradient descent, starting from any \(\bm{p} \in \mathcal{S}\). The gradient is given by
 Running gradient descent from many initial positions gives samples from \(P^*\): \( \overline{P}^* \subset P^* \).
 We can form a new set \( \overline{P}^*_\epsilon \coloneqq \{ \bm{p} \in \overline{P}^*: l( \textcolor{blue}{\bm{\tau_\text{ext}}, \bm{q}}, \bm{p}) \leq \epsilon \}\).
 To find the true contact location from \( \overline{P}^*_\epsilon\), we can compute a robot motion to collect more measurements (ongoing work).
From KKT conditions.
From AutoDiff.
Contact Detection from Joint Torque Measurements
By Pang
Contact Detection from Joint Torque Measurements
 30