Sensorless Localization and Estimation of External Contact Force

Sensorless Localization and Estimation of External Contact Force Mattia Castelmare Dept. of Computer, Control and Management Engineering Sapienza University of Rome, Italy [email protected] Alessandro De Luca Dept. of Computer, Control and Management Engineering Sapienza University of Rome, Italy [email protected] Abstract—Effectively managing unintended contacts between humans and collaborative robots is critical for ensuring safety in industrial and service environments. Sensorless methods, relying solely on proprioceptive measurements, such as joint torque sensors and encoders, and mathematical models provide a practical and cost-effective alternative to external sensing. This paper explores a hybrid strategy that integrates the classical momentum-based residual method using pseudoinversion with a contact particle filter (CPF) to enhance estimation of an external contact force and its localization across all links of a 7-DOF manipulator. For contacts on a distal robot link (i≥6), the residual method yields accurate estimates of both a pure external force and its contact point, with CPF further refining the solution. Conversely, when contact occurs on a proximal link (i<6), the associated link Jacobian loses rank and the pseudoinverse method alone fails. Instead, the proposed combination enables reliable estimation of contact force and location. Experimental validation on a KUKA LWR4+ robot highlights robustness and practical applicability of this approach. Index Terms—human-robot interaction, collision detection, contact particle filter, force estimation, collaborative robotics I. INTRODUCTION Physical human-robot interaction (pHRI) requires effective contact and collision management to ensure safety while enabling seamless collaboration. In industrial and service contexts, intentional contacts, as in tool-passing or shared manipulation, or unintentional collisions, as during semiautonomous tasks, may occur. Ensuring safe yet efficient interaction is therefore a central challenge in collaborative robotics. Different sensing solutions have been explored for collision detection. External sensors such as RGB-D cameras or onboard vision systems provide useful information about the environment [1], while tactile skin sensors mounted on the robot offer local measurements of contact forces [2], [3]. However, these approaches often suffer from practical limitations: external sensors cannot always prevent fast or unpredictable contacts, and tactile skins are costly and difficult to integrate reliably. As a result, sensorless methods based on proprioceptive measurements, such as joint torque sensors and encoders, have gained increasing attention. Among these, residual-based methods that monitor robot generalized momentum have proven effective in detecting collisions, identifying the contact link, and estimating the external force [4], [5]. For this, the residual method has been used in [6] in combination with a F/T sensor mounted at the robot base. A contact particle filter (CPF) was introduced in [7] as a probabilistic method to estimate contact location and force in simulation on an ATLAS humanoid, even in the presence of noisy joint torque measurements, while experiments were performed in [8] on a KUKA LBR iiwa. Finally, the residual method based on pseudoinversion was combined with CPF on a 6-DOF NEURA robot in [9], showing improved accuracy in contact handling at the end-effector level only. In this paper, this approach is extended to a 7-DOF robot equipped with joint encoders and torque sensors. We show its effectiveness in addressing contact force localization and estimation also in the case of proximal robot links (i < 6), where the associated contact link Jacobian loses rank, leading to failure of pseudoinversion. We validate the method in simulation and on a KUKA LWR4+ robot, showing consistent localization of the contact point and accurate reconstruction of a pure external force for contacts on link 6 (full-rank case) and link 3 (rank-deficient Jacobian). II. METHODOLOGY A. Momentum-Based Contact Detection For a robot with njoints, contact detection (and link isolation) relies on monitoring the residual derived from the robot generalized momentum ˆ p=ˆ M(q)˙ q r(t) = K0ˆ p(t)−Zt 0 (τm−ˆ β(q,˙ q)+r)ds −ˆ p(0),(1) where [5] ˆ β(q,˙ q) = ˆ g(q)−ˆ CT(q,˙ q)˙ q+ˆ τfric (2) with gain matrix K0=diag{k0,i}>0. The residual r∈Rn approximates the external joint torque τext ∈Rninduced by an external Cartesian wrench wext ∈R6for a sufficiently large gain: KO→ ∞,r≈τext =JT c(q)wext,(3) where Jc(q)is the 6×ngeometric Jacobian of the contact location on a generic link. 2025 I-RIM Conference October 17-19, Rome, Italy ISBN: 9788894580570 10.5281/zenodo.17629708 87 B. Pseudoinversion Method Consider a pure contact force fext ∈R3applied to a generic link iof the robot at an unknown contact point pc,i ∈R3. This generates an external joint torque τext = (JL c,i)T(q)fext, where JL c,i(q)is the 3×nlinear part of the (unknown) geometric Jacobian Jc,i associated to pc,i. Correspondingly, the force fi∈R3and moment mi∈R3 acting on the local (known) frame of the i-th contact link are computed from the pseudoinverse of the transpose of the corresponding 6×nlink Jacobian Ji(q), fi mi=JT i(q)#r,(4) since r≈τext. By the assumption of a pure contact force (mext =0), which is typical in contact and collision scenarios, the force/moment equilibrium is written as fi mi=I S(pc,i)fext,(5) with the skew-symmetric operator S(·). From (5), the external force is directly estimated as ˆ fext =fi, while the contact point is localized as ˆ pc,d =−S(fext)#mi.(6) Here, the pseudoinverse is needed since S(fext)has rank 2. Finally, ˆ pc,d is projected onto the surface mesh of the i-th robot link to ensure physical consistency, by finding the closest point along the line of action of fext: ˆ p(λ) = ˆ pc,d +λˆ fext    ˆ fext   , λ ∈R.(7) C. Contact Particle Filter The CPF [7] is a Bayesian, non-parametric method that estimates the contact location of an external force using only proprioceptive sensing. The state of this filter at time t≥0is represented by a set of Mweighted particles: χt={x[1] t, x[2] t, . . . , x[M] t},(8) where x[m] tdenotes the m-th candidate contact point and w[m] t its likelihood weight. a) Initialization: When a collision is detected, particles are sampled around the estimate (7), ensuring concentration near the true contact location and faster convergence, even in rank-deficient cases. This initialization typically guarantees convergence to a single contact point. b) Motion model: Particles evolve according to the random-walk propagation [7]: p(xt|xt−1)∝ N(xt;xt−1,Σmotion).(9) This model covers slowly varying contacts, with the covariance Σmotion controlling particle spread. After propagation, particles are projected back on the link surface for physical consistency. c) Observation model: For each particle x[m] t, the likelihood is obtained by comparing the residual r(t)with the joint torque induced by a candidate contact. The cost function His Hr(t)|x[m] t= r(t)−JL c(q, x[m] t)T f[m] Σ−1 meas ,(10) with the external force candidate evaluated as [9] f[m]=JL c(q, x[m] t)# r.(11) The particle weights are updated as w[m] t∝exp−1 2H(r(t)|x[m] t),(12) namely with heavier weights associated to lower costs H. d) Resampling: To avoid degeneracy, resampling favors highly weighted particles, concentrating the distribution around likely contacts. The final contact point estimate ˆ pc,i is obtained as the weighted average over the particle set. The external force estimate ˆ fext is computed similarly as weighted average of the forces (11) associated with all particles. III. EXPERIMENTS The experiments were carried out on a KUKA LWR4+ manipulator in our DIAG Laboratory. This 7R robot executes the following trajectory q(t)=q0+ 0.11111111T(1 −cos(t)) (13) for T= 12.6s, starting from q0= (0, π, 0, π/2,0, π, π/4). Two scenarios were analyzed: contact at link 6 (full-rank Jacobian case) and at link 3 (rank-deficient Jacobian case). Fig. 1. Meshes of link 3 of the KUKA LWR4+ robot. To define the ground truth of the contact location, the meshes of the robot links (see, e.g., Fig. 1) were used to select specific points where the external contact force was manually applied. Since no external force sensor was available, the force estimation was evaluated by comparing the results obtained from the two approaches in Sect. II. 88 1) Contact at link 6: Both the pseudoinversion and the CPF methods accurately reconstruct the contact point, as shown in Figs. 2–3, with CPF providing a reduction in the estimation error down to a final value ∥eCP F ∥= 0.7cm. The contact point is fixed (and expressed) in the local frame moving with the link. A visual comparison between actual and estimated contact locations is illustrated in Fig. 4. Regarding force estimation, both methods provided highly consistent results, with overlapping profiles and magnitudes, see Figs. 5–6. This agreement confirms the reliability of the force estimates in the absence of ground-truth measurements. Time [s] 0 2 4 6 8 10 12 Component error [m] -0.02 -0.01 0 0.01 0.02 0.03 ex;pinv ey;pinv ez;pinv Fig. 2. Components of the position error when estimating the contact point pc= (−0.043,−0.0495,−0.023) [m] at link 6 using only the residual method with pseudoinversion. Time [s] 0 2 4 6 8 10 12 Component error [m] -0.02 0 0.02 0.04 ex;CPF ey;CPF ez;CPF Fig. 3. Components of the position error when estimating the contact point pc= (−0.043,−0.0495,−0.023) [m] at link 6 using also the contact particle filter. 2) Contact at link 3: A contact force applied to link 3 represents the critical case, due to the rank deficiency of the corresponding Jacobian. In this configuration, the pseudoinversion method fails to provide a reliable reconstruction, producing large errors in the estimation of the contact location (Fig. 7) and inconsistent force profiles (Fig. 9). In contrast, the CPF successfully compensates for Jacobian singularities, yielding accurate reconstructions of the contact point (Fig. 8), with a final value ∥eCP F ∥= 5.2cm, and force estimates (Fig. 10). A comparison between the actual contact point and the estimates obtained with both methods is shown in Fig. 11. IV. CONCLUSIONS We have combined the classical momentum-based residual method using pseudoinversion with a contact particle filter for contact detection, link isolation, and contact point localization on a 7-DOF manipulator. The key novelty lies in initializing the filter around the solution provided by the pseudoinversion method, which guides the particles toward the correct contact region and improves convergence. The approach was validated in experiments, considering contacts at link 6 and at link 3 of a Fig. 4. Visualization of actual contact point (●) at link 6 and its estimate using only the residual method with pseudoinversion (●) or also the contact particle filter (●). 0 2 4 6 8 10 12 -30 -20 -10 0 10 20 Fig. 5. Estimation of external force ˆ fext at link 6 using only the residual method with pseudoinversion. 0 2 4 6 8 10 12 -30 -20 -10 0 10 20 Fig. 6. Estimation of external force ˆ fext at link 6 using also the contact particle filter. KUKA LWR4+ robot with elastic joints. In the full-rank case (link 6), pseudoinversion alone provides accurate estimates of the contact point and force, with the CPF further refining the results. In the rank-deficient case (link 3), pseudoinversion alone fails to produce reliable estimates, while the combined pseudoinversion–CPF approach successfully reconstructs both contact force and location on the link with high accuracy. These results were obtained relying solely on proprioceptive measurements of on-board joint torque sensors and encoders, without the help of any external sensing device. The same 89 Time [s] 0 2 4 6 8 10 12 Component error [m] -0.1 -0.05 0 0.05 0.1 0.15 0.2 ex;pinv ey;pinv ez;pinv Fig. 7. Components of the position error when estimating the contact point pc= (−0.032,−0.052,0.061) [m] at link 3 using only the residual method with pseudoinversion. The yand zcomponents are both clearly off. Time [s] 0 2 4 6 8 10 12 Component error [m] -0.04 -0.02 0 0.02 0.04 ex;CPF ey;CPF ez;CPF Fig. 8. Components of the position error when estimating the contact point pc= (−0.032,−0.052,0.061) [m] at link 3 using also the contact particle filter. 0 2 4 6 8 10 12 -6 -4 -2 0 2 Fig. 9. Estimation of external force ˆ fext at link 3 using only the residual method with pseudoinversion. All components are far below the expected level of applied force. 0 2 4 6 8 10 12 -60 -40 -20 0 20 Fig. 10. Estimation of external force ˆ fext at link 3 using also the contact particle filter. approach can be applied also to robots with rigid joints, using then only motor currents and joint position measurements. ACKNOWLEDGEMENTS Financial support from PNRR MUR project PE0000013FAIR is kindly acknowledged. A video illustrating the experimental results accompanies the paper and is also available at https://youtu.be/D6u1rY6Cjtw on the YouTube channel of the DIAG Robotics Laboratory. Fig. 11. Visualization of actual contact point (●) at link 3 and its estimate using only the residual method with pseudoinversion (●) or also the contact particle filter (●). REFERENCES [1] F. Flacco, T. Kr¨ oger, A. De Luca, and O. Khatib, “A depth space approach to human-robot collision avoidance,” in Proc. IEEE Int. Conf. on Robotics and Automation, pp. 338–345, 2012. [2] R. Dahiya, P. Mittendorfer, M. Valle, G. Cheng, and V. Lumelsky, “Directions toward effective utilization of tactile skin: A review,” IEEE Sensors J., vol. 13, no. 11, pp. 4121–4138, 2013. [3] A. Cirillo, F. Ficuciello, C. Natale, S. Pirozzi, and L. Villani, “A conformable force/tactile skin for physical human-robot interaction,” IEEE Robotics and Automation Lett., vol. 1, no. 1, pp. 41–48, 2016. [4] A. De Luca and R. Mattone, “Sensorless robot collision detection and hybrid force/motion control,” in Proc. IEEE Int. Conf. on Robotics and Automation, pp. 999–1004, 2005. [5] S. Haddadin, A. De Luca, and A. Albu-Sch¨ affer, “Robot collisions: A survey on detection, isolation, and identification,” IEEE Trans. on Robotics, vol. 33, no. 6, pp. 1292–1312, 2017. [6] G. Buondonno and A. De Luca, “Combining real and virtual sensors for measuring interaction forces and moments acting on a robot,” in Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 794–800, 2016. [7] L. Manuelli and R. Tedrake, “Localizing external contact using proprioceptive sensors: The Contact Particle Filter,” in Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 5062–5069, 2016. [8] J. Bimbo, C. Pacchierotti, N. G. Tsagarakis, and D. Prattichizzo, “Collision detection and isolation on a robot using joint torque sensing,” in Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp. 7604– 7609, 2019. [9] D. Zurlo, T. Heitmann, M. Morlock, and A. De Luca, “Collision detection and contact point estimation using virtual joint torque sensing applied to a cobot,” in Proc. IEEE Int. Conf. on Robotics and Automation, pp. 7533–7539, 2023. 90