Contact-Aware Diffusion Sampling for RRT-Based Manipulation

Abstract

Rapidly exploring Random Trees (RRT) provide probabilistic completeness but often explore inefficiently in high-DOF manipulation tasks. We address this by proposing a contact-aware, two-level planner that couples a learned toggle–subgoal predictor with a conditional diffusion sampler in joint space under a completeness-preserving mixture with uniform sampling. An upper ResNet-based network predicts task-relevant milestones from RGB images: grasp/release “toggle” configurations and intermediate joint-space subgoals that serve as phase-wise, receding-horizon targets between consecutive contact events. Conditioned on these predictions and the current state, a lower-level diffusion model samples tree-extension segments—joint-space directions and step lengths—instead of absolute configurations. These proposals act as a drop-in replacement for uniform sampling in standard RRT/RRT-Connect, while a nonzero fraction of uniform samples preserves probabilistic completeness. By biasing growth toward contact-relevant regions, the planner concentrates the search near feasible approach manifolds without altering nearest-neighbor, steering, or collision-checking primitives. In mug pick-and-place simulations, the proposed method achieves higher success rates than diffusion and other sequence-based policies trained by imitation learning, and requires fewer RRT expansions than uniform and goal-biased RRT as well as prior learning-guided samplers based on CVAE and conditional GAN, under identical collision checking and iteration limits.

1. Introduction

Sampling-based motion planning (SBMP) is a standard approach for high-dimensional and dynamically constrained planning. Rather than explicitly discretizing the state space, SBMP draws candidate states in the feasible set and connects them with local steering while delegating feasibility to collision checking. Classical sampling strategies (random or low-discrepancy) provide broad coverage so that, as the number of samples increases, the planner attains probabilistic completeness and, for suitable variants, asymptotic optimality [1,2,3].

Within SBMP, RRT and its descendants (e.g., RRT-Connect, RRT*) are widely used in both manipulation and navigation. In manipulation, they are combined with task-space goals, kinematic limits, and collision checking to plan whole-arm motions in clutter (e.g., shelf insertion, bin picking, peg-in-hole). In navigation and autonomous driving, RRT-style planners handle kinodynamic constraints and long-horizon obstacle avoidance. These uses benefit from modular components (nearest-neighbor search, steering, and collision checks), but the core sampling mechanism usually remains uniform or only weakly informed.

In practice, feasible motions concentrate in a small, structured subset of the state space due to the environment (start/goal placement, clutter, narrow passages), the system (e.g., stability regions), and implicit constraints (e.g., loop closures, multi-robot separation). Uniform sampling eventually explores such regions, but only through exhaustive trial and error, limiting efficiency and the ability to reuse prior knowledge.

Recent progress in robot learning shows that imitation learning (IL) is effective for visuomotor control—especially in manipulation—by training policies from demonstrations. In parallel, diffusion policies model action sequences or low-level controls via denoising diffusion, enabling multi-modal behaviors from demonstrations. Despite these advances, purely end-to-end controllers often degrade under covariate shift, sensing noise, or contact discontinuities and typically lack an explicit search component. A complementary direction is to retain a search-based planner while learning informative sampling distributions or heuristics, so that the planner preserves coverage and learning provides bias.

A body of work explores learning to guide SBMP by shaping sampling distributions, estimating heuristics, or proposing local connections (e.g., learned samplers for RRT/RRT*, neural encoders that map scenes to promising states, or networks that infer steering targets) [4,5]. While these methods accelerate planning, two gaps remain susceptible to manipulation: (i) capturing the multi-modal structure of promising joint-space regions (e.g., multiple IK branches, distinct approach corridors), and (ii) incorporating contact semantics (grasp/release timing) so that samples align with task events rather than only geometric proximity.

We propose a contact-aware SBMP framework for manipulation that combines (i) A learned toggle–subgoal predictor. (ii) A conditional diffusion sampler operating directly in joint space. The upper network takes a scene RGB image together with the current joint configuration and gripper mode and predicts (a) a toggle configuration at which the gripper should switch between open and close, (b) the corresponding toggle mode, and (c) an intermediate joint-space subgoal that serves as a phase-wise, receding-horizon target for the planner: for each phase, we plan from the current configuration to the predicted subgoal, execute the resulting motion (toggling the gripper near the toggle configuration), and then re-query the upper network at the new state. Conditioned on the image and these predictions, along with the current robot state, the lower-level diffusion model proposes tree-extension segments—a joint-space direction and a positive step length—that replace and augment the sampling step in RRT/RRT-Connect. Proposals are based on a mixture of the learned sampler and a uniform policy; by maintaining a nonzero fraction of uniform proposals, the planner preserves probabilistic completeness while biasing growth toward contact- and task-relevant regions. Nearest-neighbor search, steering, and collision checking remain unchanged.

We make the following contributions:

• Toggle–subgoal prediction for contact-aware planning. We introduce a ResNet-based predictor that maps a single RGB image and the current robot state (joints and gripper mode) to a toggle configuration, its toggle mode, and an intermediate joint-space subgoal. This exposes grasp/release timing and provides task-aligned guidance for where the RRT tree should expand within each short planning phase on the way to the next contact event.
• Diffusion over joint-space extension segments. We learn a conditional Denoising Diffusion Probabilistic Model (DDPM) [6] that samples joint-space directions and step lengths for RRT tree extensions instead of absolute configurations. The sampler is conditioned on the scene image, current configuration, and predicted toggle/subgoal, and plugs into RRT/RRT-Connect without modifying nearest-neighbor search, steering, or collision checking.
• Mixture-based completeness with receding-horizon targets. We mix diffusion-based segment proposals with a nonzero fraction of uniform segments, preserving probabilistic completeness while biasing the search toward contact- and task-relevant regions. Using the predicted subgoal as the current target in each phase yields a receding-horizon planner that aligns local tree growth with task progress.
• Simulation evidence on contact-rich mug pick-and-place. For a contact-rich mug pick-and-place benchmark with multiple grasp/release events, our planner achieves higher success rates than imitation-only baselines and requires fewer RRT expansions than uniform/goal-biased RRT and prior learned samplers under identical collision checking and iteration limits.

2. Related Work

Beyond classical RRT/RRT* baselines, a large body of work focuses on where to sample to accelerate the search. Informed RRT* restricts sampling to an ellipsoidal subset that can still improve the current solution, yielding large speedups when high-quality paths exist [7]. Numerous RRT* variants pursue constrained or hybrid sampling to emphasize promising regions while preserving coverage properties. These methods handcraft informative regions, whereas we learn them from data and inject contact semantics via predicted toggle states.

