State and Trajectory Estimation of Tensegrity Robots via Factor Graphs and Chebyshev Polynomials

Edgar Granados; Patrick Meng; Charles Tang; Shrimed Sangani; William R. Johnson; Rebecca Kramer-Bottiglio; Kostas Bekris

cs.ROApr 9, 2026

State and Trajectory Estimation of Tensegrity Robots via Factor Graphs and Chebyshev Polynomials

Authors: Edgar Granados, Patrick Meng, Charles Tang, Shrimed Sangani, William R. Johnson, Rebecca Kramer-Bottiglio, Kostas Bekris

Organizations: Dept. of Computer Science, Rutgers University, NJ, USA. · Mech. Eng. & Material Sc. Yale University, New Haven, CT, USA.

Abstract

Tensegrity robots offer compliance and adaptability, but their nonlinear, and underconstrained dynamics make state estimation challenging. Reliable continuous-time estimation of all rigid links is crucial for closed-loop control, system identification, and machine learning; however, conventional methods often fall short. This paper proposes a two-stage approach for robust state or trajectory estimation (i.e., filtering or smoothing) of a cable-driven tensegrity robot. For online state estimation, this work introduces a factor-graph-based method, which fuses measurements from an RGB-D camera with on-board cable length sensors. To the best of the authors' knowledge, this is the first application of factor graphs in this domain. Factor graphs are a natural choice, as they exploit the robot's structural properties and provide effective sensor fusion solutions capable of handling nonlinearities in practice. Both the Mahalanobis distance-based clustering algorithm, used to handle noise, and the Chebyshev polynomial method, used to estimate the most probable velocities and intermediate states, are shown to perform well on simulated and real-world data, compared to an ICP-based algorithm. Results show that the approach provides high fidelity, continuous-time state and trajectory estimates for complex tensegrity robot motions.

Figures & tables

Fig. 1: Tensegrity estimation: i) Point cloud from RGB-D sensor with self-occlusions. ii) Clustering the points by colors (for visualization, white points show black clusters). iii) Semi-transparent bars show the estimated pose at the example frame, and lines the estimated trajectory via Chebyshev polynomials.

Fig. 2: The tensegrity robot structure: i) A bar’s local frame (the blue bar’s frame is shown) is defined on the middle of the bar with endcaps along the $Z$ -axis. Each bar has 2 colored endcaps, labeled $A$ or $B$ . Endcaps are connected by a total of 9 sensors (6 short and 3 long), shown as color lines. ii) Side view diagram of the robot, where endcaps $A$ are behind endcaps $B$ . Color lines show the elastic sensors and black lines show the rigid black bars.

Fig. 3: Estimation process: from left to right, images are converted to a point cloud. Points are clustered by color and a factor graph estimates the robot’s state. When a trajectory finishes, an offline process computes the most likely trajectory using Chebyshev polynomials.

Factor	Variables	Definition	Condition to add to FG
${\scriptstyle f^{bar}}$	${\scriptstyle X^{R}_{i},X^{G}_{i},X^{B}_{i}}$	${\scriptstyle min(h^{bar}(X^{G}_{i})=\parallel M_{2,3}(X^{G}_{i})^{-1}z^{b}_{i}\parallel,h^{bar}(X^{R}_{i}),h^{bar}(X^{B}_{i}))}$	Black point $z^{b}_{i}$ observed
${\scriptstyle f^{between}}$	${\scriptstyle X^{j}_{i},X^{j}_{i-1}}$	${\scriptstyle(X^{j}_{i})^{-1}\circ X^{j}_{i-1}}$	Previous estimation available
${\scriptstyle f^{endcap}_{(\cdot)}}$	${\scriptstyle X_{i},R^{(\cdot)}_{i}}$	${\scriptstyle h^{endcap}(X_{i},R^{(\cdot)}_{i})-z^{e}_{i}=X_{i}(R^{(\cdot)}_{i}p^{off})-z^{e}_{i}}$	Endcap $z^{e}_{i}$ observed
${\scriptstyle f^{cable}}$	${\scriptstyle X^{j}_{i},X^{k}_{i},R^{j(\cdot)}_{i},R^{k(\cdot)}_{i}}$	${\scriptstyle\parallel h^{endcap}(X^{j}_{i},R^{j(\cdot)}_{i})-h^{endcap}(X^{k}_{i},R^{k(\cdot)}_{i})\parallel-z^{c}_{i}}$	Cable $z^{c}_{i}$ observed
${\scriptstyle f^{R0}}$	${\scriptstyle R^{jA}_{i},R^{jB}_{i}}$	${\scriptstyle(R^{jA}_{i}*R^{jB}_{i})\ominus R^{off}}$	$f^{endcap}_{(\cdot)}$ or $f^{cable}$ is added
${\scriptstyle f^{R1}}$	${\scriptstyle R^{jB}_{i},R^{jA}_{i}}$	${\scriptstyle(R^{jB}_{i}*R^{jA}_{i})\ominus R^{off}}$	$f^{endcap}_{(\cdot)}$ or $f^{cable}$ is added

