💻 computer science

Sampling-Based Follow-the-Leader Motion Planning for Manipulator-Mounted Continuum Robots

This paper introduces a sampling-based motion planner that enables efficient and theoretically guaranteed Follow-the-Leader navigation for continuum robots mounted on fully actuated manipulators by decoupling global shape search from base pose determination via closed-form geometric construction.

Original authors: Chengnan Shentu, Nicholas Baldassini, Oluwagbotemi D. Iseoluwa, Radian Gondokaryono, Jessica Burgner-Kahrs

Published 2026-05-13

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Chengnan Shentu, Nicholas Baldassini, Oluwagbotemi D. Iseoluwa, Radian Gondokaryono, Jessica Burgner-Kahrs

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine a soft, flexible robot that looks like an octopus arm or an elephant trunk. This is a Continuum Robot. Unlike a rigid robot arm with hard joints, this robot bends smoothly like a snake.

One of its coolest tricks is called "Follow-the-Leader" (FTL). Imagine a snake slithering through a narrow pipe. The snake's head (the tip) finds the way, and the rest of its body simply retraces that exact path, sliding right behind the head. This is great for surgery or inspection because it pushes gently against the walls instead of bumping into them.

The Problem:
Most robots that do this are stuck in one spot or can only slide in a straight line. But in the real world, we often mount these soft robots on a big, 6-armed robotic arm (like a human arm with a wrist) to give them more freedom.

The challenge the authors solved is this: How do you move the robot's head along a specific path while the whole robot (including the base arm) moves around it, ensuring the body perfectly follows the head without getting stuck or twisting awkwardly?

Doing this mathematically is a nightmare. It's like trying to solve a giant puzzle where every piece you move changes the shape of the whole puzzle, and you have to do it instantly.

The Solution: A "Pre-Planned" Library
The authors came up with a clever trick to avoid doing hard math in real-time. Instead of solving the puzzle on the fly, they built a massive offline library of shapes beforehand.

Think of it like a cookbook:

