Motion Planning

Motion planning for one entity of a built env: a planner takes a goal in its backend's form (joint space or task space) and returns joint-space waypoints. A planner is a MotionPlannerBase subclass registered in MOTION_PLANNER_REGISTRY and configured by PlannerOptions; two backends ship, cuRobo and OMPL. Each imports its library lazily and reports whether it is installed (IS_CUROBO_AVAILABLE, IS_OMPL_AVAILABLE). The planning and sysid guide shows them in use.

from eden.extensions.motion_planning.base import MOTION_PLANNER_REGISTRY, MotionPlannerBase
from eden.options.extensions.motion_planning import PlannerOptions

Base class and registry

Base class for motion planners.

Attributes

MOTION_PLANNER_REGISTRY module-attribute

MOTION_PLANNER_REGISTRY = Registry('MOTION_PLANNER')

Classes

MotionPlannerBase

MotionPlannerBase(env: EnvBase, options: PlannerOptions)

Bases: ConfigurableMixin[PlannerOptions]

Abstract base class for motion planners.

Subclasses provide backend-specific goal representations (joint-space, task-space, etc.) and return planned waypoints in joint space. The shared option entity_name names the entity to plan motions for (resolved at construction).

PARAMETER	DESCRIPTION
env	The environment instance. TYPE: EnvBase
options	From the backend's configure(). TYPE: PlannerOptions

Source code in eden/extensions/motion_planning/base.py

def __init__(self, env: EnvBase, options: PlannerOptions) -> None:
    super().__init__(options)
    self.env = env
    self._entity = resolve_named_entity(
        env.entities, self.entity_name, hint="PlannerOptions.entity_name names it; set it to one of those."
    )

Attributes

entity_name class-attribute instance-attribute

entity_name: str = ''

env instance-attribute

env = env

entity property

entity: Entity

allow_batch property

allow_batch: bool

Whether the planner allows batch processing.

Methods:

plan_motion abstractmethod

plan_motion(**kwargs: Any) -> list[Tensor]

Plan motion to a target configuration.

Subclasses define their own goal specification (joint-space, task-space, etc.) via keyword arguments.

RETURNS	DESCRIPTION
list[Tensor]	Planned waypoints in joint space. The layout is backend-specific: one (num_waypoints, n_dof) tensor per environment (cuRobo), or one (n_qs,) tensor per waypoint of a single environment (OMPL).

Source code in eden/extensions/motion_planning/base.py

@abstractmethod
def plan_motion(self, **kwargs: Any) -> list[torch.Tensor]:
    """Plan motion to a target configuration.

    Subclasses define their own goal specification (joint-space, task-space, etc.)
    via keyword arguments.

    Returns
    -------
    list[torch.Tensor]
        Planned waypoints in joint space. The layout is backend-specific: one
        ``(num_waypoints, n_dof)`` tensor per environment (cuRobo), or one ``(n_qs,)`` tensor
        per waypoint of a single environment (OMPL).
    """
    pass

Functions:

Options

Configuration options for motion-planning extensions.

Classes

PlannerOptions

PlannerOptions(**data)

Bases: DynamicConfigurableOptions

Options for motion planning extensions.

PARAMETER	DESCRIPTION
entity_name	The name of the entity to plan motions for

Source code in eden/options/options.py

def __init__(self, **data):
    # Seed type metadata as construction data so BaseModel validates it into
    # __pydantic_extra__ (no-op for extra="forbid" classes, which use
    # the serialization envelope instead).
    data.update(option_metadata(self))
    # NOTE: all of the option modifications/validations should be done in the __init__ method
    try:
        BaseModel.__init__(self, **data)
    except ValidationError as exc:
        # A misspelled field on an extra="forbid" class should teach the field set, not just
        # name the typo. Only rewritten when every error is extra_forbidden.
        enriched = _enrich_extra_forbidden(type(self), exc)
        if enriched is None:
            raise
        raise enriched from None

Attributes

entity_name class-attribute instance-attribute

entity_name: str = 'robot'

cuRobo

cuRobo-based motion planner.

Attributes

IS_CUROBO_AVAILABLE module-attribute

IS_CUROBO_AVAILABLE = True

Classes

CuroboPlanner

CuroboPlanner(env: EnvBase, options: PlannerOptions)

Bases: MotionPlannerBase