Learning-based guidance replaces uniform sampling with data-driven priors or proposes local connections. Ichter et al. learn sampling distributions conditioned on a problem context and inject them into RRT, demonstrating substantial speedups [4]. MPNet encodes workspace observations and predicts states that guide a planner or produce end-to-end paths [5]. More recently, Neural-Informed RRT* learns to predict informed regions for RRT*, improving convergence by biasing samples toward promising subsets of the state space [8]. Generative models such as CVAEs and conditional GANs have also been used to learn sampling distributions for RRT-style planners (e.g., in the spirit of [9,10]), and we adopt CVAE- and conditional-GAN–based samplers as baselines in our experiments. Our approach shares the goal of learned guidance but differs by (i) operating directly in joint space with a diffusion model over tree-extension segments rather than absolute states, and (ii) conditioning on a predicted toggle configuration to inject contact semantics into the sampling process.

Diffusion has proven effective for multi-modal visuomotor policies and trajectory generation. Diffusion Policy learns time-indexed action distributions from demonstrations and shows strong manipulation performance [11]; in our experiments, we include a Diffusion Policy-style baseline adapted to our single-RGB-camera setting. Diffuser plans by denoising full trajectories, bridging generative modeling and decision-making [12]. Motion Planning Diffusion generates feasible motions with diffusion and compares against standard planning baselines on simulated manipulators [13]. There is also interest in model-based diffusion that leverages differentiable dynamics or costs for trajectory optimization [14]. In contrast, we do not replace the planner: we keep RRT/RRT-Connect and use diffusion only to sample extension segments under a mixture with uniform sampling, thereby preserving probabilistic completeness.

Many learning-based planners and policies capture geometry but underutilize task events such as grasp/release timing. Prior work commonly relied on goal poses, grasp detectors, or end-effector waypoints to guide planning or control [15,16,17], and task-and-motion planning frameworks explicitly model discrete gripper actions alongside continuous motion [18,19]. There is also existing work that focuses on contact-state estimation and force/impedance control for contact-rich skills [20,21]. However, learning a joint-space toggle configuration and using it as a conditioning signal to bias SBMP sampling has, to the best of our knowledge, received limited attention. We take this route and condition a diffusion-based sampler on a predicted toggle configuration (and subgoal), aligning the search with contact-relevant regions of the configuration space while retaining SBMP’s completeness properties through a mixture with uniform sampling.

3. Preliminaries

3.1. System Model and Notation

We consider a d-DOF manipulator with configuration (joint) space $\mathcal{Q}\subset {\mathbb{R}}^d$ and task space (workspace) $\mathcal{W}\subset {\mathbb{R}}^3\times \mathrm{SO}\left(3\right)$, where $\mathrm{SO}\left(3\right)$ denotes the space of 3D rotations. We also use the generic SBMP notation $\mathcal{X}$ when referring to abstract state spaces.

• $\mathcal{X}\subset {\mathbb{R}}^n$: generic state space (for SBMP discussion).
• ${\mathcal{X}}_{\mathrm{obs}}\subset \mathcal{X}$, ${\mathcal{X}}_{\mathrm{free}}=\mathcal{X}\setminus {\mathcal{X}}_{\mathrm{obs}}$ (obstacle and collision-free subsets).
• ${\mathcal{Q}}_{\lim }\subset \mathcal{Q}$: joint-limit–feasible set (per-joint bounds enforced).
• $f_{\mathrm{FK}}:\mathcal{Q}\to \mathcal{W}$: forward kinematics to end-effector pose (used only if task-space goals are considered).
• $q_{\mathrm{start}}\in {\mathcal{Q}}_{\lim }$, ${\mathcal{Q}}_{\mathrm{term}}\subseteq {\mathcal{Q}}_{\lim }$ (start configuration and terminal goal region in joint space).
• State and observations: the current joint configuration is $q_{\mathrm{curr}}\in {\mathcal{Q}}_{\lim }$ and the current gripper mode is $g_{\mathrm{curr}}\in \{\mathrm{open},\mathrm{close}\}$. The observation consists of an RGB image $I\in {\mathbb{R}}^{H\times W\times 3}$ and $(q_{\mathrm{curr}},g_{\mathrm{curr}})$.
• Toggle and subgoal: the upper network predicts a toggle configuration $q_{\mathrm{toggle}}\in {\mathcal{Q}}_{\lim }$, a toggle mode $g_{\mathrm{toggle}}\in \{\mathrm{open},\mathrm{close}\}$, and a joint-space subgoal $q_{\mathrm{goal}}\in {\mathcal{Q}}_{\lim }$ used as a phase-wise receding-horizon target between consecutive toggle events.
• Distance and tolerance: unless stated otherwise, the tree metric is a (possibly weighted) Euclidean distance

  $$d(q,q^{\prime })={\parallel W(q-q^{\prime })\parallel }_2,$$

  where $W\in {\mathbb{R}}^{d\times d}$ is a symmetric positive-definite (typically diagonal) weight matrix over joints.

For joint-limit projection, we write ${\Pi }_{\lim }:\mathcal{Q}\to {\mathcal{Q}}_{\lim }$, which clips/projects any proposed configuration to satisfy limits.

3.2. Geometric Motion-Planning Problem

Let $\mathcal{Q}\subset {\mathbb{R}}^d$ be the configuration space, ${\mathcal{Q}}_{\lim }\subset \mathcal{Q}$ the joint-limit–feasible set, and let ${\mathcal{Q}}_{\mathrm{obs}}\subset \mathcal{Q}$ denote collision configurations under the robot–environment model, with ${\mathcal{Q}}_{\mathrm{free}}=\mathcal{Q}\setminus {\mathcal{Q}}_{\mathrm{obs}}$. A path is a continuous map $s:[0,1]\to \mathcal{Q}$ with $s\left(0\right)=q_{\mathrm{start}}$ and $s\left(1\right)\in {\mathcal{Q}}_{\mathrm{term}}$.

• Collision-free: $s\left(\tau \right)\in {\mathcal{Q}}_{\lim }\cap {\mathcal{Q}}_{\mathrm{free}}$ for all $\tau \in [0,1]$.
• Feasible path: a collision-free path from $q_{\mathrm{start}}$ to ${\mathcal{Q}}_{\mathrm{term}}$.

We target single-query geometric planning. In experiments, we primarily report the success rate; timing and check-count metrics are optional.

3.3. RRT Primitives

Let $T=(V,E)$ be the search tree in $\mathcal{Q}$ and let d denote the (possibly weighted) Euclidean metric used for nearest-neighbor queries. We also fix a goal tolerance ${\tau }_{\mathrm{goal}}>0$ so that a tree node $v\in V$ is said to reach a target $q_{\mathrm{tgt}}$ if $d(v,q_{\mathrm{tgt}})\le {\tau }_{\mathrm{goal}}$.

• $\mathrm{Nearest}(T,q)$: returns $arg{min}_{v\in V}d(v,q)$.
• $\mathrm{Steer}(q,q_{\mathrm{target}},\Delta )$: local extension from q toward $q_{\mathrm{target}}$ with step size (or cap) $\Delta$.
• $\mathrm{CollisionFree}(q,q^{\prime })$: true if the straight-line (or local steering) motion from q to $q^{\prime }$ lies in ${\mathcal{Q}}_{\lim }\cap {\mathcal{Q}}_{\mathrm{free}}$ (checked continuously).
• Tree-extension segment: an ordered pair $(u,s)$ with unit direction $u\in {\mathbb{R}}^d$ ($\parallel u\parallel =1$) and step length $s>0$, producing the proposal

  $$q_{\mathrm{new}}={\Pi }_{\lim }(q+su),$$

  where ${\Pi }_{\lim }$ enforces joint limits prior to collision checking along the induced local motion.

