SAMPLING-BASED MOTION PLANNING · 6-DOF

EST-Inspired Bidirectional Planner with RRT-Connect

结合 RRT-Connect 策略的双向扩张空间树路径规划器

A custom MoveIt 2 planner for a 6-DOF manipulator. Start and goal trees are expanded through degree-weighted seed selection, local Gaussian mirror sampling, and greedy RRT-Connect-style growth to search collision-free paths in constrained configuration spaces such as an automotive interior.

UR5e configuration-space planning inside an automotive model

MOVEIT 2 / UR5E / CONFIGURATION SPACE

Core algorithm implementation: PlanningContext plugin · Bidirectional search · Heuristic sampling · Continuous collision checking · Path reconstruction

TECH STACK: ROS 2 Humble · MoveIt 2 · C++ · Pluginlib · PlanningScene · UR5e / Robotiq

Source status

This page was checked against the uploaded source. The coursework snapshot depends on external ASBR description packages and contains one syntax artifact requiring cleanup, so it is not distributed directly.

Planning problem

Inside an automotive cabin and similar narrow passages, feasible arm configurations occupy a small part of the high-dimensional joint space. Uniform samples repeatedly collide, while a one-sided tree can spend many iterations in locally open regions. The planner therefore targets both exploration bias and inter-tree connection efficiency.

Design objectives

Register a custom PlannerManager and PlanningContext with the MoveIt 2 planning pipeline.
Maintain two trees in 6D joint configuration space and alternate expansion from start and goal.
Use node degree as a sampling weight to reduce repeated exploration of heavily expanded regions.
Check every local edge through interpolation and greedily advance the opposite tree toward each accepted node.

Project result

The 80%+ success rate and 50+ trials are reported in the presentation. The uploaded source contains no batch-test harness or raw logs.

6 DOF

UR5e joint configuration space

Alternating search trees

12,000

Iteration cap in source

80%+*

Presentation-reported success

A Pluginlib-exported PlannerManager creates the custom PlanningContext for each MoveIt request. PlanningScene performs robot–environment collision queries against the automotive model; after a connection, parent chains are reconstructed and converted into a RobotTrajectory.

REQUEST

RViz2 / move_group

Start, goal joint state, and PlanningScene

PLUGIN

ASBRPlannerManager

Registers the planner and injects request/scene

ASBRContext

Runs Bi-EST expansion, connection, and backtracking

OUTPUT

RobotTrajectory

Interpolates the joint-space path for MoveIt

This is not a direct implementation of canonical EST or canonical RRT-Connect. It combines four mechanisms tailored to narrow-passage search, with all distances and step sizes defined in joint space.

Degree-weighted sample center

W(v_i)=\dfrac{1}{\mathrm{degree}(v_i)+1}

Roulette-wheel selection favors nodes with fewer children as local sampling centers. This biases Gaussian samples toward less-expanded regions; the actual extension node is still determined by nearest-neighbor lookup.

Gaussian mirror sampling

q_{\mathrm{cand}}=\mathrm{clamp}\!\big(q_{\mathrm{seed}}+\mathcal{N}(0,0.28^{2})\big)

A local Gaussian sample is drawn around the seed. If it collides, the point mirrored across the seed is checked, with up to 160 attempts.

Random-step local expansion

\mathrm{STEP}\sim\mathcal{U}(0.18,\,0.35)\,\mathrm{rad}

The Euclidean-nearest node in the active tree advances toward the sample using a random joint-space step, providing more than one exploration scale.

Greedy dual-tree connection

\Delta q_{\mathrm{connect}}=0.25\,\mathrm{rad}

The opposite tree repeatedly advances from its nearest node until it reaches the new target or encounters a colliding local edge. Parent chains are joined on success.

Source parameters

0.12

Opposite-tree sample bias

0.28 rad

Gaussian standard deviation

0.18–0.35

Active-tree random step

0.25 rad

Greedy connection step

0.04 rad

Per-joint collision resolution

160

Local sample attempt cap

Initialize two trees

Create V₁ and V₂ at qstart and qgoal, storing a parent and child degree for every node.

Select & sample

Choose a seed with 1/(degree+1) weights. Bias toward the other tree with 12% probability; otherwise draw a local Gaussian mirror sample.

Expand locally

Find the active tree's nearest node, advance by a random step, and collision-check the interpolated edge.

Trigger Connect

Advance the opposite tree toward qnew in 0.25 rad steps, checking every segment.

Join or swap

If reached, backtrack and concatenate both parent chains. Otherwise swap active trees and expand from the other side.

Continuous collision checking

For each local edge qnear → qnew, the largest per-joint change determines the interpolation count at 0.04 rad resolution. Every interpolated state is passed to MoveIt PlanningScene for robot–environment collision checking.

N_{\mathrm{steps}}=\left\lceil\dfrac{\max_{i}\lvert q_{\mathrm{new}}^{i}-q_{\mathrm{near}}^{i}\rvert}{0.04}\right\rceil

The presentation shows multiple start-to-goal requests in a wall passage and an automotive cabin. A planner demo video is embedded below; the aggregate success figure still requires a test harness and logs.

SCENE 01
Constrained automotive interior
The arm searches around the door, seat, and center console, with multiple links simultaneously restricted by scene geometry.

SCENE 02
Wall narrow passage
A simple planar obstacle isolates sampling and connection behavior while exercising local-edge collision checks.

SCENE 03
MoveIt 2 planning interface
Random valid goals are selected through the RViz2 MotionPlanning panel and the resulting trajectory is visualized.