[isaac sim] sh파일로 로봇을 맵에 띄워보기 및 A* 알고리즘

1. 아래의 파일을 작성하고 실행. ==>  ./python.sh  run_nova_load.py

import os
import traceback

os.environ["OMNI_KIT_ACCEPT_EULA"] = "YES"

from isaacsim import SimulationApp
simulation_app = SimulationApp({"headless": False})

try:
    # SimulationApp 이후 import
    from isaacsim.core.api import World
    from isaacsim.core.utils.stage import add_reference_to_stage
    from isaacsim.storage.native import get_assets_root_path

    world = World(stage_units_in_meters=1.0)
    world.scene.add_default_ground_plane()

    assets_root = get_assets_root_path()
    print("[INFO] assets_root =", assets_root)
    if not assets_root:
        raise RuntimeError("assets_root를 찾지 못했습니다.")

    usd_path = assets_root + "/Isaac/Robots/NVIDIA/NovaCarter/nova_carter.usd"
    print("[INFO] usd_path =", usd_path)

    add_reference_to_stage(
        usd_path=usd_path,
        prim_path="/World/NovaCarter"
    )

    world.reset()
    world.play()
    print("[INFO] simulation started")

    while simulation_app.is_running():
        world.step(render=True)

except Exception:
    print("[ERROR] 예외 발생")
    traceback.print_exc()

finally:
    simulation_app.close()

 

이러한 시뮬레이터 화면이 나올거임.

 

 

 

2. 각각의 joint이름에 대해서 확인해보자

import os
import traceback

os.environ["OMNI_KIT_ACCEPT_EULA"] = "YES"

from isaacsim import SimulationApp
simulation_app = SimulationApp({"headless": False})

try:
    import omni
    from pxr import UsdPhysics
    from isaacsim.core.api import World
    from isaacsim.core.utils.stage import add_reference_to_stage
    from isaacsim.storage.native import get_assets_root_path

    world = World(stage_units_in_meters=1.0)
    world.scene.add_default_ground_plane()

    assets_root = get_assets_root_path()
    usd_path = assets_root + "/Isaac/Robots/NVIDIA/NovaCarter/nova_carter.usd"

    add_reference_to_stage(usd_path=usd_path, prim_path="/World/NovaCarter")
    world.reset()

    stage = omni.usd.get_context().get_stage()

    print("\n=== Revolute joints under /World/NovaCarter ===")
    for prim in stage.Traverse():
        path = prim.GetPath().pathString
        if not path.startswith("/World/NovaCarter"):
            continue
        if prim.IsA(UsdPhysics.RevoluteJoint):
            print("JOINT:", prim.GetName(), " | PATH:", path)

    print("\n창은 열어둡니다. 콘솔에서 wheel joint 후보를 확인.")
    while simulation_app.is_running():
        world.step(render=True)

except Exception:
    traceback.print_exc()

finally:
    simulation_app.close()

이렇게 다시 또 창이 뜰거임.

이렇게 로봇의 joint들을 볼수있다.

 

 

 

 

 

3. ./python.sh run_nova_manual_drive.py 를 하여, 로봇이 움직이는거를 확인

import os
import traceback
import numpy as np

os.environ["OMNI_KIT_ACCEPT_EULA"] = "YES"

from isaacsim import SimulationApp
simulation_app = SimulationApp({"headless": False})