The Library (The Cookbook): Before the robot even starts moving, the computer generates thousands of possible shapes the robot could take. It saves all these "recipes" in a library.
The Search (Finding the Recipe): When the robot needs to move to the next point on its path, the computer looks through the library to find the shape that fits best.
The Magic Alignment (The Chef's Trick): This is the paper's biggest innovation. Usually, figuring out where to put the robot's base to make that shape fit is a slow, iterative guessing game. The authors found a geometric shortcut.
- Imagine you have a rubber snake shape.
- Step 1: You grab the snake's tail and slide it so its head hits the target point exactly.
- Step 2: You rotate the whole snake around that head point until its body lines up with the path.
- Step 3: You twist the snake around its own spine to match the final orientation.
- Result: In one instant, using simple geometry, you know exactly where the robot's base needs to be. No guessing, no slow math.

How It Works in Practice:

Global Search: The computer quickly picks the best "recipe" (shape) from the library for the next step.
Local Interpolation: Between the big steps, the computer fills in the gaps to make the movement smooth, like drawing a smooth line between dots.
Radial Symmetry: Since these robots are round, they can twist around their spine without changing their shape. The computer uses this to align the robot so it doesn't spin wildly while moving.

The Results:
The team tested this in computer simulations and on a real robot mounted on a robotic arm.

Perfect Head Tracking: The robot's head hit every target point with 0% error. It was mathematically guaranteed to be exact.
Smooth Body: The body followed the path very closely (less than 2% deviation in simulations).
Speed: Because they used the pre-made library and the geometric shortcut, the planning was fast. They could even use a "clustering" trick (grouping similar shapes) to make it even faster, trading a tiny bit of perfection for much more speed.

In Summary:
This paper teaches a soft robot how to slither perfectly through a path while being held by a robotic arm. Instead of doing difficult math every second, it uses a pre-made library of shapes and a clever geometric trick to instantly know where to place the robot's base. This makes the robot move smoothly, safely, and accurately, which is a huge step forward for using these flexible robots in real-world tasks like surgery or inspection.

Technical Summary: Sampling-Based Follow-the-Leader Motion Planning for Manipulator-Mounted Continuum Robots

1. Problem Formulation

The paper addresses the motion planning problem for Follow-the-Leader (FTL) motion in Continuum Robots (CRs) mounted on robotic manipulators with fully actuated 6-DOF base poses ($SE(3)$).

Context: FTL is a critical motion primitive where the robot body retraces the path of its tip, minimizing lateral forces on surrounding tissues or structures. While extensively studied for snake-like robots, steerable needles, and concentric tube robots, existing methods for tendon-driven CRs (TDCRs) typically assume a fixed base or a single degree-of-freedom (1-DOF) linear insertion.
The Gap: Practical deployments increasingly involve mounting CRs on serial manipulators (e.g., surgical arms, industrial inspection robots). This introduces a 6-DOF base pose that must be coordinated with the robot's internal configuration ( $Q$ ).
The Challenge: The combined configuration space ( $K = SE(3) \times Q$ $K = S E (3) \times Q$ ) is high-dimensional and nonlinear. Existing approaches often rely on simplified kinematic models (e.g., Piecewise Constant Curvature, PCC) and iterative optimization (e.g., inverse kinematics solvers). These methods suffer from:
- Sensitivity to initialization and convergence to local minima.
- Inability to utilize complex, accurate forward models (e.g., Cosserat rod, FEM, neural networks) due to computational cost.
- Lack of formal guarantees regarding solution completeness or tip tracking accuracy.

Objective: Given an ordered set of 3D waypoints $W$ , generate a continuous motion plan where the robot tip tracks the path exactly, and the inserted body minimizes deviation from the path, while jointly optimizing the CR configuration and the 6-DOF base pose.

2. Methodology

The authors propose a sampling-based framework that decouples global shape search from local interpolation and base pose determination. The core innovation is a closed-form geometric construction that uniquely determines the 6-DOF base pose for any candidate shape, eliminating the need for iterative optimization during online planning.

A. Offline Library Generation

A library of shapes is precomputed offline by uniformly sampling the CR configuration space $Q$ .
For each sample, a general forward model (FM) computes the discretized backbone points.
This shifts the computational burden of complex kinematic models to the offline phase, allowing the use of models impractical for online optimization.
Clustering: To reduce online search complexity from $O(N_{lib})$ to $O(\sqrt{N_{lib}})$ , shapes are clustered based on pairwise similarity. During execution, the planner evaluates cluster centers first, then exhaustively searches the best-matching cluster.

B. Online Base Pose Alignment (Geometric Construction)

For each waypoint $w_i$ and a candidate shape $S$ from the library, the base pose $T_b \in SE(3)$ is determined via three sequential transformations to align the shape to the active path:

Translation: Translate the shape so the tip ( $p_D$ ) coincides exactly with the current waypoint $w_i$ .
Rotation about Tip: Rotate the shape about $w_i$ to align the vector from the active shape point to the tip with the path direction.
Axial Rotation: Resolve the remaining rotational degree of freedom (twist) by aligning a third correspondence point (based on arc length ratio) to the path.

Key Property: This construction guarantees exact tip placement ( $p_D = w_i$ ) by design, regardless of the selected shape, without iterative solving.

C. Shape Evaluation and Selection

The deviation between the transformed shape and the active path is computed using a symmetric Chamfer distance.
The shape minimizing this deviation is selected. The process is parallelizable across the library.
The selected configuration $(q, T_b)$ forms a sparse motion plan.

D. Local Interpolation

To generate a continuous trajectory for deployment:

Radial Symmetry Pre-alignment: Consecutive configurations are pre-aligned by rotating them to a common reference direction, exploiting the CR's radial symmetry to minimize orientation discontinuities.
Smooth Interpolation: Intermediate configurations are generated by linearly interpolating joint states and using SLERP for tip orientation.
Base Pose Reconstruction: For each interpolation step, the base pose is constructed using the inverse of the forward model's tip pose relative to the desired interpolated tip pose. This ensures exact tip tracking is maintained throughout the interpolation steps.

3. Theoretical Guarantees

The paper establishes formal guarantees that are typically absent in optimization-based approaches:

Resolution Completeness: As the library size $N_{lib} \to \infty$ , the probability of finding a valid plan (if one exists) approaches 1. This relies on the Lipschitz continuity of the shape deviation metric.
Exact Tip Tracking by Construction: The geometric alignment ensures the tip is exactly at the waypoint at every discrete step.
Asymptotic Tip-Tracking Convergence: As the number of interpolation steps increases, the maximum deviation of the tip from the linear path between waypoints vanishes ( $O(1/h^2)$ ).

4. Experimental Results

The method was evaluated in three settings:

A. PCC Model vs. Optimization Baseline

Setup: 120 paths across three classes (C-curves, S-curves, Robot-curves). Compared against an L-BFGS-B optimization baseline.
Results:
- Tip Error: 0% for the proposed method (vs. ~1.2% for optimization).
- Shape Deviation: The linear search variant outperformed the optimization baseline (e.g., 1.25% vs 1.74% for C-curves).
- Efficiency: The clustered variant reduced computation time by over an order of magnitude (e.g., ~1.8s vs ~40s) with minimal loss in shape accuracy.

B. MuJoCo Simulation (Pseudo-Rigid-Body Model)

Setup: Used a system-identified PRB model with 30 rigid links and elastic joints to simulate realistic deformation and segment coupling.
Results: Achieved 0% tip error. Shape deviation was slightly higher (~2.5–3.2%) due to model complexity but remained competitive, demonstrating scalability to complex forward models.

C. Hardware Demonstration

Setup: A 3-segment tendon-driven CR mounted on a 7-DOF Franka manipulator.
Results: The robot successfully executed coordinated FTL motions.
- Tip Error: Observed hardware errors (mean 5.5%–9.1%) were attributed to the gap between the system-identified model and physical reality (tendon stretch, friction, hysteresis) rather than planner failure.
- Validation: The hardware results confirmed the planner generates smooth, executable trajectories that achieve qualitatively correct FTL motion.

5. Significance and Claims

The paper claims to present the first FTL motion planning method for continuum robots with a fully-actuated $SE(3)$ base that provides formal planning guarantees.

Decoupling: The primary contribution is the decoupling of shape search from base pose determination. By solving the base pose geometrically, the method avoids the local minima and computational bottlenecks of iterative optimization in high-dimensional spaces.
Model Agnosticism: The framework supports any forward model (PCC, Cosserat, FEM, Neural Networks) by shifting heavy computation offline.
Practicality: It bridges the gap between theoretical planning and practical deployment on manipulator-mounted systems, offering a principled trade-off between speed (via clustering) and accuracy.
Safety and Predictability: The theoretical guarantees (resolution completeness and exact tip tracking) address safety-critical requirements where unpredictable planner behavior is unacceptable.

The authors note limitations, including the assumption of a base-pose-independent forward model (ignoring gravity effects) and the lack of trajectory-wide shape optimization, suggesting these as directions for future work.