Technical Summary: THREAD – Trajectory Planning for Hybrid Rigid-Soft Manipulators with Environment-Aware Diffusion

1. Problem Statement

Robots operating in confined, unstructured environments (e.g., disaster response, subterranean exploration) face significant challenges when navigating narrow apertures. While hybrid rigid-soft manipulators combine the reach of rigid arms with the compliance of soft continuum arms (SCAs) to access tight spaces, they introduce unique planning difficulties:

• Environmental Infeasibility: Backbone shapes that are kinematically valid in free space often become physically infeasible when constrained by environmental contact, leading to collisions or strain limit violations.
• Kinematic Coupling: Planning rigid and soft segments independently ignores their kinematic coupling. A poorly positioned rigid arm can force the soft segment into configurations that violate physical constraints, rendering the task impossible.
• Limitations of Existing Methods: Classical sampling-based planners lack closed-loop replanning and generalization. Supervised learning and Reinforcement Learning (RL) approaches often focus on end-effector control, lacking global backbone reasoning or explicit geometric collision awareness. Furthermore, existing diffusion models have been applied to rigid joint spaces but not to the continuous backbone shape domain of continuum manipulators.

2. Methodology: The THREAD Framework

The authors propose THREAD, a diffusion-based trajectory planner that operates in the continuous backbone shape space rather than joint or end-effector space. The system integrates perception, generative planning, and closed-loop control.

A. Unified Backbone Representation

The entire kinematic chain (rigid arm + soft continuum arm) is encoded as a single backbone of $K=64$ points sampled from the base to the tip.

• Rigid-Soft Mask: A parameterized mask divides the backbone into rigid and soft segments.
• Joint Optimization: By diffusing over the full backbone, the model ensures that the generated end-effector pose remains kinematically realizable by the rigid arm while simultaneously satisfying the physical constraints of the soft segment.

B. Diffusion Trajectory Planner

A Denoising Diffusion Probabilistic Model (DDPM) is trained to generate collision-free backbone trajectories conditioned on:

1. Environment Geometry ($\mathcal{C}_{env}$): Encoded via a point cloud encoder (MASt3R) processing reconstructed 3D scenes.
2. Goal Pose ($\mathcal{C}_{goal}$): The 3D target location.
3. Initial State ($\mathcal{C}_{state}$): The robot's current backbone configuration.

Physics-Informed Losses: During training, the model minimizes a composite loss function:

• Noise Prediction Loss: Standard MSE for denoising.
• Curvature Loss ($L_c$): Enforces physically plausible smoothness for the soft segment.
• Temporal Smoothness Loss ($L_t$): Ensures smooth transitions between time steps.
• Rigid Straightness Loss ($L_r$): Enforces that the rigid segment remains straight (zero curvature), preventing physically impossible configurations.

Inference-Time Guidance: To enforce collision avoidance without retraining, the system employs a differentiable Signed Distance Field (SDF) derived from the observed scene. During the DDIM sampling process, gradient-based guidance is applied to the predicted clean trajectory to penalize points penetrating obstacles.

C. Control and Execution

• Inverse Kinematics (IK): A learned MLP maps the planned backbone shape to actuation commands (pneumatic inputs for the SCA and joint deltas for the rigid arm).
• Residual RL Policy: A Soft Actor-Critic (SAC) policy operates in a closed loop to issue small corrective actions ($\pm 20$ mm) to compensate for modeling errors and contact uncertainties.
• Receding-Horizon Replanning: The diffusion planner is re-invoked every 32 steps to re-anchor the plan to the current robot state, correcting drift.

3. Key Contributions

1. Unified Representation: The first framework to encode the full kinematic chain of a hybrid manipulator as a single backbone shape, enabling joint optimization of rigid and soft subsystems.
2. Diffusion in Backbone Space: The first application of diffusion models to continuum manipulators, extending trajectory planning from discrete joint spaces to continuous backbone shape domains.
3. Hybrid Inference Strategy: A novel combination of inference-time geometric guidance (SDF) for collision avoidance and a residual RL policy for execution refinement.
4. Cross-Embodiment Transfer: Demonstration that planning in backbone shape space allows for direct transfer to different hardware morphologies (e.g., from a UR5 with a 2-segment SCA to a myCobot with a 1-segment SCA) with minimal online RL adaptation and no retraining of the diffusion planner.

4. Experimental Results

Experiments were conducted in the SoftManiSim simulation environment and validated on real-world hardware.

• Performance: THREAD achieved a 92.4% task success rate, significantly outperforming the strongest baseline (BC + RL at 75.3%) and reducing collisions by 5× (12.2% vs. 62.2% for BC + RL).
• Robustness: In "out-of-distribution" tests involving unseen wall angles and distances, THREAD maintained success rates of ~88%, while baselines degraded to near-zero or single-digit success rates.
• Trajectory Quality: The method produced significantly smoother trajectories (0.008 smoothness metric) compared to RL-only (0.110) and BC+RL (0.076) approaches.
• Sim-to-Real Transfer: Deployed on a real-world myCobot with a single-segment SCA (unseen during training), THREAD achieved 75.0% success with only ~100 online RL update steps, successfully threading apertures as small as 1.3× the soft segment diameter.
• Ablation Studies: Removing inference-time guidance caused a ~5.5× increase in collisions. Removing the rigidity loss resulted in physically unrealizable curved rigid segments, while removing curvature loss led to non-smooth soft bends.

5. Significance and Claims

The paper claims that THREAD addresses a fundamental gap in hybrid manipulation by shifting the planning paradigm from actuator-centric (joint/end-effector) to geometry-centric (backbone shape).

• Generative Prior: The authors argue that the right inductive bias for confined environments is a generative prior over physically realizable shapes. This allows the system to represent the full distribution of valid solutions rather than searching for a single trajectory, enabling graceful degradation as constraints tighten.
• Generalization: The backbone-centric representation is claimed to be a "structural property" that facilitates transfer across morphologically distinct hardware. By learning the geometry of feasible shapes rather than memorizing actuator trajectories, the planner generalizes to new robots and environments without retraining.
• Safety and Success: The combination of a geometric prior (diffusion) and reactive correction (RL) resolves the traditional trade-off between task success and safety, achieving high success rates without the high collision costs seen in purely reactive or imitation-based approaches.

The authors conclude that this approach offers a path toward general-purpose planners for soft and hybrid manipulators, particularly in scenarios requiring navigation through constrained, unstructured environments.