try:
    from isaacsim.core.api import World
    from isaacsim.core.utils.stage import add_reference_to_stage
    from isaacsim.storage.native import get_assets_root_path
    from isaacsim.robot.wheeled_robots.robots import WheeledRobot
    from isaacsim.robot.wheeled_robots.controllers.differential_controller import DifferentialController

    world = World(stage_units_in_meters=1.0)
    world.scene.add_default_ground_plane()

    assets_root = get_assets_root_path()
    if not assets_root:
        raise RuntimeError("assets root를 찾지 못했습니다.")

    usd_path = assets_root + "/Isaac/Robots/NVIDIA/NovaCarter/nova_carter.usd"
    print("[INFO] usd_path =", usd_path)

    add_reference_to_stage(
        usd_path=usd_path,
        prim_path="/World/NovaCarter"
    )

    robot = world.scene.add(
        WheeledRobot(
            prim_path="/World/NovaCarter",
            name="nova_carter",
            wheel_dof_names=["joint_wheel_left", "joint_wheel_right"],
        )
    )

    controller = DifferentialController(
        name="nova_diff_controller",
        wheel_radius=0.14,
        wheel_base=0.54,
        max_linear_speed=0.8,
        max_angular_speed=1.5,
        max_wheel_speed=30.0,
    )

    world.reset()
    robot.initialize()

    print("[INFO] num_dof =", robot.num_dof)
    print("[INFO] wheel_dof_indices =", robot.wheel_dof_indices)

    world.play()
    world.step(render=True)

    print("[INFO] manual drive start")
    step = 0

    while simulation_app.is_running():
        if step < 300:
            cmd = np.array([0.4, 0.0])
        elif step < 600:
            cmd = np.array([0.0, 0.8])
        elif step < 900:
            cmd = np.array([0.25, -0.5])
        else:
            cmd = np.array([0.0, 0.0])

        action = controller.forward(cmd)

        # 핵심 수정
        robot.apply_wheel_actions(action)

        if step % 60 == 0:
            pos, orn = robot.get_world_pose()
            print(f"[STEP {step}] pos={pos}, cmd={cmd}")

        world.step(render=True)
        step += 1

except Exception:
    print("[ERROR] 예외 발생")
    traceback.print_exc()

finally:
    simulation_app.close()

실행을 하면 로봇이 움직이는걸 볼수있음.

 

 

 

 

 

https://jihong.tistory.com/791

[isaac sim] ROS 2 Navigation

 

4. 위의 내용처럼하여 occupancy map을 준비한다.

 

carter_warehouse_navigation.yaml

0.00MB

 

 

 

5. 아래 파일을 실행하면 a*기반으로 주행됨.

import os
import math
import heapq
import traceback
from pathlib import Path

import numpy as np
import yaml
from PIL import Image

# -----------------------------
# user settings
# -----------------------------
MAP_YAML = "/home/jihong/Downloads/maps/carter_warehouse_navigation.yaml"

# None이면 맵의 좌하단/우상단 근처에서 자동으로 free cell을 찾음
REQUESTED_START_WORLD = None
REQUESTED_GOAL_WORLD = None

# 예시:
# REQUESTED_START_WORLD = (-6.0, -4.0)
# REQUESTED_GOAL_WORLD  = ( 5.0,  3.5)

LOOKAHEAD = 0.7
GOAL_TOL = 0.25

# Nova Carter 시작용 추정값
WHEEL_RADIUS = 0.14
WHEEL_BASE = 0.54

# -----------------------------
# helper: map loading
# -----------------------------
def load_ros_occupancy_map(yaml_path: str):
    yaml_path = Path(yaml_path).expanduser().resolve()
    with open(yaml_path, "r") as f:
        meta = yaml.safe_load(f)

    image_field = meta["image"]
    image_path = Path(image_field)
    if not image_path.is_absolute():
        image_path = (yaml_path.parent / image_field).resolve()

    img = Image.open(image_path).convert("L")
    img = np.array(img, dtype=np.float32)

    resolution = float(meta["resolution"])
    origin = meta["origin"]  # [x, y, yaw]
    origin_x = float(origin[0])
    origin_y = float(origin[1])
    origin_yaw = float(origin[2])

    negate = int(meta.get("negate", 0))
    occupied_thresh = float(meta.get("occupied_thresh", 0.65))
    free_thresh = float(meta.get("free_thresh", 0.196))

    # ROS map_server style interpretation
    if negate == 0:
        occ_prob = (255.0 - img) / 255.0
    else:
        occ_prob = img / 255.0

    # grid_sem:
    #   0 = free
    #   1 = occupied
    #  -1 = unknown
    grid_sem = np.full(img.shape, -1, dtype=np.int8)
    grid_sem[occ_prob > occupied_thresh] = 1
    grid_sem[occ_prob < free_thresh] = 0

    # planning grid:
    #   0 = free
    #   1 = blocked
    plan_grid = np.ones_like(grid_sem, dtype=np.int8)
    plan_grid[grid_sem == 0] = 0
    plan_grid[grid_sem == 1] = 1
    plan_grid[grid_sem == -1] = 1   # 처음엔 unknown도 막힌 셀로 처리

    return {
        "yaml_path": str(yaml_path),
        "image_path": str(image_path),
        "resolution": resolution,
        "origin_x": origin_x,
        "origin_y": origin_y,
        "origin_yaw": origin_yaw,
        "grid_sem": grid_sem,
        "plan_grid": plan_grid,
        "height": plan_grid.shape[0],
        "width": plan_grid.shape[1],
    }