This segment-based formulation matches the learned sampler introduced in Section 4.3.

3.4. Sensing and Task Assumptions

We assume RGB scene observations and a binary gripper.

• Observation: a scene image $I\in {\mathbb{R}}^{H\times W\times 3}$ and the current robot state $(q_{\mathrm{curr}},g_{\mathrm{curr}})$.
• Gripper mode: $g\in \{\mathrm{open},\mathrm{close}\}$.
• Toggle state: a joint configuration at which the gripper should switch between modes for the task at hand (used as a contact-aware cue for sampling).

4. Proposed Method

4.1. Overview

We adopt the notation from Section 3. In particular, $\mathcal{Q}$ is the joint space, ${\mathcal{Q}}_{\lim }\subset \mathcal{Q}$ denotes the joint-limit–feasible set, and $q_{\mathrm{start}}$ and ${\mathcal{Q}}_{\mathrm{term}}$ denote the start and the terminal goal region. We propose a two-level framework that couples learned contact semantics with a sampling-based search (Figure 1). The upper level is a ResNet-based network that takes a single RGB image together with the current joint configuration and gripper mode and predicts three quantities: a toggle configuration at which the gripper should switch, the corresponding toggle mode, and a joint-space subgoal used as a phase-wise receding-horizon target.

The lower planner treats the predicted subgoal as the current RRT target for the current phase and runs a diffusion-guided RRT/RRT-Connect from the current configuration toward this subgoal, drawing candidate segments from a mixture of the learned sampler and a uniform segment policy. Once a path is found and executed (with the gripper toggled near the predicted toggle configuration when appropriate), the resulting state becomes the start of the next planning phase, where the upper network is queried again. Repeating this procedure yields a phase-wise receding-horizon behavior that progresses through the sequence of contact events in the task.

4.2. Upper Network: Toggle–Subgoal Predictor

Architecture. Given $(I,q_{\mathrm{curr}},g_{\mathrm{curr}})$, a ResNet encoder extracts a visual embedding from I and fuses it with $(q_{\mathrm{curr}},g_{\mathrm{curr}})$ via lightweight MLP layers. Three prediction heads branch from shared features: (i) A regression head for the toggle configuration $q_{\mathrm{toggle}}$. (ii) A binary classification head for the toggle mode $g_{\mathrm{toggle}}\in \{\mathrm{open},\mathrm{close}\}$, and (iii) a regression head for the joint-space subgoal $q_{\mathrm{goal}}$, which serves as a phase-wise receding-horizon target between consecutive toggle events. We apply standard feature normalization and dropout to the heads. Since all experiments are performed via simulations, only basic resizing/cropping is used for images.

Supervision and losses. From expert executions, we detect grasp/release events to label $(q_{\mathrm{toggle}}^{★},g_{\mathrm{toggle}}^{★})$ and extract approach subgoals $q_{\mathrm{goal}}^{★}$ along the corresponding segments (e.g., just before contact or aligned with task progression). In principle, $q_{\mathrm{goal}}^{★}$ is chosen slightly before the toggle configuration along the approach trajectory; however, whenever this pre-contact waypoint is already within a small tolerance of $q_{\mathrm{toggle}}^{★}$ (in joint space), we simply set $q_{\mathrm{goal}}^{★}=q_{\mathrm{toggle}}^{★}$ for that phase.

The total loss is a weighted sum of two robust regressions and one binary classification, plus a mild regularizer enforcing joint-limit feasibility (and, optionally, proximity between predicted toggle and subgoal):

$$\begin{array}{cc}\hfill {\mathcal{L}}_{\mathrm{toggle-subgoal}}& =w_q\mathrm{Huber}\left(q_{\mathrm{toggle}}^{\mathrm{pred}}-q_{\mathrm{toggle}}^{★}\right)\hfill \\ \hfill & +w_g\mathrm{CE}\left(\sigma \left(z_{\mathrm{toggle}}^{\mathrm{pred}}\right),g_{\mathrm{toggle}}^{★}\right)\hfill \\ \hfill & +w_s\mathrm{Huber}\left(q_{\mathrm{goal}}^{\mathrm{pred}}-q_{\mathrm{goal}}^{★}\right)\hfill \\ \hfill & +w_r\mathcal{R}.\hfill \end{array}$$

(1)

Here, $\mathrm{Huber}(·)$ is applied element-wise per joint and averaged; the binary cross-entropy

$$\mathrm{CE}(p,y)=-\left[ylogp+(1-y)log(1-p)\right],y\in \{0,1\},p=\sigma \left(z\right)\in (0,1),$$

uses the logit $z_{\mathrm{toggle}}^{\mathrm{pred}}$ and sigmoid $\sigma (·)$. Weights $w_q,w_g,w_s,w_r>0$ balance the terms (typical ranges: $w_q,w_s\in [1,10]$, $w_g\in [0.5,2]$, $w_r\in [{10}^{-4},{10}^{-1}]$). Joint values are in radians (optionally normalized).

Regularization: We use

$$\mathcal{R}={\lambda }_{\lim }\left({\mathcal{R}}_{\lim }(q_{\mathrm{toggle}}^{\mathrm{pred}})+{\mathcal{R}}_{\lim }(q_{\mathrm{goal}}^{\mathrm{pred}})\right)+{\lambda }_{\mathrm{prox}}\parallel q_{\mathrm{goal}}^{\mathrm{pred}}-q_{\mathrm{toggle}}^{\mathrm{pred}}{\parallel }_2^2,$$

(2)

with a soft hinge outside tightened limits:

$${\mathcal{R}}_{\lim }\left(q\right)=\sum _{j=1}^d\left[max{(0,q_j-(q_j^{max}-\delta ))}^2+max{(0,(q_j^{min}+\delta )-q_j)}^2\right],$$

(3)

where $\delta >0$ discourages sitting on the bounds, ${\lambda }_{\lim }$ controls limit penalties, and ${\lambda }_{\mathrm{prox}}$ (kept relatively small) softly encourages the predicted subgoal $q_{\mathrm{goal}}^{\mathrm{pred}}$ not to deviate excessively from the predicted toggle $q_{\mathrm{toggle}}^{\mathrm{pred}}$ when appropriate. Standard weight decay is handled by the optimizer and excluded from $\mathcal{R}$. This term mainly affects phases where the demonstrated subgoal lies close to the toggle, acting as a weak consistency prior between the two heads.

4.3. Lower Planner: Diffusion-Guided Segment Sampling

