[Bug]: `shortcut_path` crash in `plan_path` with `smooth_path=True` (0.4.0 regression)

[udayanbb]

Bug Description

plan_path() with smooth_path=True crashes about 4 out of 5 runs with a torch.AcceleratorError. It's a regression in 0.4.0 — same code works fine on 0.3.11.

The issue is that shortcut_path calls torch.multinomial(..., 2) which needs at least 2 valid entries to sample from. When RRTConnect happens to find a very short path (< 3 nodes), there aren't enough entries and the call fails. There's no guard for this case.

torch.AcceleratorError: index -2 is out of bounds: 0, range 0 to 1

File ".../genesis/utils/path_planning.py", line 282, in shortcut_path
    ind = torch.multinomial(path_mask.T, 2).sort().values.to(gs.tc_int)  # B, 2

Steps to Reproduce

"""Crashes ~4 out of 5 runs on Genesis 0.4.0. Works on 0.3.11."""
import genesis as gs
import numpy as np

gs.init()

scene = gs.Scene(show_viewer=False)
scene.add_entity(gs.morphs.Plane())
franka = scene.add_entity(
    gs.morphs.MJCF(file="xml/franka_emika_panda/panda.xml")
)
scene.add_entity(gs.morphs.Cylinder(pos=(0.4, 0, 0.05), radius=0.03, height=0.02))
scene.add_entity(
    gs.morphs.Cylinder(
        pos=(0.3, 0.2, 0.05), radius=0.005, height=0.05, fixed=True
    )
)
scene.build()

franka.set_dofs_kp(
    kp=np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100])
)
franka.set_dofs_kv(kv=np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]))

ee = franka.get_link("hand")
target_pos = np.array([0.4, 0.0, 0.10])
gripper_down = np.array([0, 1, 0, 0])
seed = np.array(
    [-1.238, -0.127, 2.128, -0.699, -0.824, 0.245, -0.368, 0.04, 0.04]
)
qpos_goal = franka.inverse_kinematics(
    link=ee, pos=target_pos, quat=gripper_down, init_qpos=seed
)

# Crashes ~80% of the time on 0.4.0
path = franka.plan_path(
    qpos_goal.cpu().numpy(), num_waypoints=200, smooth_path=True
)
print(f"OK: path shape = {path.shape}")

The non-determinism comes from RRT's random sampling — when the trees connect in very few nodes, the resulting path is too short for shortcut_path to handle.

Root Cause

In path_planning.py, plan() (line 1012) passes the RRT solution into shortcut_path():

sol = self.shortcut_path(
    torch.ones_like(sol[..., 0]),  # path_mask: shape [N, B], all ones
    sol,                            # path: shape [N, B, DoF]
    iterations=10,
    ...
)

Inside shortcut_path() (line 282):

ind = torch.multinomial(path_mask.T, 2).sort().values.to(gs.tc_int)  # B, 2

path_mask.T has shape [B, N]. torch.multinomial(input, 2) needs each row to have at least 2 nonzero entries. When the RRT path has N < 2 valid waypoints, this fails — there's no bounds check.

Suggested fix — just skip shortcutting for short paths:

def shortcut_path(self, path_mask, path, iterations=50, ...):
    if path.shape[0] < 3:  # need at least 3 waypoints to shortcut
        return path
    for i in range(iterations):
        ind = torch.multinomial(path_mask.T, 2).sort().values.to(gs.tc_int)
        ...

Expected Behavior

plan_path(smooth_path=True) should work regardless of how short the RRT path is. If the path is too short to shortcut, just return it as-is.

Screenshots/Videos

No response

Relevant log output

Environment

• OS: macOS (Darwin 25.2.0, Apple Silicon)
• Python: 3.10.11
• PyTorch: 2.10.0
• Backend: MPS (also reproduced on CPU)

Release version or Commit ID

• Affected: Genesis 0.4.0
• Works on: Genesis 0.3.11

Additional Context

No response