Source-linked AI summary
3D-Force Estimation for Dual-arm Manipulation of Deformable Linear Objects via Factor Graphs
Annika Delucchi, Vincenzo Di Paola, Bruno Marco Quadrelli, Matteo Zoppi
TL;DR
Underactuation makes estimating contact forces on deformable linear objects challenging, while reliable contact estimates matter for their manipulation. The paper uses factor graphs to estimate 3D force and rod shape from visual marker positions alone. Simulations report recovery of force, contact location, and shape with minimal error, including under non-negligible torsion.
Problem
Method
Results
The method recovered contact force, location, and DLO shape with minimal error using marker positions alone, even with non-negligible torsion.
Source: §V · p. 9Takeaways & Limitations
In quasi-static symmetric pull and asymmetric push simulations, the method estimated 3D contact forces without prior knowledge of their magnitude, direction, or location.
Source: §V · p. 9Experimental validation remains future work.
Source: §V · p. 9
Abstract
from arXiv · showhide
Estimating forces acting on Deformable Linear Objects (DLOs) is challenging due to their underactuated nature. However, reliable contact estimation is crucial to precisely control them and enable safe manipulation tasks. Differently from the various approaches presented in the literature, this work adopts a probabilistic formulation where the DLO state and sensor data are related through conditional distributions. In particular, factor graphs are used to determine the magnitude, direction, and location of a 3D force acting on a DLO, while simultaneously reconstructing its shape. Since DLOs are not actuated and are typically thin, like cables and wires, strain gauges and actuation readings are usually impractical. Therefore, to overcome this issue, this work is tested using one type of sensing information only, thus developing a generic technique suited for a wider variety of tasks. Realistic tests are performed by introducing noise into the measurements while the DLO follows a prescribed path in the presence of external forces that are accurately estimated.
I. INTRODUCTION
Underactuation makes contact estimation and control of deformable linear objects complex, despite their usefulness in contact-rich manipulation. The paper uses factor graphs to estimate 3D contact forces from discrete spatial measurements and models the rod with Kirchhoff inextensible theory.
I. INTRODUCTION
- Underactuation makes contact estimation and control of deformable linear objects highly complex, limiting their use in contact-rich manipulation.The introduction identifies threading, knotting, and routing as recurring tasks involving ropes and cables.Source: §I · p. 1
- Factor graphs estimate the location, direction, and magnitude of 3D forces from discrete positions along the rod while reconstructing its shape.The sensing setup uses camera-tracked positions only, rather than combining multiple measurement types.Source: §I · p. 1
II. KIRCHHOFF INEXTENSIBLE MODEL
- The Kirchhoff inextensible model represents rod cross-sections by poses along arc length and fixes linear strain to enforce constant elongation and no shear.For homogeneous material, the stiffness matrix is constant along the rod.Source: §II · p. 2
- Rod statics relate strain-dependent stress to distributed loads through an equilibrium equation.The section introduces the constitutive relation between strain and stress and the Euler–Poincaré static equilibrium equation.Source: §II · p. 2
A. Local Geometric Variable Strain
The rod model approximates angular strain with basis functions and solves for the shape and loads under dual-arm boundary conditions. Local linear Lagrange bases are used to represent concentrated forces.
A. Local Geometric Variable Strain
- The strain approximation uses a selection matrix and shape functions with generalized coordinates, while the vector e1 enforces inextensibility and no shear.The kinematics are forward integrated, then stress resultants and generalized external loads are obtained by backward integration.Source: A. Local Geometric Variable Strain · p. 2
- The static equilibrium constraint balances generalized stiffness and external loads, including a concentrated contact wrench and the end wrench.The generalized stiffness matrix Ke has dimensions compatible with q.Source: A. Local Geometric Variable Strain · p. 2
- Dual-arm manipulation is formulated as a boundary-value problem with the rod’s end poses fixed to the relative pose imposed by the manipulators.The boundary conditions are T(0) = I and T(1) = Tdes.Source: A. Local Geometric Variable Strain · p. 2
- Local linear Lagrange polynomial bases are used because local bases represent concentrated forces better than global bases.The paper notes this advantage while allowing that different polynomial bases are possible.Source: A. Local Geometric Variable Strain · p. 2
III. FACTOR GRAPHS AND FORCE ESTIMATION
Factor graphs express estimation as inference over state variables connected by factors that encode system information. With Gaussian potentials, maximum a posteriori estimation becomes nonlinear least squares.
III. FACTOR GRAPHS AND FORCE ESTIMATION
- A factor graph connects state variables to factors that encode system information through associated densities and cost terms.The graph is bipartite, with variable nodes and factor nodes linked by edges.Source: §III · p. 2
- Gaussian noise models yield Gaussian potentials, with the squared Mahalanobis distance measuring deviation from the mean.The covariance matrix specifies the Gaussian distribution’s uncertainty.Source: §III · p. 3
- The joint density is a product of factors, and maximizing that product gives the maximum a posteriori estimate.For Gaussian potentials, taking the negative logarithm converts the factor product into a sum of squared residuals.Source: §III · p. 3
A. Rod Discretization
The rod is discretized into nodes whose poses and internal wrenches define its estimated shape. Contact forces are represented by Gaussian basis functions, with parameters for force location and magnitude.
A. Rod Discretization
- The rod’s continuous behavior is discretized into N nodes, with piecewise-constant strain over each segment and a modeling-noise term for discretization and linearization errors.Each node has an associated pose and internal wrench that determine the reconstructed shape.Source: A. Rod Discretization · p. 3
- Unknown contact forces enter the discretized equilibrium as concentrated loads represented by Gaussian basis functions.The representation assumes no external moments and uses as many Gaussian bases as external forces.Source: A. Rod Discretization · p. 3
- Force estimation includes the Gaussian mean for contact location and amplitude for force magnitude; the location parameter is bounded to the rod domain.A fixed Gaussian width can represent point loads, and β_h maps the location into [0, L].Source: A. Rod Discretization · p. 3
- The discretized equilibrium includes gravity and external-force terms, with a moment-approximation error term.The intervals between neighboring nodes have length Δs.Source: A. Rod Discretization · p. 3
B. Problem Definition
The problem is to estimate a 3D external force while reconstructing the rod shape, using a discretized factor graph that incorporates the unknown force into equilibrium. The graph combines rod-state variables with tip priors, mechanics, and available shape measurements.
B. Problem Definition
- The rod is discretized into N equally spaced nodes and N −1 segments, with tip nodes carrying pose and internal-wrench unknowns.Known positions at N −2 interior points provide shape data.Source: B. Problem Definition · pp. 3–4
- The graph estimates an unknown external force and rod shape together, linking the force variable to the rod state through equilibrium factors.Rod-state variables contain pose and internal wrench; the unknown force acts at an unknown location.Source: B. Problem Definition · p. 3, Figure 2 · p. 4
- The factor-graph diagram distinguishes prior, equilibrium, kinematics, and measurement factors, alongside rod-state and force variables.Factor nodes are squares; variable nodes are circles.Source: Figure 2 · p. 4
C. Variable and Factors Nodes
The rod state is represented by variable nodes connected to factors that encode physical grounding and sensor observations.
C. Variable and Factors Nodes
- Variable nodes encode the rod state, while factor nodes provide physical grounding and sensor observations.Each factor connects only to the variable nodes on which it depends.Source: C. Variable and Factors Nodes · p. 4
1) Prior Factors:
Prior factors constrain the rod’s tip poses, while mechanics factors enforce kinematic consistency and equilibrium between adjacent nodes.
1) Prior Factors:
- Prior factors enforce the known poses of the rod’s base and tip at the first and last nodes.The same covariance is used for both priors.Source: 1) Prior Factors · p. 4
2) Mechanics Factors:
- Mechanics factors enforce kinematics using constant strain over each interval and impose wrench balance between adjacent nodes.Equilibrium includes the external force term.Source: 2) Mechanics Factors · p. 4
3) Measurements Factors:
Measurements use discrete positions along the rod, and the cost function collects contributions from the graph’s priors, mechanics, and available measurements.
3) Measurements Factors:
- The measurement factors use known positions at N −2 points along the rod, assumed available from camera tracking.The passage identifies these positions as node position vectors and refers to sensor uncertainty.Source: 3) Measurements Factors · p. 4
D. Cost Function
- The cost function includes prior, kinematics, equilibrium, and available measurement factors.The force direction is represented by azimuth α and elevation ε after expressing the force in the world frame.Source: D. Cost Function · p. 4, Figure 3 · p. 5
IV. SIMULATIONS
The simulations test force and shape estimation using noisy marker positions on two trajectories designed to excite bending and torsion. The setup uses one sensing modality and restricts force magnitude and contact location to practical ranges.
IV. SIMULATIONS
- The test object is a 1 m Nitinol rod represented by 20 nodes, with a 1.5 × 10−3 m circular cross-section diameter.Its Young’s modulus is 50 GPa and shear modulus is 30 GPa.Source: §IV · p. 5
A. Statistical Analysis
- Two trajectories test force estimation under bending and torsion, using deflection information alone and no sensor fusion.The authors contrast this single-sensor setup with cited approaches that assume force location or constrain force orientation.Source: A. Statistical Analysis · p. 5
- Direct literature comparison is limited because continuum manipulators have stiffer structures than DLOs.The force parameters are demonstrative and do not limit the approach’s generality.Source: A. Statistical Analysis · p. 5
1) Dataset Generation:
- The dataset uses nine equally spaced rod positions and tip-pose priors, with additive Gaussian noise of σ = 1 × 10−4 m on marker measurements.Measurements emulate markers spaced 0.1 m apart; the noise corresponds to a maximum 3D offset of 5 × 10−4 m.Source: 1) Dataset Generation · p. 5, 2) Measurements Noise · p. 5
3) Force Parameters Range:
- Forces are applied at s ∈ [0.3, 0.7], with maxima of 1.5 N for non-axial components and 0.3 N for the axial component.The force limits can be adapted to the task and DLO material and geometry.Source: 3) Force Parameters Range · p. 5
4) Initialization:
The estimator starts from a straight-rod, near-zero-force guess and uses selected factor covariances. Evaluation reports force magnitude, direction, and contact-location accuracy.
4) Initialization:
- Initialization assumes a straight rod and near-zero force, with the initial contact location at the rod’s midpoint.The force Gaussian standard deviation is set to 1.2 × 10−2 m to represent concentrated forces.Source: 4) Initialization · p. 6
5) Covariances:
- Factor covariances are selected for the estimator, with diagonal isotropic noise models assuming uncorrelated components and no preferred direction.The force covariance is Σf = diag(2, 2, 2, 3) [N,-].Source: 5) Covariances · p. 6
6) Evaluation Metrics:
- Evaluation measures force components, direction, and contact location, using azimuth and elevation angles for direction and arc length for location.The force components are expressed in the world frame; direction angles are reported in degrees.Source: 6) Evaluation Metrics · p. 6
B. Trajectory Simulation
Across pull and push trajectories, the method estimates contact forces and reconstructs rod shape from noisy marker positions, including in the torsion-bearing push case. The evaluation is quasi-static and simulation-based; experimental validation remains future work.
B. Trajectory Simulation
- The quasi-static tests use pull and push trajectories, with force and tip-pose updates available only at discrete steps.Pull deforms an initial U-shape toward a V-shape; push flattens it toward an M-shape, with maximum update steps of 0.05 N, 62 mm, and 6°.Source: B. Trajectory Simulation · p. 6
- The estimator reconstructs force magnitude, location, and orientation with high fidelity under severe deformation, while curvature peaks localize force application.The figures show pull at s = 0.5 and push at s = 0.6, with estimated node poses and forces displayed in red.Source: Figure 4 · p. 7, Figure 5 · p. 8, B. Trajectory Simulation · p. 9
- Mean contact-location errors were 2.4 × 10−2 m for pull and 2.2 × 10−2 m for push.Mean errors for pull force components fx, fy, fz were 1.5 × 10−2 N, 8 × 10−3 N, and 2.2 × 10−2 N; for push they were 1.7 × 10−2 N, 1.5 × 10−2 N, and 1 × 10−2 N, respectively. Mean direction errors were 1.5° and 0.8° for pull, and 2.4° and 1° for push, in azimuth and elevation.Source: B. Trajectory Simulation · p. 9
V. CONCLUSION
- The two tasks estimate 3D contact force without prior knowledge of its magnitude, direction, or location, using marker positions corrupted by σ = 1 × 10−4 m noise.The conclusion reports recovery of force, location, and DLO shape even with non-negligible torsion, relying on one visual sensor type.Source: §V · p. 9
- Experimental validation and planning strategies are identified as future work.The stated future direction is to move toward safe control and guidance.Source: §V · p. 9