Learning a sampling distribution (via diffusion). We operate on tree-extension segments rather than absolute configurations. Given a base node $q\in \mathcal{Q}$, a segment is an ordered pair $(u,s)$ with unit direction $u\in {\mathbb{R}}^d$ ($\parallel u\parallel =1$) and step length $s>0$, producing the proposal

$$q_{\mathrm{new}}={\Pi }_{\lim }(q+su),$$

(4)

followed by continuous collision checking along the local motion; proposals that collide or violate limits are rejected. The diffusion sampler $D_{\theta }$ generates $(u,s)$ conditionally on

$$\mathrm{cond}=(I,q_{\mathrm{curr}},q_{\mathrm{goal}},q_{\mathrm{toggle}},g_{\mathrm{toggle}}),$$

where $q_{\mathrm{goal}}$ is the upper-level subgoal (current RRT target within the phase), $q_{\mathrm{curr}}$ is the current configuration at which the phase starts, and I denotes the current RGB image for that phase. An optional lightweight tree summary $\mathrm{TreeContext}\left(T\right)$ (e.g., node count, recent expansion directions) can be included.

Diffusion objective and architecture. Let $x_0\in {\mathbb{R}}^{d+1}$ be the clean segment vector obtained by concatenating the target direction and step, $x_0:=[u^{★};s^{★}]$. We use the standard DDPM forward noising process with a variance schedule ${\left\{{\beta }_t\right\}}_{t=1}^T$ and ${\alpha }_t=1-{\beta }_t$, ${\overline{\alpha }}_t={\prod }_{k=1}^t{\alpha }_k$:

$$x_t=\sqrt{{\overline{\alpha }}_t}{x}_0+\sqrt{1-{\overline{\alpha }}_t}ϵ,ϵ\sim \mathcal{N}(0,I),t\sim \mathcal{U}\{1,\dots ,T\}.$$

Here, $x_t$ is the noised sample at step t. We train a noise-prediction model ${ϵ}_{\theta }$ conditioned on $\mathrm{cond}=(I,q_{\mathrm{curr}},q_{\mathrm{goal}},q_{\mathrm{toggle}},g_{\mathrm{toggle}})$:

$${\mathcal{L}}_{\mathrm{diff}}={\mathbb{E}}_{t,ϵ}\parallel ϵ-{ϵ}_{\theta }\left(x_t,\mathrm{cond},t\right){\parallel }_2^2.$$

(5)

In our implementation, the denoiser ${ϵ}_{\theta }$ is instantiated as a lightweight 1D U-Net similar in spirit to Diffusion Policy [11], but operating on a single $(d+1)$-dimensional segment vector $(u,s)$ instead of a full action sequence. The global conditioning vector is constructed by concatenating (i) A ResNet-18–GN + spatial softmax encoder applied to the single RGB image I. (ii) An MLP embedding of the current joint configuration and gripper mode. $(q_{\mathrm{curr}},g_{\mathrm{curr}})$, and (iii) an MLP embedding of the predicted toggle and subgoal $(q_{\mathrm{toggle}},q_{\mathrm{goal}},g_{\mathrm{toggle}})$. This concatenated feature is projected to a 256-dimensional vector and injected into each residual block of the U-Net via FiLM-style affine modulation (scale–shift on activations). The diffusion timestep t is embedded with sinusoidal position encodings and concatenated into the same conditioning pathway.

We train with the standard $ϵ$-prediction objective in Equation (5) using a cosine noise schedule with $T=100$ diffusion steps. At test time, we use a deterministic DDIM sampler with $T=25$ steps (and $T=10$ in the ablations), obtaining ${\widehat{x}}_0=[\widehat{u};\widehat{s}]$, normalizing $\widehat{u}$ to unit length and clipping $\widehat{s}$ to $[s_{min},s_{max}]$, and finally forming a tree-extension segment via Equation (4).

Base-node selection and uniform segments. We use a routine $SelectBase(T,\mathrm{target},\mathrm{strategy})$ to choose the node $q_{\mathrm{base}}\in V$ from which an extension is attempted: nearest-to-target (for learned proposals), $q_{\mathrm{base}}=arg{min}_{v\in V}d(v,q_{\mathrm{goal}})$, which aligns growth toward the subgoal; and rrt-default (for uniform proposals), $q_{\mathrm{base}}=Nearest(T,q_{\mathrm{rand}})$, which recovers the classical RRT behavior. Optionally, ties can favor nodes nearer to $q_{\mathrm{toggle}}$ during grasp/release phases. The routine UniformSegment() samples a unit direction u uniformly on the sphere and a positive step s from a bounded distribution, then clips s to $[s_{min},s_{max}]$.

Algorithm and integration with RRT/RRT-Connect (phase-wise planning and execution). Algorithm 1 summarizes this phase-wise planning–execution loop. We fix a goal tolerance ${\tau }_{\mathrm{goal}}>0$ so that a tree node v is said to reach $q_{\mathrm{goal}}$ if $d(v,q_{\mathrm{goal}})\le {\tau }_{\mathrm{goal}}$. At the beginning of each short planning phase, we acquire the current RGB image and query the upper network $F_{\psi }$ on the current observation to predict $(q_{\mathrm{toggle}},g_{\mathrm{toggle}},q_{\mathrm{goal}})$. Within that phase, at each tree expansion, we draw a candidate segment either from the conditional diffusion sampler or, with nonzero probability $p_{\mathrm{uni}}>0$, from UniformSegment(). Once a node sufficiently close to $q_{\mathrm{goal}}$ is found (or an iteration limit is reached), we stop planning, extract and smooth a path segment, and execute it on the robot; the gripper is toggled to $g_{\mathrm{toggle}}$ when the trajectory passes near $q_{\mathrm{toggle}}$. The resulting state $(q_{\mathrm{curr}},g_{\mathrm{curr}})$ becomes the start of the next planning phase, and the procedure is repeated with a fresh image and a new query to $F_{\psi }$.

In practice, several such planning phases may be chained on the way to the same physical grasp or release event, so multiple successive subgoals can be predicted before the corresponding toggle configuration is actually reached. Nearest-neighbor search, steering, and collision checking remain unchanged, and the same interface applies to RRT-Connect by expanding two trees symmetrically and attempting to connect them when appropriate.

In our implementation, the upper network $F_{\psi }$ is queried once per planning–execution phase (one iteration of the outer while-loop in Algorithm 1). At the beginning of each phase, we render the current RGB image and form the observation $(I_{\mathrm{curr}},q_{\mathrm{curr}},g_{\mathrm{curr}})$, predict $(q_{\mathrm{toggle}},g_{\mathrm{toggle}},q_{\mathrm{goal}})$, and then keep these predictions fixed while the RRT/RRT-Connect planner runs for up to $N_{\mathrm{rrt}}$ expansions toward $q_{\mathrm{goal}}$. After a path segment is found and executed, the resulting state $(q_{\mathrm{curr}},g_{\mathrm{curr}})$ becomes the start of the next phase, at which point we render a new image and query $F_{\psi }$ again. Thus, the re-query frequency of the upper network is tied to how often we re-plan (per phase), rather than to the number of individual RRT expansions. A single physical toggle event (e.g., the first grasp) may be reached over several successive phases as the predicted subgoals progressively move the robot closer to the toggle configuration.