Motion planner using cuRobo for collision-aware trajectory generation.

Options (besides the shared entity_name):

• robot_config_file (str): path to the robot configuration YAML file (cuRobo format).
• ee_link_name (str): the end-effector link.
• collision_entities_name (list[str] | None): entities whose geoms are collision obstacles.
• interpolation_dt (float): time step, in seconds, between interpolated waypoints.

PARAMETER	DESCRIPTION
env	The environment instance. TYPE: EnvBase
options	From CuroboPlanner.configure(). TYPE: PlannerOptions

Source code in eden/extensions/motion_planning/curobo/planner.py

def __init__(
    self,
    env: EnvBase,
    options: PlannerOptions,
) -> None:
    if not IS_CUROBO_AVAILABLE:
        raise ImportError(
            "curobo is not installed. Please see `https://curobo.org/get_started/1_install_instructions.html` for installation instructions."
        )

    super().__init__(env, options)

    if self.collision_entities_name is None:
        self.collision_entities_name = []

    _, ee_link_idx = self.entity.find_named_links_idx_local(self.ee_link_name)
    self.ee_link_idx = ee_link_idx[0]

    self._dtype = torch.float32  # curobo internally only supports fp32
    self._tensor_args = TensorDeviceType(device=env.device, dtype=torch.float32)

    self._collision_entities = [self.env.entities[name] for name in self.collision_entities_name]

    self._init_robot_config()
    self._motion_gen: MotionGen | None = None
    self._world_model_list: list[WorldConfig] | None = None

Attributes

robot_config_file class-attribute instance-attribute

robot_config_file: str = ''

ee_link_name class-attribute instance-attribute

ee_link_name: str = ''

collision_entities_name class-attribute instance-attribute

collision_entities_name: list[str] | None = None

interpolation_dt class-attribute instance-attribute

interpolation_dt: float = 0.05

ee_link_idx instance-attribute

ee_link_idx = ee_link_idx[0]

allow_batch property

allow_batch: bool

Whether the planner allows batch processing.

Methods:

update_world

update_world(carried_entities: list[Entity] | None = None, num_envs: int = 1, envs_idx: int | slice | Tensor | None = None) -> None

Rebuild the collision world and motion generator.

Call this before plan_motion when the scene has changed.

Source code in eden/extensions/motion_planning/curobo/planner.py

def update_world(
    self,
    carried_entities: list[Entity] | None = None,
    num_envs: int = 1,
    envs_idx: int | slice | torch.Tensor | None = None,
) -> None:
    """Rebuild the collision world and motion generator.

    Call this before plan_motion when the scene has changed.
    """
    if carried_entities is None:
        carried_entities = []
    self._world_model_list = self._build_world_model(carried_entities, num_envs, envs_idx)
    self._motion_gen = self._build_motion_gen(self._world_model_list)

plan_motion

plan_motion(ee_goal_pos: Tensor, ee_goal_quat: Tensor, qpos_start: Tensor | None = None, envs_idx: int | slice | Tensor | None = None) -> list[Tensor]

Plan a collision-free motion to the target end-effector pose.

PARAMETER	DESCRIPTION
ee_goal_pos	Target end-effector position in the world frame, shape (num_envs, 3). TYPE: Tensor
ee_goal_quat	Target end-effector quaternion (wxyz) in the world frame, shape (num_envs, 4). TYPE: Tensor
qpos_start	Start joint configuration. Defaults to current entity qpos. TYPE: Tensor DEFAULT: None
envs_idx	Environment indices to plan for. Defaults to every environment. TYPE: int | slice | Tensor | None DEFAULT: None

RETURNS	DESCRIPTION
list[Tensor]	Planned waypoints for each environment, each of shape (num_waypoints, n_dof). An environment whose plan failed gets its start configuration as a single waypoint, and every environment gets an empty (0, n_dof) tensor when no plan succeeded.

Source code in eden/extensions/motion_planning/curobo/planner.py

