Skip to content

Environments

Base Environment

Bases: ABC

Get current joint positions.

Source code in nav_planning/envs/base_env.py 
 8
 9
10
11
@abstractmethod
def get_joint_states(self) -> np.ndarray:
    """Get current joint positions."""
    raise NotImplementedError

Set joint positions.

Source code in nav_planning/envs/base_env.py 
13
14
15
@abstractmethod
def set_joint_states(self, config: np.ndarray):
    """Set joint positions."""

Get robot localization as [x, y, theta].

Source code in nav_planning/envs/base_env.py 
17
18
19
20
@abstractmethod
def get_localization(self) -> np.ndarray:
    """Get robot localization as [x, y, theta]."""
    raise NotImplementedError

Get observations.

Source code in nav_planning/envs/base_env.py 
22
23
24
25
@abstractmethod
def get_obs(self) -> Any:
    """Get observations."""
    raise NotImplementedError

PyBullet Environment

Bases: BaseEnv

Source code in nav_planning/envs/pybullet_env.py 
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
def __init__(
    self,
    config: RobotConfig,
    visualize: bool = True,
    viz_urdf_path: str | None = None,
):
    self.config = config
    urdf = viz_urdf_path if viz_urdf_path else config.urdf_path
    self.sim = vpb.PyBulletSimulator(urdf, config.joint_names, visualize=visualize)
    self.joint_names = config.joint_names

    # Find camera link index
    self.camera_link_idx = -1
    # Use skel_id (body id) to query joints
    num_joints = self.sim.client.getNumJoints(self.sim.skel_id)
    for i in range(num_joints):
        info = self.sim.client.getJointInfo(self.sim.skel_id, i)
        # info[12] is the child link name (bytes)
        if info[12].decode("utf-8") == config.camera.link_name:
            self.camera_link_idx = i
            break

    # Both robots' home pose: base at the origin, camera straight ahead.
    self.set_joint_states(np.zeros(len(self.joint_names)))

Apply a full 5-DOF config (base x, y, theta, pan, tilt).

The base is on the URDF's virtual planar joints, so this is a joint-state update.

Source code in nav_planning/envs/pybullet_env.py 
48
49
50
51
52
53
54
def set_configuration(self, config: np.ndarray):
    """Apply a full 5-DOF config (base x, y, theta, pan, tilt).

    The base is on the URDF's virtual planar joints, so this is a
    joint-state update.
    """
    self.set_joint_states(config)

Base pose [x, y, theta].

Source code in nav_planning/envs/pybullet_env.py 
56
57
58
def get_localization(self) -> np.ndarray:
    """Base pose ``[x, y, theta]``."""
    return self.get_joint_states()[:3]

Block until key is pressed in the PyBullet GUI.

Prints message to stdout, then polls keyboard events until the requested key is triggered. Accepts either a single character ("n") or a raw PyBullet key code (e.g. pb.B3G_RIGHT_ARROW). Returns early if the GUI window is closed.

Source code in nav_planning/envs/pybullet_env.py 
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
def wait_key(self, key: str | int, message: str = "") -> None:
    """Block until *key* is pressed in the PyBullet GUI.

    Prints *message* to stdout, then polls keyboard events until the
    requested key is triggered.  Accepts either a single character
    (``"n"``) or a raw PyBullet key code (e.g. ``pb.B3G_RIGHT_ARROW``).
    Returns early if the GUI window is closed.
    """
    client = self.sim.client
    key_code = ord(key) if isinstance(key, str) else int(key)
    if message:
        print(message)
    while client.isConnected():
        keys = client.getKeyboardEvents()
        if key_code in keys and keys[key_code] & pb.KEY_WAS_TRIGGERED:
            break
        time.sleep(0.01)

Block until the user closes the PyBullet GUI window.

Source code in nav_planning/envs/pybullet_env.py 
125
126
127
128
129
130
def wait_for_close(self) -> None:
    """Block until the user closes the PyBullet GUI window."""
    client = self.sim.client
    while client.isConnected():
        client.getKeyboardEvents()  # keeps the event queue alive
        time.sleep(0.05)

Interactively play back a full-DOF path, VAMP-style.

Each row of path is handed to :meth:set_configuration. The method blocks on the PyBullet GUI with these controls:

  • SPACE — toggle auto-play (loops end → start)
  • ← / → — step one waypoint back / forward (while paused)
  • next_key (opt.) — exit and return True — useful when the caller wants to drive a sequence of demos

Exits when the user closes the GUI window (returns False) or presses next_key if one is provided (returns True). The animation starts paused so the caller can read the banner before anything moves.

Parameters:

Name Type Description Default
path ndarray

(N, dof) array of full-DOF configurations.

 required
fps float

playback frame rate while auto-playing.

 60.0
next_key str | int | None

optional single-character string or raw PyBullet key code; pressing it exits the viewer.

 None

Returns:

Type Description
bool

True if next_key was pressed, False otherwise.
Source code in nav_planning/envs/pybullet_env.py 
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
def animate_path(
    self,
    path: np.ndarray,
    fps: float = 60.0,
    next_key: str | int | None = None,
) -> bool:
    """Interactively play back a full-DOF path, VAMP-style.

    Each row of *path* is handed to :meth:`set_configuration`.  The
    method blocks on the PyBullet GUI with these controls:

    * ``SPACE``             — toggle auto-play (loops end → start)
    * ``← / →``             — step one waypoint back / forward (while paused)
    * ``next_key`` (opt.)   — exit and return ``True`` — useful when
      the caller wants to drive a sequence of demos

    Exits when the user closes the GUI window (returns ``False``) or
    presses *next_key* if one is provided (returns ``True``).  The
    animation starts paused so the caller can read the banner before
    anything moves.

    Args:
      path: ``(N, dof)`` array of full-DOF configurations.
      fps:  playback frame rate while auto-playing.
      next_key: optional single-character string or raw PyBullet key
        code; pressing it exits the viewer.

    Returns:
      ``True`` if *next_key* was pressed, ``False`` otherwise.
    """
    client = self.sim.client
    dt = 1.0 / fps
    n = int(len(path))
    idx = 0
    playing = False

    left = pb.B3G_LEFT_ARROW
    right = pb.B3G_RIGHT_ARROW
    space = ord(" ")
    next_code: int | None = None
    if next_key is not None:
        next_code = ord(next_key) if isinstance(next_key, str) else int(next_key)

    parts = ["SPACE play/pause", "←/→ step"]
    if next_code is not None:
        parts.append(f"'{next_key}' next")
    parts.append("close window to exit")
    print("  " + "  |  ".join(parts))

    advanced_to_next = False
    while client.isConnected():
        self.set_configuration(path[idx])

        keys = client.getKeyboardEvents()
        if space in keys and keys[space] & pb.KEY_WAS_TRIGGERED:
            playing = not playing
        elif (
            next_code is not None
            and next_code in keys
            and keys[next_code] & pb.KEY_WAS_TRIGGERED
        ):
            advanced_to_next = True
            break
        elif not playing and left in keys and keys[left] & pb.KEY_WAS_TRIGGERED:
            idx = (idx - 1) % n
        elif not playing and right in keys and keys[right] & pb.KEY_WAS_TRIGGERED:
            idx = (idx + 1) % n
        elif playing:
            idx = (idx + 1) % n

        time.sleep(dt)

    return advanced_to_next