Completeness under mixture. Let $\lambda \in [0,1)$ denote the fraction of learned proposals and $(1-\lambda )$ the fraction of uniform proposals selected at each expansion (equivalently, $p_{\mathrm{uni}}=1-\lambda$ in Algorithm 1). Since $(1-\lambda )>0$, every open subset of the free configuration space retains nonzero sampling probability from the uniform component. Therefore, the planner inherits the probabilistic completeness behavior of RRT: as the total number of proposals $N\to \infty$, the probability of finding a feasible solution (if one exists) converges to one. This follows the same argument used for learned sampling mixtures in [4], where theoretical guarantees hold after replacing the number of samples N with the number of uniform samples $(1-\lambda )N$. Intuitively, adding learned samples can only accelerate the search, while the uniform sub-sampling preserves coverage of ${\mathcal{Q}}_{\mathrm{free}}$.

Path post-processing (smoothing and parameterization): After a successful search, we apply two-stage post-processing: (i) Shortcut smoothing that attempts to establish direct connections between random waypoint pairs with continuous collision checks and limit projection. (ii) A continuous fit using cubic B-splines (or piecewise quintic) followed by time-parameterization under joint velocity/acceleration limits. The resulting trajectory remains in ${\mathcal{Q}}_{\lim }$ and is validated with dense collision checks before execution, reducing path length/curvature and improving tracking robustness without altering the planner interface.

Algorithm 1 Toggle–subgoal conditioned diffusion-guided RRT (phase-wise planning and execution)
Require: Access to current RGB image $I_{\mathrm{curr}}$ (camera/rendering); initial robot state $(q_{\mathrm{start}},g_{\mathrm{start}})$; terminal region ${\mathcal{Q}}_{\mathrm{term}}$; RRT iteration limit $N_{\mathrm{rrt}}$; uniform-proposal probability $p_{\mathrm{uni}}>0$; goal tolerance ${\tau }_{\mathrm{goal}}$; upper network $F_{\psi }$; diffusion sampler $D_{\theta }$
1:  $(q_{\mathrm{curr}},g_{\mathrm{curr}})\leftarrow (q_{\mathrm{start}},g_{\mathrm{start}})$
2:  while $q_{\mathrm{curr}}\notin {\mathcal{Q}}_{\mathrm{term}}$ do
3:        Obtain current RGB image $I_{\mathrm{curr}}$
4:        $(q_{\mathrm{toggle}},g_{\mathrm{toggle}},q_{\mathrm{goal}})\leftarrow F_{\psi }(I_{\mathrm{curr}},q_{\mathrm{curr}},g_{\mathrm{curr}})$
▹ predict next toggle and subgoal for the current phase
5:        Initialize tree $T=(V,E)$ with root $q_{\mathrm{curr}}$: $V\leftarrow \left\{q_{\mathrm{curr}}\right\},E\leftarrow \varnothing$
6:       $q_{\mathrm{reach}}\leftarrow q_{\mathrm{curr}}$	▹ best node reached so far toward $q_{\mathrm{goal}}$
7:        for $k=1$ to $N_{\mathrm{rrt}}$ do
8:              $\mathrm{use\_learned}\leftarrow (\mathrm{Uniform}(0,1)\ge p_{\mathrm{uni}})$
9:              if $\mathrm{use\_learned}$ then
10:                  $(u,s)\leftarrow D_{\theta }(I_{\mathrm{curr}},q_{\mathrm{curr}},q_{\mathrm{goal}},q_{\mathrm{toggle}},g_{\mathrm{toggle}},\mathrm{TreeContext}\left(T\right))$
▹$q_{\mathrm{curr}}$ is the phase start state
11:                  $q_{\mathrm{base}}\leftarrow SelectBase(T,\mathrm{target}=q_{\mathrm{goal}},\mathrm{strategy}=\mathrm{nearest-to-target})$
12:              else
13:                  $(u,s)\leftarrow$ UniformSegment()
14:                  $q_{\mathrm{base}}\leftarrow SelectBase(T,\mathrm{none},\mathrm{strategy}=\mathrm{rrt-default})$
15:              end if
16:              $q_{\mathrm{new}}\leftarrow {\Pi }_{\lim }(q_{\mathrm{base}}+su)$
17:              if $CollisionFree(q_{\mathrm{base}},q_{\mathrm{new}})$ then
18:                  $V\leftarrow V\cup \left\{q_{\mathrm{new}}\right\}$;   $E\leftarrow E\cup \left\{(q_{\mathrm{base}},q_{\mathrm{new}})\right\}$
19:                  if $d(q_{\mathrm{new}},q_{\mathrm{goal}})<d(q_{\mathrm{reach}},q_{\mathrm{goal}})$ then
20:                        $q_{\mathrm{reach}}\leftarrow q_{\mathrm{new}}$
21:                  end if
22:              end if
23:              if $d(q_{\mathrm{reach}},q_{\mathrm{goal}})\le {\tau }_{\mathrm{goal}}$ then	▹ phase goal reached
24:                  break
25:              end if
26:        end for
27:        ${\mathrm{path}}_{\mathrm{phase}}\leftarrow$ ExtractPath(T, qreach)
28:        ${\mathrm{path}}_{\mathrm{phase}}\leftarrow$ Smooth(pathphase)
29:        Execute ${\mathrm{path}}_{\mathrm{phase}}$ on the robot/simulator, toggling the gripper to $g_{\mathrm{toggle}}$ when $q\approx q_{\mathrm{toggle}}$
30:        Update $(q_{\mathrm{curr}},g_{\mathrm{curr}})$ from the final state of the executed trajectory
31:  end while
32:  return concatenation of all executed phase trajectories

4.4. Inference Procedure

Given access to an RGB camera that provides the current scene image and an initial robot state $(q_{\mathrm{start}},g_{\mathrm{start}})$, inference proceeds in a phase-wise manner:

• Initialize the current state as $(q_{\mathrm{curr}},g_{\mathrm{curr}})\leftarrow (q_{\mathrm{start}},g_{\mathrm{start}})$.
• While $q_{\mathrm{curr}}$ has not reached the terminal region ${\mathcal{Q}}_{\mathrm{term}}$:

  (a)

  Upper-level prediction (current phase). Acquire the current RGB image $I_{\mathrm{curr}}$ and query the upper network on the current observation to obtain

  $$(q_{\mathrm{toggle}},g_{\mathrm{toggle}},q_{\mathrm{goal}})=F_{\psi }(I_{\mathrm{curr}},q_{\mathrm{curr}},g_{\mathrm{curr}}),$$

  where $q_{\mathrm{goal}}$ is a joint-space subgoal (typically near the next toggle) that serves as the target for the upcoming phase.

  (b)

  Diffusion-guided RRT/RRT-Connect. Initialize an RRT/RRT-Connect tree T with root $q_{\mathrm{curr}}$ and grow it toward $q_{\mathrm{goal}}$ for up to $N_{\mathrm{rrt}}$ expansions. At each expansion, obtain a learned segment $(u,s)$ from the diffusion sampler $D_{\theta }$ conditioned on $(I_{\mathrm{curr}},q_{\mathrm{curr}},q_{\mathrm{goal}},q_{\mathrm{toggle}},g_{\mathrm{toggle}})$ (and an optional tree summary), and with probability $p_{\mathrm{uni}}$ replace it with a UniformSegment(). Each proposal is converted to $q_{\mathrm{new}}$ via (4) and accepted only if CollisionFree holds. The phase terminates when a node v in the tree satisfies $d(v,q_{\mathrm{goal}})\le {\tau }_{\mathrm{goal}}$ or the iteration limit $N_{\mathrm{rrt}}$ is reached.

  (c)

  Path extraction and execution. Extract a path from $q_{\mathrm{curr}}$ to the best-reached node near $q_{\mathrm{goal}}$, apply shortcut smoothing and spline-based time-parameterization under joint limits, and validate the resulting trajectory with dense collision checks. Execute this trajectory on the robot (or in simulation), toggling the gripper to $g_{\mathrm{toggle}}$ when the trajectory passes near $q_{\mathrm{toggle}}$. The final state of the executed trajectory becomes the new $(q_{\mathrm{curr}},g_{\mathrm{curr}})$ for the next phase.

• The overall motion is obtained by concatenating the phase trajectories until $q_{\mathrm{curr}}\in {\mathcal{Q}}_{\mathrm{term}}$ or a global time/iteration limit is exceeded.

5. Experimental Results

Our evaluations are based entirely on simulations of a single contact-rich benchmark—uprighting and placing a mug—chosen to stress correct grasp/release timing. Our primary outcome is success rate. In addition, we report (i) per proposal inference time for the learned samplers, (ii) episode-level time-to-first-solution (TTFS), defined as the total wall-clock planning time accumulated over all short planning phases in an episode from the initial state until a trajectory reaching the terminal region ${\mathcal{Q}}_{\mathrm{term}}$ is found (planner overhead included), and (iii) an efficiency comparison within RRT-style planners using the number of RRT expansions (“RRT steps”) normalized to our method. Unless noted otherwise, all search-based methods use identical random seeds, iteration limits, nearest-neighbor metrics, steering procedures, collision checking, and post-processing (shortcut smoothing plus spline time-parameterization).

5.1. Simulation Setup and Data Collection

Environment. All experiments are conducted in PyBullet (v3.2.7) using a 7-DOF Franka Emika Panda arm equipped with a parallel-jaw gripper. We implement all deep learning components in PyTorch (v2.9.1). RGB images are rendered from a single fixed eye-in-hand–style camera and resized to $88\times 136$ pixels.

Task. We consider a mug manipulation task with two contact events: (1) From a side-lying pose, grasp near the rim (grasp₁), upright and place (release₁); (2) Re-approach, grasp the handle (grasp₂), transport, and place the mug inside a target box (release₂). This task stresses correct contact reasoning; grasping inappropriate regions typically yields slip or collision.

We deliberately design the benchmark to involve two distinct contact modes: an initial rim grasp used to establish the upright position of the mug and stabilize it, followed by a handle grasp for transport and final placement. The second stage is only attempted if the first rim grasp and uprighting succeed, making the task a sequential, contact-rich benchmark rather than a single pick-and-place.

Demonstrations and labels. Expert rollouts are generated with bidirectional RRT (biRRT) plus scripted grasp heuristics in joint space. From each rollout, we record $(I_{0:T},q_{0:T},g_{0:T})$, annotate the four toggle events (grasp₁, release₁, grasp₂, release₂), and extract approach the subgoals used as intermediate waypoints. A total of 2000 rollouts are used for training; all data are collected via simulation.

Subgoals $q_{\mathrm{goal}}^{★}$ are chosen as pre-contact approach waypoints along the expert trajectory. If such a waypoint is already very close (within a small joint-space tolerance) to $q_{\mathrm{toggle}}^{★}$, we simply set $q_{\mathrm{goal}}^{★}=q_{\mathrm{toggle}}^{★}$ for that phase.

Network I/O parameterization. Joint angles are normalized (radians) to $[-1,1]$. When an orientation target is needed, we adopt the continuous 6D rotation representation of Zhou et al. [22] to avoid discontinuities. (joint variables remain scalars; the 6D representation is used only for $\mathrm{SO}\left(3\right)$ targets/features).

Training/inference hyperparameters. The upper network (ResNet backbone + MLP heads) is trained with Equation (1) using loss weights $w_q=2.0$, $w_g=1.0$, $w_s=1.0$, $w_r={10}^{-4}$, a Huber threshold $\kappa =2.0$, and regularization coefficients ${\lambda }_{\lim }=1.0$ and ${\lambda }_{\mathrm{prox}}=0.5$. The lower sampler is a conditional diffusion model (1D U-Net) trained with Equation (5). For diffusion, training uses $T=100$ noise steps with a cosine schedule; evaluation uses a deterministic DDIM sampler [23] with $T=25$ steps (and $T=10$ in the ablations). Unless otherwise noted, the RRT mixture uses $p_{\mathrm{uni}}=0.2$ (uniform-segment probability), and the step length is clipped to $s\in [0.05,0.2]$ before forming proposals via (4).

5.2. Qualitative Overview of the Proposed Method

Figure 2 shows two major phases (grasp and release) and illustrates how the learned sample endpoints (translucent blue dots) concentrate along task-relevant approach corridors while the tree grows toward the predicted subgoal. Building on this, Figure 3 walks through a single episode from the first grasp to final placement, showing how the phase-wise receding-horizon updates keep $q_{\mathrm{goal}}^{\mathrm{pred}}$ close to the currently relevant toggle for that phase. Finally, Figure 4 demonstrates consistency across randomized scenes: in three different initializations, the learned proposal distribution remains phase-appropriate (rim → release → handle → terminal region) while adapting to pose variations.

5.3. Evaluation Protocol and Metrics

Each episode randomizes the mug SE(2) pose on the table (with small roll/pitch); the arm starts from a fixed home pose with an open gripper. Unless otherwise noted, each comparison is evaluated over 300 randomized episodes. We consider two complementary comparisons, each with its own metric and baselines:

• Comparison 1 (Primary): Success rate. End-to-end imitation policies (no explicit search or collision checking) versus our search-based planner.
• Comparison 2 (Efficiency within the RRT family): normalized RRT expansions. RRT-style planners that share the same collision checker are compared by the number of expansions relative to ours, along with associated runtime metrics (per-proposal time and episode-level TTFS).