TABLE I: Factors and variables used for online state estimation of the Tensegrity Robot.

Fig. 4: State estimation factor graph: each variable $X_{i}^{k}\in SE(3)$ ( $k\in\{R,G,B\}$ ) is a bar pose, and factors (dark discs) relate measurements to variables. Fig. 5 expands the subgraph between $f^{cable}$ , $f^{endcap}$ , and related variables.

Fig. 5: Subgraph relating a cable meassurement between bar endcap $A$ of bar $j$ and endcap $B$ of bar $k$ . Factors $f^{R_{0}},f^{R_{1}}$ , and $f^{R_{2}}$ are needed for the factor graph to assign the unlabeled endcap observations correctly. The full factor graph may contain up to 9 $f^{cable}$ and 6 $f^{endcap}$ factors, with the total variables remaining constant.

Fig. 6: Ablation study of Chebyshev polynomials on 25 trajectories of trajectories collected from a simulator at 200Hz. Observations are at 30Hz, 20Hz, 10Hz and 5Hz. An endcap or cable observation $z^{(\cdot)}_{i}$ is generated by adding gaussian noise to the ground-truth value $p^{(\cdot)}_{i}$ i.e.: $z^{(\cdot)}_{i}=p^{(\cdot)}_{i}+N(0,\sigma^{(\cdot)})$ . Endcap miss represents the probability of an endcap not being observed, representing occlusions. Each polynomial is evaluated against the ground-truth state $X^{GT}_{i}$ at the full resolution of the data, obtaining the error per state as $\texttt{error}_{i}=|X^{GT}_{i}\ominus({\bf X\cdot w_{i}})|$ . Colors represent the sum of the elements of the average error vector except for the rotation on the Z-axis.

	Original Dataset (Long)			Original Dataset (Short)			New Dataset
	Center of Mass Error	Translation Error	Rotation Error	Center of Mass Error	Translation Error	Rotation Error	Center of Mass Error	Translation Error	Rotation Error
ICP	0.02837	0.03046	0.14599	0.00850	0.01170	0.10149	0.02241	0.02885	0.52331
Factor Graph	0.02965	0.03198	0.34468	0.02857	0.03116	0.37081	0.02559	0.02484	0.36583
Chebychev	0.04493	0.04805	0.31728	0.03419	0.03743	0.41592	0.03587	0.03959	0.41473

TABLE II: Experimental results on trajectories for real tensegrity robots (error measured in meter)

Fig. 7: i) Estimated state (white) on original dataset, ground-truth (color) poses obtained from mocaps. ii) Chebyshev polynomial on the new dataset, points show all estimated endcaps.

Tensegrity Robotics

See all 8 papers

Mar 3, 2026

Tensegrity Robot Endcap-Ground Contact Estimation with Symmetry-aware Heterogeneous Graph Neural Network

Tensegrity robots possess lightweight and resilient structures but present significant challenges for state estimation due to compliant and distributed ground contacts. This paper introduces a symmetry-aware heterogeneous graph neural network (Sym-HGNN) that infers contact states directly from proprioceptive measurements, including IMU and cable-length histories, without dedicated contact sensors. The network incorporates the robot's dihedral symmetry

D_3

