Contact Detection from Joint Torque Measurements

\newcommand{\rv}[1]{\mathsfit{#1}} \newcommand{\rvv}[1]{\bm{\mathsfit{#1}}}

\newcommand{\rv}[1]{\mathsfit{#1}} \newcommand{\rvv}[1]{\bm{\mathsfit{#1}}}

Pang, joint work with Jack

Method by Haddadin

Identify the link in contact as the last link with non-zero 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 Albu-Schäffer. "Robot collisions: A survey on detection, isolation, and identification." IEEE Transactions on Robotics 33.6 (2017): 1292-1312.

\bm{J}_{L_{i_c}}^\intercal \begin{bmatrix} \bm{m}_{i_c} \\ \bm{f}_{i_c} \end{bmatrix} = \bm{\tau}_\text{ext}

\bm{J}_{L_{i_c}}^\intercal \begin{bmatrix} \bm{m}_{i_c} \\ \bm{f}_{i_c} \end{bmatrix} = \bm{\tau}_\text{ext}

\begin{bmatrix} \bm{I}_3 & \textcolor{blue}{\hat{\bm{p}}} \\ \bm{0}_3 & \bm{I}_3 \end{bmatrix} \begin{bmatrix} \bm{0} \\ \bm{f}_\text{ext} \end{bmatrix} = \begin{bmatrix} \bm{m}_{i_c} \\ \bm{f}_{i_c} \end{bmatrix}

\begin{bmatrix} \bm{I}_3 & \textcolor{blue}{\hat{\bm{p}}} \\ \bm{0}_3 & \bm{I}_3 \end{bmatrix} \begin{bmatrix} \bm{0} \\ \bm{f}_\text{ext} \end{bmatrix} = \begin{bmatrix} \bm{m}_{i_c} \\ \bm{f}_{i_c} \end{bmatrix}

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.

\bm{p}

\bm{p}

\bm{f}

\bm{f}

\bm{p}_{xy}

\bm{p}_{xy}

\bm{f}_{xy}

\bm{f}_{xy}

p_y

p_y

p_x

p_x

f_x

f_x

f_y

f_y

\frac{2 \epsilon }{p_x}

\frac{2 \epsilon }{p_x}

x

y

z

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.
Small-torque 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

\mu=1, \; \epsilon / \|\bm{f}\| = 0.1 \; \left(\text{N} \cdot \text{m} / \text{N}\right)

\mu=1, \; \epsilon / \|\bm{f}\| = 0.1 \; \left(\text{N} \cdot \text{m} / \text{N}\right)

\mu=1, \; \epsilon / \|\bm{f}\| = 0.05 \; \left(\text{N} \cdot \text{m} / \text{N}\right)

\mu=1, \; \epsilon / \|\bm{f}\| = 0.05 \; \left(\text{N} \cdot \text{m} / \text{N}\right)

\mu=1, \; \epsilon / \|\bm{f}\| = 0.02 \; \left(\text{N} \cdot \text{m} / \text{N}\right)

\mu=1, \; \epsilon / \|\bm{f}\| = 0.02 \; \left(\text{N} \cdot \text{m} / \text{N}\right)

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}$ . Re-sampling 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. 5062-5069). IEEE

l(\bm{\tau_\text{ext}, p, q}) \coloneqq \underset{\bm{v_f} \geq 0}{\text{min}} \; \| \bm{J}(\bm{q}, \bm{p})^{\intercal} \bm{v_f} - \bm{\tau}_\text{ext} \|^2

l(\bm{\tau_\text{ext}, p, q}) \coloneqq \underset{\bm{v_f} \geq 0}{\text{min}} \; \| \bm{J}(\bm{q}, \bm{p})^{\intercal} \bm{v_f} - \bm{\tau}_\text{ext} \|^2

\underset{\bm{v_f} \geq 0 \; \bm{p} \in \mathcal{S}}{\text{min}} \; \| \bm{J}(\bm{q}, \bm{p})^{\intercal} \bm{v_f} - \bm{\tau}_\text{ext} \|^2

\underset{\bm{v_f} \geq 0 \; \bm{p} \in \mathcal{S}}{\text{min}} \; \| \bm{J}(\bm{q}, \bm{p})^{\intercal} \bm{v_f} - \bm{\tau}_\text{ext} \|^2

\exp{ -\frac{l(\bm{\tau_\text{ext}, p, q})}{\sigma^2}}

\exp{ -\frac{l(\bm{\tau_\text{ext}, p, q})}{\sigma^2}}

$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%

l(\bm{\tau_\text{ext}, p, q}) \coloneqq \underset{\bm{v_f} \geq 0}{\text{min}} \; \| \bm{J}(\bm{q}, \bm{p})^{\intercal} \bm{v_f} - \bm{\tau}_\text{ext} \|^2

l(\bm{\tau_\text{ext}, p, q}) \coloneqq \underset{\bm{v_f} \geq 0}{\text{min}} \; \| \bm{J}(\bm{q}, \bm{p})^{\intercal} \bm{v_f} - \bm{\tau}_\text{ext} \|^2

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!

\underbrace{\bm{J}(\textcolor{blue}{\bm{q}}, \bm{p})}_{n_v \times n_q}\bm{v_f} = \underbrace{\textcolor{green}{\bm{v}(\bm{p})^\intercal}}_{n_v \times 3} \underbrace{ \begin{bmatrix} -\hat{\bm{p}} & \textcolor{blue}{\bm{I}_3} \end{bmatrix} }_{3 \times 6} \underbrace{\textcolor{blue}{\bm{J}_L(\bm{q})}}_{6 \times n_q} \bm{v_f}

\underbrace{\bm{J}(\textcolor{blue}{\bm{q}}, \bm{p})}_{n_v \times n_q}\bm{v_f} = \underbrace{\textcolor{green}{\bm{v}(\bm{p})^\intercal}}_{n_v \times 3} \underbrace{ \begin{bmatrix} -\hat{\bm{p}} & \textcolor{blue}{\bm{I}_3} \end{bmatrix} }_{3 \times 6} \underbrace{\textcolor{blue}{\bm{J}_L(\bm{q})}}_{6 \times n_q} \bm{v_f}

Constants

L

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:
1. Find a set of samples: $\overline{P}_{\epsilon} \subset P_{\epsilon}$ using rejection sampling.
2. Run gradient descent on all $\bm{p} \in \overline{P}_{\epsilon}$ , creating a new set of converged samples: $\overline{P}_{\epsilon}^*$
3. Ask the robot to collect more data to find the true contact from $\overline{P}_{\epsilon}^*$ (TODO).

l( \textcolor{blue}{\bm{\tau_\text{ext}}, \bm{q}}, \bm{p}) \coloneqq \underset{\bm{v_f} \geq 0}{\text{min}} \; \| \bm{J}(\bm{q}, \bm{p})^{\intercal} \bm{v_f} - \bm{\tau}_\text{ext} \|^2 \\

l( \textcolor{blue}{\bm{\tau_\text{ext}}, \bm{q}}, \bm{p}) \coloneqq \underset{\bm{v_f} \geq 0}{\text{min}} \; \| \bm{J}(\bm{q}, \bm{p})^{\intercal} \bm{v_f} - \bm{\tau}_\text{ext} \|^2 \\

10000 samples / link

1000 samples / link

$\overline{P}_\epsilon^*$ : white points.

Gradient descent

$P_\epsilon$ : points in white box

	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%

Contact Detection from Joint Torque Measurements \newcommand{\rv}[1]{\mathsfit{#1}} \newcommand{\rvv}[1]{\bm{\mathsfit{#1}}} Pang, joint work with Jack

Contact Detection from Joint Torque Measurements

Motivation

Three ways to detect contact from joint torques

Method by Haddadin

Pitfalls

Thresholding joint torque

Computing the probability of $|\tau_i| \leq \epsilon$

Results

Contact Particle Filter

But there could be more than one $\bm{p}$ where $l(\bm{\tau_\text{ext}, p, q}) = 0$

CPF failure modes

Multiple Local Minima of $l$

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$ ?

Alternative strategy

Gradient descent on $\mathcal{S}$

How well does this work?

Failure modes