Unless otherwise noted, all RRT-family planners (ours, uniform RRT, goal-biased RRT, CVAE, conditional GAN) use the same per-phase iteration limit $N_{\mathrm{rrt}}$. This budget was chosen via preliminary sweeps to be large enough that success rates have essentially saturated on our mug benchmark; increasing $N_{\mathrm{rrt}}$ further mainly increases computation while providing diminishing returns in success. As in standard sampling-based motion planning, $N_{\mathrm{rrt}}$ therefore controls the usual trade-off between planning reliability and computational cost, but our comparative conclusions are drawn under a fixed, shared $N_{\mathrm{rrt}}$ across all methods.

5.4. Comparison 1: Success Rate vs. Imitation Policies

We compare our search-based planner to three imitation (no-search) policies evaluated only by success rate: (i) A Diffusion Policy baseline [11] adapted to our setting (inputs $(I,q_{\mathrm{curr}},g_{\mathrm{curr}})$), (ii) An LSTM–GMM policy following Mandlekar et al. [24], and (iii) a Transformer-based policy [25]. All imitation policies take the same inputs $(I,q_{\mathrm{curr}},g_{\mathrm{curr}})$ and predict a short 16-step action rollout. At test time, we execute the first eight predicted steps and then re-query the policy in a receding-window fashion until termination; this preserves closed-loop operation and mitigates long-horizon open-loop drift across policies. Because these methods do not expose RRT expansions or explicit collision checking, success rate is the only fair metric here.

Our implementation of Diffusion Policy adheres to the public codebase as closely as possible (network architecture, loss, and training procedure), but uses a single RGB camera matching our simulation setup rather than the multi-camera configuration in the original work. Consequently, the absolute performance numbers are not directly comparable to those reported in [11], but this baseline still represents a strong, modern diffusion-based imitation policy for our mug manipulation benchmark. The LSTM–GMM and Transformer baselines are re-tuned for our task to avoid strawman comparisons.

Table 1. Comparison 1 (success over 300 episodes). Imitation learning (IL) baselines are no-search policies; to keep evaluation closed-loop and mitigate long-horizon open-loop drift, each policy executes only a short prefix of its predicted sequence before re-planning from new observations. All methods share the same perception setup and task specification.

Method	Success (%)
Diffusion Policy (IL)	89.33
LSTM–GMM (IL)	69.67
Transformer (IL)	60.33
Ours (Diffusion Sampler + Toggle/Subgoal)	95.67

reports results over 300 episodes. Our method achieves the highest success rate by combining (a) a learned, contact-aware sampling distribution with (b) explicit collision checking inside RRT. Consequently, unless the upper network provides severely misleading guidance, the planner remains robust to moderate perception errors and local misproposals. Among the imitation baselines, the Diffusion Policy is competitive but still trails our planner; the LSTM–GMM and Transformer policies underperform due to error accumulation across phases that require sequential contact events (rim grasp → upright release → handle grasp → final placement). In such settings, pure imitation struggles to recover from compounding deviations, whereas our planner’s search component provides corrective exploration and safety via collision checks.

5.5. Comparison 2: Normalized RRT Expansions Within the RRT Family

We now restrict attention to planners that share the same SBMP backbone and collision checker and report normalized RRT expansions (number of tree expansions) relative to ours (To factor out execution differences, normalization is computed on successful episodes only.). In all comparisons, we replace only the lower planner while keeping the upper network fixed. Baselines include (i) Uniform RRT. (ii) Goal-biased RRT that samples the subgoal $q_{\mathrm{goal}}$ with a probability of $0.5$. (iii) A CVAE-based sampler [9] following Ichter et al. [4], and (iv) a conditional GAN sampler [10]. All methods use identical nearest-neighbor metrics, steering, collision checking, iteration limits, goal tolerance ${\tau }_{\mathrm{goal}}$, and the same smoothing/time-parameterization pipeline. Results (500 trials; averages taken over successful episodes) are reported in

Table 2. Comparison 2 (efficiency within the RRT family). Expansions are normalized to our method (lower is better). Values are averaged over successful episodes only. All learned samplers share identical conditioning signals and the same $p_{\mathrm{uni}}$. Our approach achieves the lowest normalized expansion count under identical conditioning and mixture settings.

RRT Family Method	Normalized RRT Expansions
Ours (Diffusion Sampler + Toggle/Subgoal)	1.00
Uniform RRT	2.75
Goal-biased RRT (0.5 to $q_{\mathrm{goal}}$)	1.73
CVAE Sampler + RRT	1.36
Cond. GAN Sampler + RRT	1.67

.

The learned samplers (CVAE, conditional GAN, and ours) receive the same conditioning signals and mixture probability, $\mathrm{cond}=(I,q_{\mathrm{curr}},q_{\mathrm{goal}},q_{\mathrm{toggle}},g_{\mathrm{toggle}})$ with identical $p_{\mathrm{uni}}$. Hence, differences arise due to the quality of the learned proposal distribution, rather than due to differing inputs or mixture weights.

Because diffusion denoising requires multiple steps, the learned proposal generators have different per-proposal runtimes across model families. We therefore report the average inference time per proposal and a normalized time with DDIM ($T=25$ steps) set to $1.00$. Timings are measured with batch size 1 using PyTorch in FP32 and exclude collision checking, nearest-neighbor, and other planner overheads. The results are averaged over 500 proposal evaluations per method under identical hardware/software settings. As shown in

Table 3. Average inference time per learned proposal (planner overhead excluded). Times are normalized to DDIM $T=25=1.00$ and averaged over 500 proposals per method.

Method	Normalized Time
Ours (Diffusion, DDIM $T=25$)	1.00
Ours (Diffusion, DDIM $T=10$)	0.42
CVAE sampler	0.09
Conditional-GAN sampler	0.11

, reducing DDIM steps (e.g., from $T=25$ to $T=10$) provides an approximately linear speedup, while feed-forward generators (CVAE/conditional GAN) are substantially faster per proposal. In practice, for deployment scenarios where per-proposal latency is critical, a shorter schedule such as DDIM with $T=10$ can be a reasonable trade-off.

To assess end-to-end planning latency, we report an episode-level time-to-first-solution (TTFS), defined as the total wall-clock planning time accumulated over all planning phases in an episode until a feasible trajectory that reaches the terminal region ${\mathcal{Q}}_{\mathrm{term}}$ is found (

Table 4. End-to-end, episode-level TTFS. Sum of planning time across all phases (planner overhead included), averaged over 500 successful episodes. Each column is independently normalized so that DDIM ($T=25$) equals 1.00; values are not comparable across columns.

Method	Normalized TTFS (per ${\mathit{p}}_{\mathbf{uni}}$ Setting)
	${\mathit{p}}_{\mathbf{uni}}\mathbf{=}\mathbf{0}.\mathbf{2}$	${\mathit{p}}_{\mathbf{uni}}\mathbf{=}\mathbf{0}.\mathbf{6}$
Ours (Diffusion, DDIM $T=25$)	1.00	1.00
Ours (Diffusion, DDIM $T=10$)	0.45	0.49
CVAE sampler	0.16	0.24
Conditional-GAN sampler	0.20	0.27

). TTFS includes planner overhead (nearest-neighbor search, collision checking, etc.) and is averaged over 500 successful episodes. For each $p_{\mathrm{uni}}$ setting, values are normalized independently, with DDIM ($T=25$) taken as the 1.00 baseline.