# -----------------------------
# helper: coord conversion
# -----------------------------
def world_to_cell(x, y, map_info):
    """world -> (row, col)"""
    res = map_info["resolution"]
    ox = map_info["origin_x"]
    oy = map_info["origin_y"]
    h = map_info["height"]

    mx = int((x - ox) / res)
    my = int((y - oy) / res)

    col = mx
    row = h - 1 - my
    return row, col

def cell_to_world(row, col, map_info):
    """(row, col) -> world"""
    res = map_info["resolution"]
    ox = map_info["origin_x"]
    oy = map_info["origin_y"]
    h = map_info["height"]

    mx = col
    my = h - 1 - row

    x = ox + (mx + 0.5) * res
    y = oy + (my + 0.5) * res
    return x, y

def in_bounds(grid, rc):
    r, c = rc
    return 0 <= r < grid.shape[0] and 0 <= c < grid.shape[1]

def nearest_free_cell(grid, rc, max_radius=200):
    if in_bounds(grid, rc) and grid[rc[0], rc[1]] == 0:
        return rc

    r0, c0 = rc
    for rad in range(1, max_radius + 1):
        rmin, rmax = r0 - rad, r0 + rad
        cmin, cmax = c0 - rad, c0 + rad

        for r in range(rmin, rmax + 1):
            for c in (cmin, cmax):
                if in_bounds(grid, (r, c)) and grid[r, c] == 0:
                    return (r, c)

        for c in range(cmin, cmax + 1):
            for r in (rmin, rmax):
                if in_bounds(grid, (r, c)) and grid[r, c] == 0:
                    return (r, c)

    raise RuntimeError(f"nearest free cell not found near {rc}")

# -----------------------------
# helper: A*
# -----------------------------
def heuristic(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def reconstruct_path(came_from, current):
    out = [current]
    while current in came_from:
        current = came_from[current]
        out.append(current)
    out.reverse()
    return out

def astar(grid, start, goal):
    if not in_bounds(grid, start) or not in_bounds(grid, goal):
        raise ValueError(f"start/goal out of bounds: {start}, {goal}")
    if grid[start[0], start[1]] != 0:
        raise ValueError(f"start is blocked: {start}")
    if grid[goal[0], goal[1]] != 0:
        raise ValueError(f"goal is blocked: {goal}")

    open_heap = []
    heapq.heappush(open_heap, (heuristic(start, goal), 0, start))

    came_from = {}
    g_score = {start: 0}
    visited = set()

    neighbors = [(1,0), (-1,0), (0,1), (0,-1)]

    while open_heap:
        _, gcur, cur = heapq.heappop(open_heap)
        if cur in visited:
            continue
        visited.add(cur)

        if cur == goal:
            return reconstruct_path(came_from, cur)

        r, c = cur
        for dr, dc in neighbors:
            nxt = (r + dr, c + dc)
            if not in_bounds(grid, nxt):
                continue
            if grid[nxt[0], nxt[1]] != 0:
                continue

            tentative_g = gcur + 1
            if tentative_g < g_score.get(nxt, 1e18):
                g_score[nxt] = tentative_g
                came_from[nxt] = cur
                f = tentative_g + heuristic(nxt, goal)
                heapq.heappush(open_heap, (f, tentative_g, nxt))

    return []

def downsample_path(path_cells, stride=6):
    if len(path_cells) <= 2:
        return path_cells
    out = [path_cells[0]]
    for i in range(stride, len(path_cells) - 1, stride):
        out.append(path_cells[i])
    if out[-1] != path_cells[-1]:
        out.append(path_cells[-1])
    return out

# -----------------------------
# helper: controller
# -----------------------------
def normalize_angle(a):
    return (a + np.pi) % (2.0 * np.pi) - np.pi

def quat_wxyz_to_yaw(q):
    w, x, y, z = q
    return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))