def plan_motion(
    self,
    ee_goal_pos: torch.Tensor,
    ee_goal_quat: torch.Tensor,
    qpos_start: torch.Tensor | None = None,
    envs_idx: int | slice | torch.Tensor | None = None,
) -> list[torch.Tensor]:
    """Plan a collision-free motion to the target end-effector pose.

    Parameters
    ----------
    ee_goal_pos : torch.Tensor
        Target end-effector position in the world frame, shape (num_envs, 3).
    ee_goal_quat : torch.Tensor
        Target end-effector quaternion (wxyz) in the world frame, shape (num_envs, 4).
    qpos_start : torch.Tensor, optional
        Start joint configuration. Defaults to current entity qpos.
    envs_idx : int | slice | torch.Tensor | None, optional
        Environment indices to plan for. Defaults to every environment.

    Returns
    -------
    list[torch.Tensor]
        Planned waypoints for each environment, each of shape (num_waypoints, n_dof). An
        environment whose plan failed gets its start configuration as a single waypoint, and
        every environment gets an empty ``(0, n_dof)`` tensor when no plan succeeded.
    """
    if envs_idx is None:
        envs_idx = slice(None)
    if isinstance(envs_idx, slice):
        num_envs = len(range(*envs_idx.indices(self.env.num_envs)))
    elif isinstance(envs_idx, torch.Tensor):
        num_envs = envs_idx.shape[0]
    else:
        num_envs = 1

    if qpos_start is None:
        qpos_start = self.entity.get_qpos(envs_idx=envs_idx)

    ee_goal_robot_frame = self._compute_ee_goal_robot_frame(ee_goal_pos, ee_goal_quat, num_envs, envs_idx)

    if self._motion_gen is None:
        self.update_world(num_envs=num_envs, envs_idx=envs_idx)

    solved, all_waypoints = self._plan_path_batch(
        qpos_start[:, : self._n_dof],
        ee_goal_robot_frame,
        self._motion_gen,
        input_dtype=qpos_start.dtype,
        num_envs=num_envs,
    )

    return all_waypoints

OMPL

OMPL-based motion planner.

Attributes

IS_OMPL_AVAILABLE module-attribute

IS_OMPL_AVAILABLE = True

OMPL_BOOST_PYTHON module-attribute

OMPL_BOOST_PYTHON = IS_OMPL_AVAILABLE and hasattr(ob, 'StateValidityCheckerFn')

Classes

OMPLPlanner

OMPLPlanner(env: EnvBase, options: PlannerOptions)

Bases: MotionPlannerBase

Motion planner backed by OMPL.

Plans in the entity's full qpos space for one environment at a time, checking collisions by setting the entity's qpos in the scene.

PARAMETER	DESCRIPTION
env	The environment instance. TYPE: EnvBase
options	From OMPLPlanner.configure(). TYPE: PlannerOptions

Source code in eden/extensions/motion_planning/ompl/planner.py

def __init__(self, env: EnvBase, options: PlannerOptions) -> None:
    super().__init__(env, options)

    if not IS_OMPL_AVAILABLE:
        raise ImportError(
            "OMPLPlanner needs OMPL's Python bindings, which are not installed. Install them with "
            "`pip install ompl` (OMPL 1.7 and 2.x both work). Note that OMPL is not distributed on Windows OS."
        )

Methods:

plan_motion

plan_motion(qpos_goal: Tensor, qpos_start: Tensor | None = None, resolution: float = 0.01, timeout: float = 5.0, max_retry: int = 1, smooth_path: bool = True, num_waypoints: int = 100, ignore_collision: bool = False, planner: str = 'RRTConnect', envs_idx: int = 0) -> list[Tensor]

Plan a collision-free joint-space path from the start to the goal configuration.

A start or goal outside the joint limits widens the planning bounds to contain it (with a warning), and collision pairs already active in the scene when it is called are ignored throughout. The entity's qpos is set to qpos_start before returning.

PARAMETER	DESCRIPTION
qpos_goal	The goal configuration of the entity, shape (n_qs,). TYPE: Tensor
qpos_start	The start configuration, shape (n_qs,). None uses the entity's current qpos. TYPE: Tensor | None DEFAULT: None
resolution	State-validity checking resolution along a motion, as OMPL's setStateValidityCheckingResolution takes it (a fraction of the space's extent). TYPE: float DEFAULT: 0.01
timeout	Time budget of one solve attempt, in seconds. TYPE: float DEFAULT: 5.0
max_retry	The maximum number of solve attempts while the planner times out or finds only an approximate solution. TYPE: int DEFAULT: 1
smooth_path	Whether to shortcut and B-spline-smooth the path. TYPE: bool DEFAULT: True
num_waypoints	The number of waypoints the path is interpolated to. TYPE: int DEFAULT: 100
ignore_collision	Whether to skip collision checking. TYPE: bool DEFAULT: False
planner	The name of an ompl.geometric planner class, e.g. "RRTConnect". TYPE: str DEFAULT: 'RRTConnect'
envs_idx	The environment index. This planner does not support batch processing. TYPE: int DEFAULT: 0