While diffusion-based sampling improves search efficiency in terms of normalized RRT expansions, it incurs a higher absolute per-proposal cost due to its multi-step denoising process. Consequently, the end-to-end, episode-level time to first solution (TTFS) may be worse than that of feed-forward samplers under identical settings. Optimizing wall-clock latency is outside the main scope of this work; however, we observe that simple schedule reductions (e.g., DDIM with fewer steps, such as $T=10$) yield an approximately linear speedup with only modest degradation in planning performance. This provides a practical tuning parameter when per-proposal latency is critical.

All of the experiments in this work are conducted via simulation, and we do not claim hard real-time guarantees on any specific robotic platform. As shown in Table 3 and Table 4, the diffusion-based sampler incurs a higher per-proposal cost than feed-forward samplers, so the method is not intended for high-frequency inner-loop control (e.g., 10–100 Hz torque or velocity control). Instead, our framework targets task-level or episodic planning, where a motion plan for a multi-second manipulation is computed and then executed by a lower-level controller. Once a trajectory is generated, execution only requires standard interpolation and tracking and does not involve running the diffusion model in the control loop. In this regime, planning latency on the order of seconds can be acceptable for static or slowly changing environments. A detailed, platform-specific real-time characterization on physical robot hardware is left for future work.

5.6. Qualitative Comparison of Learned Sample Distributions

Figure 5 contrasts the learned proposal distributions near contact-heavy scenes. In all panels, blue dots show only the endpoints proposed by each model’s learned sampler (candidate tree–extension targets; uniform samples are omitted). Our diffusion sampler concentrates mass along approach corridors to the rim and handle while preserving useful diversity, whereas CVAE/CGAN often allocate probability to task-irrelevant regions (marked with green dashed example arcs).

5.7. Robustness to Viewpoint and Illumination

While our main study is conducted via simulation, we assess the sensitivity of the toggle–subgoal predictor to camera pose and photometric shifts by perturbing the rendered image with (i) viewpoint jitter (yaw/pitch rotations and small per-axis translations) and (ii) photometric jitter (

Table 5. Robustness to viewpoint and illumination (50 episodes per setting). $\alpha$: per-axis yaw/pitch bound; $\tau$: per-axis translation bound; $\sigma$: std. of additive image noise. Toggle err is the mean joint-space distance (in radians) between the predicted toggle configuration and the nearest ground-truth grasp/release toggle.

Perturbation	$\mathit{\alpha }$ (deg)	$\mathit{\tau }$ (m)	$\mathit{\sigma }$	Success (%)	Toggle Err (Rad)
Nominal	0	0.00	0.000	96	0.0112
Viewpoint (mild)	5	0.01	0.000	88	0.0201
Viewpoint (strong)	10	0.02	0.000	82	0.0285
Photometric	0	0.00	0.020	90	0.0197

). For the latter, additive Gaussian noise with standard deviation $\sigma =0.02$ is applied to images whose pixel values are scaled to $[0,1]$, followed by clamping back to $[0,1]$.

We perturb the camera extrinsics with independent, zero-mean, per-axis jitter: yaw and pitch are sampled uniformly from $[-\alpha ,\alpha ]$ degrees (with roll fixed to $0^{\circ }$ for all tests), and translations along the camera $(x,y,z)$ axes are sampled uniformly from $[-\tau ,\tau ]$ meters. Unless otherwise noted, we set $(\alpha ,\tau )=(5^{\circ },0.01)$ for the mild setting and $({10}^{\circ },0.02)$ for the stronger setting. After updating extrinsics, we re-render the RGB image and feed it to the upper network. The planner keeps the same mixture setting ($p_{\mathrm{uni}}=0.2$) and re-queries the upper network in a receding-horizon manner.

Because the method estimates the toggle configuration from camera input, severe appearance changes can degrade the upper network’s predictions. In practice, periodic re-querying (receding horizon) together with a nonzero fraction of uniform samples helps us to recover from transient mispredictions by recentering the search after a few inner iterations.

We report both the success rate and the mean joint-space toggle error. Here, Toggle err denotes the mean joint-space distance (in radians) between the predicted toggle configuration $q_{\mathrm{toggle}}^{\mathrm{pred}}$ and the nearest ground-truth toggle configuration (either grasp or release) for that episode.

Overall, the nominal setting achieves 96% success in this 50-episode study; success degrades to 88% under mild viewpoint jitter and 82% under the stronger jitter, with photometric noise alone yielding 90% success. These results indicate a graceful degradation of toggle–subgoal predictions under moderate viewpoint and illumination shifts, rather than catastrophic failure.

5.8. Limitations and Practical Considerations

Our evaluation is simulation-based. Deploying the method on real hardware also requires (i) calibrated sensing (e.g., camera/hand–eye), (ii) perception robustness to viewpoint and illumination changes, (iii) verified robot–scene geometry with enforced joint/velocity limits for collision checking, and (iv) profiling of the full planning loop to satisfy cycle-time and safety constraints. While our sampler is designed to be interface-compatible with the RRT/RRT-Connect sampling module, a thorough hardware study—covering calibration, safety checks, and latency budgeting (e.g., shorter DDIM schedules)—will be conducted in future work.

6. Conclusions

We presented a two-level, contact-aware sampling-based motion-planning framework for robotic manipulation. An upper network predicts a gripper toggle configuration, its mode (open/close), and a joint-space subgoal from a single RGB image and the current robot state. A lower planner then runs RRT/RRT-Connect with a conditional diffusion sampler that proposes tree-extension segments aligned with these contact cues, while mixing in a nonzero fraction of uniform samples to retain probabilistic completeness. This receding-horizon coupling injects grasp/release semantics into sampling without modifying standard nearest-neighbor, steering, or collision-checking primitives.

On a contact-rich mug benchmark, our method achieves higher success rates than imitation-learning baselines and requires fewer RRT expansions than classical and prior learning-guided samplers by concentrating proposals along contact-relevant approach manifolds. The resulting efficiency gains come at the cost of higher per-proposal compute due to diffusion denoising, but simple schedule reductions provide a practical latency–performance trade-off at test time.

Our study is limited to simulations, a single robot, and one manipulation family, and we do not yet provide hardware-level real-time guarantees. A real-robot evaluation and a more detailed, platform-specific latency characterization are therefore important directions for future work. Additional extensions include a transfer to hardware via domain randomization, scaling to multi-task and multi-object settings, incorporating uncertainty estimates to adapt the uniform mixture rate online, leveraging richer perceptual inputs (e.g., depth, multi-view, affordance cues), and extending the framework to asymptotic-optimal variants (e.g., RRT*) and dynamic scenes.