def choose_waypoint(path_world_xy, pos_xy, current_idx, lookahead):
    idx = current_idx
    while idx < len(path_world_xy) - 1:
        if np.linalg.norm(path_world_xy[idx] - pos_xy) >= lookahead:
            break
        idx += 1
    return idx, path_world_xy[idx]

def follower(pos_xy, yaw, wp_xy):
    dx = wp_xy[0] - pos_xy[0]
    dy = wp_xy[1] - pos_xy[1]

    target_yaw = math.atan2(dy, dx)
    yaw_err = normalize_angle(target_yaw - yaw)
    dist = math.hypot(dx, dy)

    v = np.clip(0.9 * dist, 0.0, 0.55)
    w = np.clip(2.0 * yaw_err, -1.1, 1.1)

    # 방향이 많이 틀어져 있으면 직진 속도 줄이기
    if abs(yaw_err) > 0.7:
        v *= 0.2

    return np.array([v, w], dtype=np.float32)

# -----------------------------
# plan path from saved yaml/png
# -----------------------------
map_info = load_ros_occupancy_map(MAP_YAML)
plan_grid = map_info["plan_grid"]

print("[INFO] yaml       :", map_info["yaml_path"])
print("[INFO] image      :", map_info["image_path"])
print("[INFO] resolution :", map_info["resolution"])
print("[INFO] origin     :", (map_info["origin_x"], map_info["origin_y"], map_info["origin_yaw"]))
print("[INFO] grid shape :", plan_grid.shape)

if REQUESTED_START_WORLD is None:
    requested_start = (
        map_info["origin_x"] + 2.0,
        map_info["origin_y"] + 2.0,
    )
else:
    requested_start = REQUESTED_START_WORLD

if REQUESTED_GOAL_WORLD is None:
    requested_goal = (
        map_info["origin_x"] + (map_info["width"] * map_info["resolution"]) - 2.0,
        map_info["origin_y"] + (map_info["height"] * map_info["resolution"]) - 2.0,
    )
else:
    requested_goal = REQUESTED_GOAL_WORLD

start_cell_raw = world_to_cell(requested_start[0], requested_start[1], map_info)
goal_cell_raw  = world_to_cell(requested_goal[0],  requested_goal[1],  map_info)

start_cell = nearest_free_cell(plan_grid, start_cell_raw)
goal_cell  = nearest_free_cell(plan_grid, goal_cell_raw)

start_world = cell_to_world(start_cell[0], start_cell[1], map_info)
goal_world  = cell_to_world(goal_cell[0], goal_cell[1], map_info)

print("[INFO] requested start world:", requested_start)
print("[INFO] requested goal  world:", requested_goal)
print("[INFO] snapped start cell   :", start_cell, "=>", start_world)
print("[INFO] snapped goal  cell   :", goal_cell,  "=>", goal_world)

path_cells = astar(plan_grid, start_cell, goal_cell)
if not path_cells:
    raise RuntimeError("A* could not find a path on the saved map.")

path_cells = downsample_path(path_cells, stride=6)
path_world_xy = np.array([cell_to_world(r, c, map_info) for (r, c) in path_cells], dtype=np.float32)

print("[INFO] path cells:", len(path_cells))
print("[INFO] first waypoints:", path_world_xy[:5])

# optional debug overlay
try:
    import matplotlib.pyplot as plt
    vis = np.where(plan_grid == 0, 255, 0).astype(np.uint8)
    plt.figure(figsize=(8, 8))
    plt.imshow(vis, cmap="gray", origin="upper")
    rr = [p[0] for p in path_cells]
    cc = [p[1] for p in path_cells]
    plt.plot(cc, rr, linewidth=2)
    plt.scatter([start_cell[1]], [start_cell[0]], s=50, marker="o")
    plt.scatter([goal_cell[1]], [goal_cell[0]], s=50, marker="x")
    out_png = str(Path(MAP_YAML).with_name("astar_overlay_debug.png"))
    plt.tight_layout()
    plt.savefig(out_png, dpi=150)
    plt.close()
    print("[INFO] saved debug overlay:", out_png)