RETURNS	DESCRIPTION
waypoints	One (n_qs,) configuration per waypoint; empty when planning is infeasible or no path was found. TYPE: list[Tensor]

Source code in eden/extensions/motion_planning/ompl/planner.py

def plan_motion(
    self,
    qpos_goal: torch.Tensor,
    qpos_start: torch.Tensor | None = None,
    resolution: float = 0.01,
    timeout: float = 5.0,
    max_retry: int = 1,
    smooth_path: bool = True,
    num_waypoints: int = 100,
    ignore_collision: bool = False,
    planner: str = "RRTConnect",
    envs_idx: int = 0,
) -> list[torch.Tensor]:
    """Plan a collision-free joint-space path from the start to the goal configuration.

    A start or goal outside the joint limits widens the planning bounds to contain it (with a
    warning), and collision pairs already active in the scene when it is called are ignored
    throughout. The entity's ``qpos`` is set to ``qpos_start`` before returning.

    Parameters
    ----------
    qpos_goal : torch.Tensor
        The goal configuration of the entity, shape ``(n_qs,)``.
    qpos_start : torch.Tensor | None
        The start configuration, shape ``(n_qs,)``. ``None`` uses the entity's current ``qpos``.
    resolution : float
        State-validity checking resolution along a motion, as OMPL's
        ``setStateValidityCheckingResolution`` takes it (a fraction of the space's extent).
    timeout : float
        Time budget of one solve attempt, in seconds.
    max_retry : int
        The maximum number of solve attempts while the planner times out or finds only an
        approximate solution.
    smooth_path : bool
        Whether to shortcut and B-spline-smooth the path.
    num_waypoints : int
        The number of waypoints the path is interpolated to.
    ignore_collision : bool
        Whether to skip collision checking.
    planner : str
        The name of an ``ompl.geometric`` planner class, e.g. ``"RRTConnect"``.
    envs_idx : int
        The environment index. This planner does not support batch processing.

    Returns
    -------
    waypoints : list[torch.Tensor]
        One ``(n_qs,)`` configuration per waypoint; empty when planning is infeasible or no
        path was found.
    """
    if timeout <= 0.0 or not math.isfinite(timeout):
        raise ValueError("Timeout must be a positive finite number.")
    if max_retry <= 0:
        raise ValueError("Max retry must be a positive integer.")
    planner_cls = _ompl_planner_class(planner)

    if qpos_start is None:
        qpos_start = self.entity.get_qpos(envs_idx=envs_idx).squeeze(0)
    qpos_start = tensor_to_array(qpos_start)
    qpos_goal = tensor_to_array(qpos_goal)

    if qpos_start.shape != (self.entity.n_qs,) or qpos_goal.shape != (self.entity.n_qs,):
        raise ValueError(
            f"Invalid shape for `qpos_start` or `qpos_goal`. Expected ({(self.entity.n_qs,)}), got ({qpos_start.shape}, {qpos_goal.shape})."
        )

    ######### process joint limit ##########
    q_limit_lower, q_limit_upper = (tensor_to_array(limit[0]) for limit in self.entity.get_qpos_limit(envs_idx))

    if (qpos_start < q_limit_lower).any() or (qpos_start > q_limit_upper).any():
        en.logger.warning(
            "`qpos_start` exceeds joint limit. Relaxing joint limit to contain `qpos_start` for planning."
        )
        q_limit_lower = np.minimum(q_limit_lower, qpos_start)
        q_limit_upper = np.maximum(q_limit_upper, qpos_start)

    if (qpos_goal < q_limit_lower).any() or (qpos_goal > q_limit_upper).any():
        en.logger.warning(
            "`qpos_goal` exceeds joint limit. Relaxing joint limit to contain `qpos_goal` for planning."
        )
        q_limit_lower = np.minimum(q_limit_lower, qpos_goal)
        q_limit_upper = np.maximum(q_limit_upper, qpos_goal)

    ######### setup OMPL ##########
    ou.setLogLevel(ou.LOG_ERROR)
    space = ob.RealVectorStateSpace(self.entity.n_qs)
    bounds = ob.RealVectorBounds(self.entity.n_qs)

    for i_q in range(self.entity.n_qs):
        bounds.setLow(i_q, float(q_limit_lower[i_q]))
        bounds.setHigh(i_q, float(q_limit_upper[i_q]))
    space.setBounds(bounds)
    ss = og.SimpleSetup(space)

    geoms_idx = tuple(range(self.entity.geom_start, self.entity.geom_start + len(self.entity.geoms)))
    mask_collision_pairs = set(
        (i_ga, i_gb)
        for i_ga, i_gb in self.entity.detect_collision(env_idx=envs_idx)
        if i_ga in geoms_idx or i_gb in geoms_idx
    )
    if not ignore_collision and mask_collision_pairs:
        en.logger.info("Ignoring collision pairs already active for starting pos.")

    def is_ompl_state_valid(state):
        if ignore_collision:
            return True
        qpos = torch.tensor(
            [state[i] for i in range(self.entity.n_qs)],
            dtype=gs.tc_float,
            device=self.env.device,
        )
        self.entity.set_qpos(qpos, zero_velocity=False, envs_idx=envs_idx)
        collision_pairs = set(map(tuple, self.entity.detect_collision(env_idx=envs_idx)))
        return not (collision_pairs - mask_collision_pairs)

    ss.setStateValidityChecker(_as_validity_checker(is_ompl_state_valid))

    si = ss.getSpaceInformation()
    si.setStateValidityCheckingResolution(resolution)

    def allocOBValidStateSampler(si):
        vss = ob.UniformValidStateSampler(si)
        vss.setNrAttempts(100)
        return vss

    si.setValidStateSamplerAllocator(_as_sampler_allocator(allocOBValidStateSampler))

    ss.setPlanner(planner_cls(si))

    ss.setStartAndGoalStates(_ompl_state(space, qpos_start), _ompl_state(space, qpos_goal))

    ######### solve ##########
    waypoints = []
    for i in range(max_retry):
        if ss.getPlanner():
            ss.getPlanner().clear()
        status = ss.solve(timeout)
        status_type = status.getStatus()

        if status_type in (
            ob.PlannerStatus.UNKNOWN,
            ob.PlannerStatus.CRASH,
            ob.PlannerStatus.ABORT,
        ):
            raise ValueError("Unknown error.")
        if status_type in (
            ob.PlannerStatus.INVALID_START,
            ob.PlannerStatus.INVALID_GOAL,
            ob.PlannerStatus.UNRECOGNIZED_GOAL_TYPE,
            ob.PlannerStatus.INFEASIBLE,
        ):
            en.logger.warning("Path planning infeasible. Returning empty path.")
            break

        if status:
            path = ss.getSolutionPath()

            if smooth_path:
                ps = og.PathSimplifier(si)
                try:
                    ps.partialShortcutPath(path)
                    ps.ropeShortcutPath(path)
                except Exception as e:
                    en.logger.warning(f"Path simplification failed: {e}")
                    ps.shortcutPath(path)
                ps.smoothBSpline(path)

            if num_waypoints is not None:
                path.interpolate(num_waypoints)

            waypoints = [
                torch.as_tensor(
                    [state[i] for i in range(self.entity.n_qs)],
                    dtype=gs.tc_float,
                    device=gs.device,
                )
                for state in path.getStates()
            ]

        # A timeout or an approximate solution retries until max_retry attempts are used up.
        if status_type in (
            ob.PlannerStatus.TIMEOUT,
            ob.PlannerStatus.APPROXIMATE_SOLUTION,
        ):
            if i + 1 < max_retry:
                en.logger.warning("Path planning did not converge. Trying again...")
                continue
            else:
                if waypoints:
                    en.logger.warning("Path planning did not converge. Returning approximation path.")
                else:
                    en.logger.warning("Path planning did not converge. Returning empty path.")
                break
        en.logger.info("Path solution found successfully.")
        break

    ########## restore original state #########
    self.entity.set_qpos(qpos_start, zero_velocity=False, envs_idx=envs_idx)

    return waypoints

See also

• Planning and sysid guide
• Grasp sampling