Soft Growing Robot Motion Planning Suite

Virtual-Joint based Soft Robot Motion Planning Suite

# Soft Growing Robot Motion Planning Suite

A MATLAB implementation of motion planning algorithms for soft growing manipulator. The codebase covers two families of planners: sampling-based (RRT variants) and search-based (A* variants), all operating on the same robot configuration space.

---

## Configuration Space

Each robot configuration is a `j×3` matrix, where `j` is the number of joints. Each row encodes one joint as `[rotation_x, rotation_y, length]`:

- `rotation_x`, `rotation_y` — bending angles, bounded by `sp.steerBounds`

- `length` — how much of this link is currently extended, bounded by `[0, sp.design(i)]`

---

## Algorithm Families

### 1. RRT Variants (sampling-based)

| Algorithms |

|---|

| `RRT` |

| `Soft-RRT` |

| `RRT*` |

| `Soft-RRT*` |

| `BiRRT*` |

| `Soft-BiRRT*` |

**BiRRT* and Soft-BiRRT*** also runs two post-insertion optimisation steps:

- **Random Vertex Contraction** — picks two nodes on the current best path and tries to connect them directly, skipping intermediate nodes if collision-free

- **Branch-and-Bound** — removes any tree node whose `cost_to_node + heuristic_to_goal > best_known_cost`

**"Soft" variants** introduce new parent selection approach (`getParentCandidates`, tailored for the soft growing manipulators' needs.

#### Running RRT variants

Open `rrt_main.m` and set three variables:

```matlab

sp = envs{1, 5}; % task environment (1=empty, 2=wall, 3=hole, 4=windows, 5=corner, 6=custom)

rrtConf = combs{1, 7}; % parameter combination, indices 1–32

selected_algo_name = "Soft-BiRRT*"; % see table above for valid names

```

RRT parameters (`rrtConf` fields):

| Field | Effect |

|---|---|

| `iteration` | Number of nodes attempted; more iterations = better path, longer runtime (500 - 2000 recommended) |

| `stepSize` | Maximum extension distance per expansion step (6-20 recommended) |

| `pOfGoal` | Probability of sampling the goal config directly each iteration (0.1 is recommended) |

| `pOfVC` | (BiRRT* only) Probability of attempting vertex contraction after each insertion (0.1 is recommended) |

---

### 2. A* Variants

| Fringes |

|---|

| `Deterministic` |

| `Stochastic` |

- **Deterministic** fringe is a data structure based on the generalized priority queue, or min-max heap, which deterministically removes the worst element from the fringe.

- **Stochastic** fringe is a stochastic deque, in which when the fringe reaches its maximum capacity, one random element among the worst ones is popped out.

#### Running A* variants

Open `astar_main.m` and set three variables:

```matlab

sp = envs{1, 5}; % task environment (1=empty, 2=wall, 3=hole, 4=windows, 5=corner, 6=custom)

astarComb = astar_comb{1, 7}; % parameter combination, indices 1–70

```

A* parameters (`astarComb` fields):

| Field | Effect |

|---|---|

| `astarType` | Determines the type of the fringe. If it is 0/1, it is stochastic/deterministic.|

| `velocities` | Robot action speeds used for cost calculation. It represents respectively steering, retraction, and growth speeds. (10 is recommended for steering, 20 or higher values are recommended for rectraction and growth.) |

| `fringe_size` | Maximum capacity of the fringe. (Between 50 and 10k is recommended) |