Contact Estimation from External Joint Torque Measurements on a serial robotic arm
Tao Pang, Jack Umenberger and Russ Tedrake
Robot Locomotion Group
{pangtao, ju, russt}@csail.mit.edu
Motivation
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7543462/pasted-from-clipboard.png)
estimated contact force
- Robot arms such as KUKA IIWA and FRANKA PANDA can estimate \(\tau_{\text{ext}}\) using the generalized momentum observer.
- Assuming there is one external, point-contact force, existing methods can estimate \(\bm{p}_C\) and \(\bm{f}_C\) from \(\tau_{\text{ext}}\).
- However, some \(\tau_{\text{ext}}\) can be explained equally well by multiple contact points.
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.
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).
- Existing methods do not take into account multiple solutions.
- The method by Haddadin et al. returns a contact point on the last link with non-zero torque measurement.
- Contact Particle Filter by Manuelli and Tedrake can return either one of the solutions.
- Goal: estimating from only external torque \(\tau_{\text{ext}}\)
- \(\bm{p}_C \in \mathcal{S}\): contact point C on the robot's surface \(\mathcal{S}\).
- \(\bm{f}_C \in \mathbb{R}^3\): contact force at point C.
motor torque.
external torque due to contact
gravitational torque.
joint angles.
Coriolis.
Mathematically...
- We would like to solve
for the contact position \(\bm{p}_C\) and contact force \(\bm{f}_C\), subject to
- \(\bm{p}_C \in \mathcal{S}\): point C is on the robot's surface.
- \(\bm{f}_C \in \mathcal{K}_C\): force \(\bm{f}_C\) is inside the friction cone at C.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905340/pasted-from-clipboard.png)
- This is equivalent to solving
- The minimum squared error of at a known point C can be obtained by solving a quadratic program (QP):
- Common assumptions:
- Only one external contact.
- Contacts create no moment about the contact point.
Components of \(\bm{f}_C\) along \(\bm{v}_{C_i}\).
Link 6 of IIWA.
- Finding all possible contact positions is the same as finding all global minima of
The landscape of
on the robot's surface \(\mathcal{S}\)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905584/pasted-from-clipboard.png)
- We propose an algorithm to find all local minima of
- Considering that torque measurements are noisy, knowing the local minima in addition to the global ones could actually be beneficial.
Find all local minima of \(l(\cdot; \bm{q}, \bm{\tau}_\text{ext})\) with Rejection Sampling
- Sample points on the robot's surface, take the points with \(l \leq \epsilon\).
- Rejection rate is 98% with \(\epsilon = 0.005\).
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905398/pasted-from-clipboard.png)
(a): accepted samples out of 10000 samples/link.
(b): accepted samples out of 500 samples/link.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905584/pasted-from-clipboard.png)
Find all local minima of \(l(\cdot; \bm{q}, \bm{\tau}_\text{ext})\) with Rejection Sampling + Gradient Descent (RSGD)
- Rejection sampling with a larger threshold \(\delta\).
- For example, acceptance rate rises to 17.5% with \(\delta = 0.1\)
- Run gradient descent on the accepted samples to converge to local minima of \(l(\cdot)\).
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905452/pasted-from-clipboard.png)
- \(P_\delta\): set of accepted samples.
- \(P_\delta^*\): set of optimized samples.
Find all local minima of \(l(\cdot; \bm{q}, \bm{\tau}_\text{ext})\) with Rejection Sampling + Gradient Descent (RSGD)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905493/pasted-from-clipboard.png)
- \(P_\delta\): set of accepted samples.
- \(P_\delta^*\): set of optimized samples.
- Accepted / all samples: 172/1000
- Link 5 , converged_samples / accepted samples: 78/84
- Link 6 , converged_samples / accepted samples: 80/88
Works in other cases too! Runs at 10Hz on average.
Active Contact Discrimination
- Disambiguate candidate contact positions found by Rejection Sampling + Gradient Descent by making small motions.
Active Contact Discrimination
- Disambiguate candidate contact positions found by Rejection Sampling + Gradient Descent.
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905544/pasted-from-clipboard.png)
Total number of contacts.
Normal and Jacobian at contact \(i\).
Push/pull at contact \(i\).
Constrains sideways motion.
Small motion.
Take-aways
- Detecting contacts from \(\bm{\tau}_\text{ext}\) on multiple links is probably not reliable in practice.
- Needs low-noise, unbiased estimate of \(\bm{\tau}_\text{ext} \).
- Can be ambiguous even in the single contact case.
- Cannot handle more than one contacts simultaneously.
- Tactile skin is perhaps the better solution?
How well does this work?
No. Tests | Gradient Descent successes* | Success% | RDGD 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?
-
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7563191/pasted-from-clipboard.png)
Flipped normal
test id 4838
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7563225/pasted-from-clipboard.png)
Proximity query returns a point "off-mesh".
test id 510
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7563265/pasted-from-clipboard.png)
True contact on a "ledge", not covered by samples. Therefore the local minimum doesn't exist on the robot surface.
test id 868
Find all local minima of \(l(\cdot; \bm{q}, \bm{\tau}_\text{ext})\) with Rejection Sampling + Gradient Descent (RSGD)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905493/pasted-from-clipboard.png)
- \(P_\delta\): set of accepted samples.
- \(P_\delta^*\): set of optimized samples.
- Accepted / all samples: 172/1000
- Link 5 , converged_samples / accepted samples: 78/84
- Link 6 , converged_samples / accepted samples: 80/88
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905521/pasted-from-clipboard.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/7905466/pasted-from-clipboard.png)
Average Number of QP solves per gradient descent: 43.4.
Contact from joint torque ICRA21
By Pang
Contact from joint torque ICRA21
- 14