into the message-passing process to enhance sample efficiency and generalization. The predicted contacts are integrated into a state-of-the-art contact-aided invariant extended Kalman filter (InEKF) for improved pose estimation. Simulation results demonstrate that the proposed method achieves up to 15% higher accuracy and 5% higher F1-score using only 20% of the training data compared to the CNN and MI-HGNN baselines, while maintaining low-drift and physically consistent state estimation results comparable to ground truth contacts. This work highlights the potential of fully proprioceptive sensing for accurate and robust state estimation in tensegrity robots. Code available at: https://github.com/Jonathan-Twz/Tensegrity-Sym-HGNN

Wenzhe Tong, Yicheng Jiang, Chi Zhang +2

Robotics Department, University of Michigan, Ann Arbor, MI 48109 USA

Tensegrity Robotics Robotics Robot State Estimation Heterogeneous GNNs Spatiotemporal GNNs

Ask PDF

May 28, 2026

Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs

Robust and efficient state estimation is crucial for perception, navigation, and control in robotics. State estimation problems are conveniently modeled using the factor-graph framework as enabled by modern software packages such as GTSAM or g2o. However, the standard solvers included in such frameworks are local and may converge to poor local minima, posing significant safety concerns. Conversely, techniques based on convex relaxations have been shown to provide a means of globally solving or certifying many state estimation problems. However, these relaxations 1) often require substantial effort to formulate, and 2) may incur significantly higher cost compared to efficient local solvers, as they require solving a large semidefinite program (SDP). In this work, we address both shortcomings by 1) creating a new procedure within the GTSAM framework for automatically constructing convex SDP relaxations for any factor graphs with common factor and variable types, and by 2) exploiting the Bayes tree constructions native to GTSAM to decompose the SDP problem, leading to significant speedup in solver time for chordally sparse problems. We demonstrate the favorable scaling of this structure-exploiting global estimator compared to standard local solvers for two case studies: A 3D pose-graph SLAM problem with a ring factor graph and a 2D localization problem with a chain factor graph. The software framework is available at https://github.com/borglab/gtsam.

Avinash Subramanian, Connor Holmes, Timothy D. Barfoot +2

College of Computing, Georgia Institute of Technology, Atlanta, GA, USA · Robotics Institute, University of Toronto, Toronto, ON, Canada · Department of Mechanical Engineering, Carnegie Mellon University, Pittsburgh, PA, USA

Semidefinite Programming Factor Graph Optimization Constrained Optimization

Ask PDF

Jul 17, 2026

Scalable Open-Source Visuotactile Sensor for 6-Axis Contact Wrench Estimation in Tensegrity Robots

This paper presents a scalable, open-source visuotactile sensing system for tensegrity robots that enables six-axis wrench estimation and contact detection. The proposed endcap sensor integrates an elastomeric shell, a 3D-printed thermoplastic polyurethane (TPU) interface, and a rigid base housing an embedded camera and LED illumination ring. A novel gyroid-infill bonding technique is introduced to form a durable elastomer-TPU interface without adhesives, yielding a lightweight and modular design compatible with large-scale tensegrity structures. A tactile-to-wrench neural network maps shear vector fields to six-dimensional force and torque measurements. Experimental results demonstrate accurate and stable wrench estimation with a mean squared error (MSE) of 0.1531 on static validation data and out-of-domain generalization under dynamic motion. Furthermore, full-system integration on a 12 kg tensegrity robot confirms the sensor's ability to reliably identify ground contacts. The system substantially improves the practicality of tactile feedback for tensegrity robots, offering a low-cost, reproducible, and physically interpretable pathway toward contact-aware proprioception and state estimation. Open source files are available at github.com/Jonathan-Twz/tensegrity-gelfoot

Wenzhe Tong, Jonathan Mi, Xili Yi +2

Robotics Department, University of Michigan, Ann Arbor, MI, 48109, USA

Vision-Based Tactile Sensing Tensegrity Robotics Robotics Robotic Tactile Sensing Contact Force Estimation

Ask PDF

Abstract

Tensegrity Robotics

Explore similar work

Tensegrity Robot Endcap-Ground Contact Estimation with Symmetry-aware Heterogeneous Graph Neural Network

Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs

Scalable Open-Source Visuotactile Sensor for 6-Axis Contact Wrench Estimation in Tensegrity Robots