except Exception as e:
    print("[WARN] debug overlay skipped:", e)

# -----------------------------
# now launch Isaac Sim
# -----------------------------
os.environ["OMNI_KIT_ACCEPT_EULA"] = "YES"

from isaacsim import SimulationApp
simulation_app = SimulationApp({"headless": False})

try:
    import omni
    from pxr import UsdGeom

    from isaacsim.core.api import World
    from isaacsim.core.utils.stage import add_reference_to_stage
    from isaacsim.storage.native import get_assets_root_path
    from isaacsim.robot.wheeled_robots.robots import WheeledRobot
    from isaacsim.robot.wheeled_robots.controllers.differential_controller import DifferentialController

    world = World(stage_units_in_meters=1.0)

    assets_root = get_assets_root_path()
    if not assets_root:
        raise RuntimeError("Could not find Isaac Sim assets root.")

    env_usd = assets_root + "/Isaac/Environments/Simple_Warehouse/warehouse_with_forklifts.usd"
    robot_usd = assets_root + "/Isaac/Robots/NVIDIA/NovaCarter/nova_carter.usd"

    print("[INFO] env_usd  :", env_usd)
    print("[INFO] robot_usd:", robot_usd)

    add_reference_to_stage(env_usd, "/World/Warehouse")
    add_reference_to_stage(robot_usd, "/World/NovaCarter")

    stage = omni.usd.get_context().get_stage()

    # 로봇 시작 위치를 planned start world에 맞춤
    robot_prim = stage.GetPrimAtPath("/World/NovaCarter")
    robot_xform = UsdGeom.XformCommonAPI(robot_prim)
    robot_xform.SetTranslate((float(start_world[0]), float(start_world[1]), 0.0))
    robot_xform.SetRotate((0.0, 0.0, 0.0), UsdGeom.XformCommonAPI.RotationOrderXYZ)

    robot = world.scene.add(
        WheeledRobot(
            prim_path="/World/NovaCarter",
            name="nova_carter",
            wheel_dof_names=["joint_wheel_left", "joint_wheel_right"],
        )
    )

    controller = DifferentialController(
        name="nova_diff_controller",
        wheel_radius=WHEEL_RADIUS,
        wheel_base=WHEEL_BASE,
        max_linear_speed=0.8,
        max_angular_speed=1.5,
        max_wheel_speed=30.0,
    )

    world.reset()
    robot.initialize()
    world.play()
    world.step(render=True)

    print("[INFO] robot num_dof        :", robot.num_dof)
    print("[INFO] robot wheel indices :", robot.wheel_dof_indices)
    print("[INFO] start driving...")

    wp_idx = 0
    step = 0
    goal_xy = np.array(goal_world, dtype=np.float32)

    while simulation_app.is_running():
        pos, orn = robot.get_world_pose()
        pos_xy = np.array([pos[0], pos[1]], dtype=np.float32)
        yaw = quat_wxyz_to_yaw(orn)

        if np.linalg.norm(goal_xy - pos_xy) < GOAL_TOL:
            action = controller.forward(np.array([0.0, 0.0], dtype=np.float32))
            robot.apply_wheel_actions(action)
            print("[INFO] goal reached.")
            world.step(render=True)
            continue

        wp_idx, wp_xy = choose_waypoint(path_world_xy, pos_xy, wp_idx, LOOKAHEAD)
        cmd = follower(pos_xy, yaw, wp_xy)

        action = controller.forward(cmd)
        robot.apply_wheel_actions(action)

        if step % 60 == 0:
            print(
                f"[STEP {step}] "
                f"pos=({pos_xy[0]:.2f}, {pos_xy[1]:.2f}) "
                f"wp_idx={wp_idx}/{len(path_world_xy)-1} "
                f"wp=({wp_xy[0]:.2f}, {wp_xy[1]:.2f}) "
                f"cmd=({cmd[0]:.2f}, {cmd[1]:.2f})"
            )

        world.step(render=True)
        step += 1

except Exception:
    print("[ERROR] exception occurred")
    traceback.print_exc()

finally:
    simulation_app.close()