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

Tensegrity Robot Endcap-Ground Contact Estimation with Symmetry-aware Heterogeneous Graph Neural Network
Mar 3, 2026Wenzhe Tong, Yicheng Jiang, Chi Zhang +2Tensegrity Robotics Robotics
Robotics Department, University of Michigan, Ann Arbor, MI 48109 USA
Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs
May 28, 2026Avinash Subramanian, Connor Holmes, Timothy D. Barfoot +2Semidefinite Programming Factor Graph Optimization
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
Scalable Open-Source Visuotactile Sensor for 6-Axis Contact Wrench Estimation in Tensegrity Robots
Jul 17, 2026Wenzhe Tong, Jonathan Mi, Xili Yi +2Vision-Based Tactile Sensing Tensegrity Robotics
Robotics Department, University of Michigan, Ann Arbor, MI, 48109, USA

Mar 3, 2026J/K move · Enter open · S save

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

Wenzhe Tong, Yicheng Jiang, Chi Zhang, Maani Ghaffari+1

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

Tensegrity Robotics Robotics Robot State Estimation Heterogeneous GNNs Spatiotemporal GNNs

Open paper AskPDF arXiv

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

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