Gazebo Sim2Sim

IsaacGym에서 학습한 정책을 Gazebo에서 검증하기

JungYeon Lee

2026-03-26

강의 로드맵

회차	주제
Lec 02	4족 보행 로봇 & IsaacGym 시뮬레이터
Lec 03	PPO 알고리즘 — 보행 정책 학습
Lec 04	VAE & DreamWaQ — 지형 인식 없이 걷기
Lec 05 (오늘)	Gazebo Sim2Sim — 배포 전 검증

오늘 목표: quadruped_sim2sim 레포의 load_dreamwaq_policy.py를 분석하고 Gazebo에서 검증

코드: quadruped_sim2sim

Part 1. Sim2Sim이란?

왜 Sim2Sim이 필요한가?

학습 환경         검증 환경         실제 환경
(IsaacGym)  ──▶  (Gazebo)   ──▶  (실물 로봇)
  Sim2Sim           Sim2Real

IsaacGym만으로는 부족한 이유

GPU 물리 엔진 (PhysX) 특유의 아티팩트
실제 물리와 차이 존재
ROS 인터페이스 없음
실물 제어 코드와 분리됨

Gazebo의 역할

CPU 기반 정밀 물리 (ODE)
ROS2 생태계와 직접 통합
센서 시뮬레이션 (IMU, 카메라)
실물 로봇과 동일한 제어 코드 사용

quadruped_sim2sim 프로젝트 구조

quadruped_sim2sim/
├── policy_model/
│   ├── dreamwaq/model_5000.pt         # 학습된 체크포인트
│   ├── himloco/model_10000.pt
│   └── walk-these-ways/...            # WTW 모델
├── robot_controller/                   # (ament_python) 정책 실행 노드
│   └── robot_controller/
│       ├── load_dreamwaq_policy.py     # ★ DreamWaQ 컨트롤러
│       ├── load_wtw_policy.py          # Walk-These-Ways 컨트롤러
│       └── load_him_policy.py          # HIMLoco 컨트롤러
├── robot_description/                  # (ament_cmake) Go1 URDF, meshes
│   ├── urdf/go1/go1.urdf.xacro
│   └── meshes/                         # STL, DAE 파일
├── robot_gazebo/                       # (ament_cmake) Gazebo launch, world
│   ├── launch/go1_gazebo.launch.py
│   ├── config/go1_ros_control.yaml
│   └── worlds/go1_world.world
└── robot_UI/                           # (ament_python) 속도 명령 UI
    └── robot_UI/UI_controller.py       # PyQt5 조이스틱

4개의 ROS2 패키지

패키지	빌드	역할
`robot_controller`	ament_python	정책 배포 노드 (3종)
`robot_description`	ament_cmake	Go1 URDF/xacro, 메쉬 (Go2 URDF로 교체 가능)
`robot_gazebo`	ament_cmake	Gazebo launch, world, ros2_control
`robot_UI`	ament_python	PyQt5 속도 명령 UI

ROS2 토픽 구조

┌──────────────┐    /robot_velocity_command     ┌──────────────┐
│   robot_UI   │ ──────────────────────────────▶│  policy_node │
│  (PyQt5 UI)  │    PoseStamped (vx, vy, vyaw) │ (DreamWaQ)   │
└──────────────┘                                │              │
                                                │              │
┌──────────────┐    /joint_states               │              │
│   Gazebo     │ ──────────────────────────────▶│              │
│  (Physics)   │    JointState (pos, vel)       │              │
│              │                                │              │
│              │    /imu_plugin/out              │              │
│              │ ──────────────────────────────▶│              │
│              │    Imu (orientation, ang_vel)   │              │
│              │                                │              │
│              │    /joint_effort_controller     │              │
│              │◀──────────────────────────────│              │
│              │    Float64MultiArray (torques)  └──────────────┘
└──────────────┘

Part 2. 관절 순서 매핑

IsaacGym vs Gazebo 관절 순서

가장 흔한 Sim2Sim 실수: 관절 순서가 다름!

IsaacGym 순서 (go2_dreamwaq)

 0: FL_hip        6: RL_hip
 1: FL_thigh      7: RL_thigh
 2: FL_calf       8: RL_calf
 3: FR_hip        9: RR_hip
 4: FR_thigh     10: RR_thigh
 5: FR_calf      11: RR_calf

Gazebo 순서 (/joint_states)

 0: FR_calf       6: FL_thigh
 1: RR_calf       7: FL_hip
 2: FR_thigh      8: RL_calf
 3: RL_thigh      9: FL_calf
 4: RR_thigh     10: RL_hip
 5: RL_hip        11: RR_hip

(URDF 파싱 순서에 따라 결정)

인덱스 매핑 코드

# load_dreamwaq_policy.py — Gazebo → IsaacGym 순서 매핑
ISAAC_IDX = [9, 6, 7, 0, 2, 8, 5, 3, 10, 1, 4, 11]

# /joint_states 콜백에서
def joint_callback(self, msg):
    for i in range(12):
        gazebo_idx = ISAAC_IDX[i]
        self.dof_pos[0, i] = msg.position[gazebo_idx]
        self.dof_vel[0, i] = msg.velocity[gazebo_idx]

Tip

관절 순서가 맞지 않으면 로봇이 즉시 넘어지거나 비정상 동작을 합니다. Sim2Sim에서 가장 먼저 확인해야 할 항목입니다.

Part 3. DreamWaQ 컨트롤러

네트워크 정의 (노드 내 인라인)

# load_dreamwaq_policy.py — 노드 안에 직접 정의
class RunningMeanStd(nn.Module):
    """관측 정규화 — 체크포인트에서 로드"""
    def __init__(self, shape):
        super().__init__()
        self.register_buffer("mean", torch.zeros(shape))
        self.register_buffer("var", torch.ones(shape))
        self.register_buffer("count", torch.tensor(1e-4))

    def normalize(self, x):
        return (x - self.mean) / (self.var.sqrt() + 1e-8)

class CENet(nn.Module):
    """Context-aided Estimator — go2_dreamwaq와 동일 구조"""
    def __init__(self):
        super().__init__()
        self.encoder = nn.Sequential(
            nn.Linear(225, 128), nn.ELU(),   # obs_history 5×45
            nn.Linear(128, 64),  nn.ELU(),
            nn.Linear(64, 35),               # est_vel(3) + mu(16) + logvar(16)
        )
        # Decoder는 추론 시 사용 안 함
        self.decoder = nn.Sequential(
            nn.Linear(19, 64), nn.ELU(),
            nn.Linear(64, 128), nn.ELU(),
            nn.Linear(128, 48), nn.ELU(),
            nn.Linear(48, 45),
        )

class Actor(nn.Module):
    """정책 네트워크 — [512, 256, 128]"""
    def __init__(self):
        super().__init__()
        self.actor = nn.Sequential(
            nn.Linear(64, 512), nn.ELU(),   # obs(45) + est_vel(3) + mu(16)
            nn.Linear(512, 256), nn.ELU(),
            nn.Linear(256, 128), nn.ELU(),
            nn.Linear(128, 12),             # 12 actions
        )

    def act_inference(self, obs):
        return self.actor(obs)

체크포인트 로드

# load_dreamwaq_policy.py
def load_learned_models(pt_file, cenet, actor):
    """go2_dreamwaq 체크포인트에서 가중치 로드"""

    # ★ 트릭: checkpoint에 RunningMeanStd가 rsl_rl.utils.rms 모듈로 저장됨
    # → 가짜 모듈을 sys.modules에 등록하여 torch.load가 역직렬화 가능하게 함
    import types
    fake_module = types.ModuleType("rsl_rl.utils.rms")
    fake_module.RunningMeanStd = RunningMeanStd
    sys.modules["rsl_rl.utils.rms"] = fake_module
    sys.modules["rsl_rl"] = types.ModuleType("rsl_rl")
    sys.modules["rsl_rl.utils"] = types.ModuleType("rsl_rl.utils")

    loaded = torch.load(pt_file, map_location="cpu")

    # CENet 가중치 로드
    cenet.load_state_dict(loaded["cenet_state_dict"])

    # Actor 가중치 로드
    actor.load_state_dict(loaded["model_state_dict"], strict=False)

    # RunningMeanStd (obs 정규화)
    obs_rms = loaded["rms"]  # mean, var 포함

    return obs_rms

AgentNode — 초기화

class AgentNode(Node):
    def __init__(self):
        super().__init__("agent_node")

        # 네트워크 로드
        self.cenet = CENet()
        self.actor = Actor()
        self.obs_rms = load_learned_models(
            "policy_model/dreamwaq/model_5000.pt",
            self.cenet, self.actor
        )
        self.cenet.eval()
        self.actor.eval()

        # 상태 버퍼
        self.dof_pos = torch.zeros(1, 12)
        self.dof_vel = torch.zeros(1, 12)
        self.actions = torch.zeros(1, 12)
        self.obs_history = torch.zeros(1, 5, 45)  # 5스텝 히스토리

        # 기본 관절 자세
        self.default_dof_pos = torch.tensor([[
            0.1, 0.8, -1.5, -0.1, 0.8, -1.5,   # FL, FR
            0.1, 1.0, -1.5, -0.1, 1.0, -1.5,   # RL, RR
        ]])

        # PD 게인 (Go2 기준)
        self.Kp = 20.0
        self.Kd = 0.5

AgentNode — 구독 & 타이머

        # 구독
        self.joint_sub = self.create_subscription(
            JointState, "/joint_states",
            self.joint_callback, 10)

        self.imu_sub = self.create_subscription(
            Imu, "/imu_plugin/out",
            self.imu_callback, 10)

        self.cmd_sub = self.create_subscription(
            PoseStamped, "/robot_velocity_command",
            self.cmd_callback, 10)

        # 퍼블리시
        self.effort_pub = self.create_publisher(
            Float64MultiArray,
            "/joint_effort_controller/commands", 10)

        # 제어 루프 — 200Hz (정책은 4스텝마다 = 50Hz)
        self.timer = self.create_timer(0.005, self.control_loop)
        self.decimation_counter = 0

관측값 구성

def build_observation(self):
    """IsaacGym과 동일한 45차원 관측 벡터"""

    # IMU — 각속도 (body frame)
    base_ang_vel = self.imu_ang_vel                        # [1, 3]

    # 중력 벡터 (body frame 투영)
    projected_gravity = quat_rotate(                       # [1, 3]
        self.base_quat, torch.tensor([[0., 0., -1.]]))

    # 속도 명령
    commands = self.velocity_commands                       # [1, 3]

    # 관측 벡터 조합 — go2_dreamwaq와 동일 순서
    obs = torch.cat([
        base_ang_vel, projected_gravity, commands,
        self.dof_pos - self.default_dof_pos,               # [1, 12]
        self.dof_vel,                                      # [1, 12]
        self.actions,                                      # [1, 12]
    ], dim=-1)                                             # [1, 45]

    return obs

Tip

quat_rotate를 사용하는 것에 주의 — go2_dreamwaq의 학습 코드와 일치해야 합니다. (HIMLoco는 quat_rotate_inverse 사용)

제어 루프 — 핵심 코드

def control_loop(self):
    """200Hz 제어 루프 — 정책은 50Hz (4스텝마다)"""
    self.decimation_counter += 1

    if self.decimation_counter >= 4:  # 50Hz 정책 실행
        self.decimation_counter = 0

        # 1. 관측값 구성
        obs = self.build_observation()

        # 2. 히스토리 업데이트 & 정규화
        self.obs_history = torch.roll(self.obs_history, shifts=-1, dims=1)
        self.obs_history[:, -1] = obs
        obs_norm = self.obs_rms.normalize(obs)
        obs_history_norm = self.obs_rms.normalize(
            self.obs_history.flatten(1).reshape(-1, 45)
        ).reshape(1, -1)                                  # [1, 225]

        # 3. CENet — est_vel + mu (context_vec)
        h = self.cenet.encoder(obs_history_norm)          # [1, 35]
        est_vel = h[:, :3]                                # [1, 3]
        mu = h[:, 3:19]                                   # [1, 16] (context)

        # 4. Actor 추론
        actor_obs = torch.cat([obs_norm, est_vel, mu], dim=-1)  # [1, 64]
        raw_actions = self.actor.act_inference(actor_obs)       # [1, 12]

        # 5. Action scaling (hip × 0.5)
        self.actions = raw_actions * 0.25
        self.actions[:, [0, 3, 6, 9]] *= 0.5             # hip 관절 스케일 감소

    # 6. PD 제어 → 토크 계산 (매 스텝, 200Hz)
    q_des = self.default_dof_pos + self.actions
    torques = self.Kp * (q_des - self.dof_pos) - self.Kd * self.dof_vel

    # 7. 토크 퍼블리시
    msg = Float64MultiArray()
    msg.data = torques.squeeze().tolist()
    self.effort_pub.publish(msg)

제어 주파수 정리

┌─────────────────────────────────────────────────────┐
│  200Hz (0.005s)                                      │
│  ├── PD 제어 (토크 계산 & 퍼블리시)  ← 매 스텝      │
│  │                                                   │
│  └── 50Hz (0.02s = 4스텝마다)                        │
│      ├── 관측값 구성                                  │
│      ├── CENet 추론 (context_vec 생성)               │
│      ├── Actor 추론 (action 결정)                    │
│      └── Action scaling                              │
└─────────────────────────────────────────────────────┘

PD 제어는 200Hz로 더 빈번하게 실행 → 물리적 안정성 향상

Part 4. Gazebo 환경

Go1 URDF/Xacro

<!-- robot_description/urdf/go1/go1.urdf.xacro -->
<robot name="go1" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- 몸체 -->
  <link name="trunk">
    <inertial>
      <mass value="4.8"/>  <!-- 4.8 kg -->
      <origin xyz="0.0116 0.0041 -0.0003"/>
    </inertial>
  </link>

  <!-- 관절 예시 (FL_hip) -->
  <joint name="FL_hip_joint" type="revolute">
    <axis xyz="1 0 0"/>
    <limit effort="23.7" velocity="30.1"
           lower="-0.863" upper="0.863"/>
  </joint>

  <!-- IMU 센서 (1000Hz) -->
  <gazebo reference="imu_link">
    <sensor name="imu_sensor" type="imu">
      <update_rate>1000</update_rate>
      <plugin filename="libgazebo_ros_imu_sensor.so"
              name="imu_plugin">
        <topicName>imu_plugin/out</topicName>
      </plugin>
    </sensor>
  </gazebo>

ros2_control 설정

# robot_gazebo/config/go1_ros_control.yaml
controller_manager:
  ros__parameters:
    update_rate: 1000      # 1kHz 컨트롤러 매니저
    use_sim_time: true

# 관절 상태 브로드캐스터
joint_states_controller:
  ros__parameters:
    type: joint_state_broadcaster/JointStateBroadcaster

# 토크 제어기 (effort)
joint_effort_controller:
  ros__parameters:
    type: effort_controllers/JointGroupEffortController
    joints:
      - FL_hip_joint
      - FL_thigh_joint
      - FL_calf_joint
      - FR_hip_joint
      - FR_thigh_joint
      - FR_calf_joint
      - RL_hip_joint
      - RL_thigh_joint
      - RL_calf_joint
      - RR_hip_joint
      - RR_thigh_joint
      - RR_calf_joint

Launch 파일

# robot_gazebo/launch/go1_gazebo.launch.py
def generate_launch_description():
    return LaunchDescription([
        # 1. Gazebo 실행 (go1_world.world)
        IncludeLaunchDescription(
            "gazebo_ros", "launch/gazebo.launch.py",
            launch_arguments={"world": world_path}.items()
        ),

        # 2. robot_state_publisher (URDF → TF)
        Node(package="robot_state_publisher",
             executable="robot_state_publisher",
             parameters=[{"robot_description": robot_desc}]),

        # 3. 로봇 스폰 (z=0.4m 높이)
        Node(package="gazebo_ros",
             executable="spawn_entity.py",
             arguments=["-entity", "go1", "-topic", "robot_description",
                        "-x", "0", "-y", "0", "-z", "0.4"]),

        # 4. 컨트롤러 스폰
        Node(package="controller_manager",
             executable="spawner",
             arguments=["joint_effort_controller"]),
        Node(package="controller_manager",
             executable="spawner",
             arguments=["joint_states_controller"]),
    ])

실행 명령어

# 1. 워크스페이스 빌드
cd ~/ros2_ws
colcon build --packages-select \
    robot_description robot_gazebo robot_controller robot_UI

source install/setup.bash

# 2. Gazebo + Go1 로봇 실행
ros2 launch robot_gazebo go1_gazebo.launch.py

# 3. DreamWaQ 정책 실행 (별도 터미널)
ros2 run robot_controller run_dreamwaq_policy

# 4. 속도 명령 UI (별도 터미널)
ros2 run robot_UI run_ui

# 또는 CLI로 직접 속도 명령
ros2 topic pub /robot_velocity_command \
    geometry_msgs/PoseStamped \
    "{pose: {position: {x: 0.5, y: 0.0, z: 0.0}}}" --rate 10

Part 5. 속도 명령 UI

PyQt5 조이스틱 UI

# robot_UI/UI_controller.py
class UIController(Node):
    """속도 명령 UI — PyQt5 기반"""

    def __init__(self):
        super().__init__("velocity_command_ui")
        self.publisher = self.create_publisher(
            PoseStamped, "/robot_velocity_command", 10)

# robot_UI/submodule/robot_UI.py
class JoystickWidget(QWidget):
    """원형 조이스틱 위젯"""
    commandChanged = pyqtSignal(float, float, float)

    # 수직 드래그 → command_x (전진 속도, -1.5 ~ 1.5)
    # 수평 드래그 → command_z (yaw 각속도, -1.0 ~ 1.0)
    # command_y = 0.0 (항상)

텍스트 모드: X, Y, Angular Z 직접 입력
조이스틱 모드: 마우스 드래그로 속도 명령
PoseStamped의 position.x/y/z에 (vx, vy, vyaw) 매핑

Part 6. 3개 정책 비교

DreamWaQ vs WTW vs HIMLoco

	DreamWaQ	Walk-These-Ways	HIMLoco
관측 차원	45	70	45
히스토리	5 × 45 = 225	30 × 70 = 2100	6 × 45 = 270
추정기	CENet (VAE)	Adaptation Module (JIT)	HIMEstimator (L2-norm)
Actor 입력	obs + est_vel + mu = 64	obs_history + latent	obs + vel + latent = 64
명령	3 (vx, vy, vyaw)	15 (gait params 포함)	3 (vx, vy, vyaw)
Clock inputs	없음	있음 (4 sin signals)	없음
PD Kp / Kd	20 / 0.5	20 / 0.5	40 / 1.0
PD 주파수	200Hz	50Hz	500Hz
정책 주파수	50Hz	50Hz	50Hz
Hip 스케일	× 0.5	× 0.5	없음
토크 클리핑	100 Nm	10 (action clip)	80 Nm

중력 회전 함수 주의

# DreamWaQ — quat_rotate 사용
projected_gravity = quat_rotate(base_quat, torch.tensor([[0., 0., -1.]]))

# HIMLoco — quat_rotate_inverse 사용
projected_gravity = quat_rotate_inverse(base_quat, torch.tensor([[0., 0., -1.]]))

Tip

학습 코드와 배포 코드에서 동일한 함수를 사용해야 합니다. 함수가 다르면 중력 벡터가 뒤집혀서 로봇이 넘어집니다.

Part 7. 트러블슈팅 & 정리

자주 겪는 문제와 해결법

증상	원인	해결
로봇이 즉시 넘어짐	관절 순서 불일치	`ISAAC_IDX` 매핑 확인
다리가 심하게 떨림	PD 게인 차이	Kp, Kd를 학습 설정과 일치
제자리에서 진동	action_scale 차이	0.25 + hip × 0.5 확인
한쪽으로 돌아감	중력 회전 함수	quat_rotate vs inverse
지면에 빠짐	URDF collision 누락	collision 메쉬 확인
정책 로드 실패	rsl_rl 모듈 없음	sys.modules 가짜 모듈 등록
obs_rms 불일치	체크포인트 버전 차이	mean/var shape 확인

전체 강의 요약

Lec 02 — 4족 로봇 구조 (12 DoF), IsaacGym GPU 시뮬레이션, 보상 함수
Lec 03 — PPO 클리핑, GAE, Adaptive LR, ActorCritic [512,256,128]
Lec 04 — CENet(225→35→19→45), AdaBoot, PPO+VAE 동시 학습
Lec 05 — 관절 매핑, ROS2 토크 제어, 200Hz PD + 50Hz 정책

실무 적용 흐름

논문 이해     go2_dreamwaq       IsaacGym         quadruped_sim2sim
(DreamWaQ) → (CENet + PPO 구현) → (4096 env 학습) → (Gazebo 검증)
                                                         │
                                                         ▼
                                                   실물 로봇 배포
                                                   (동일 ROS2 노드)

Gazebo 노드(load_dreamwaq_policy.py)를 그대로 실물 로봇에 연결할 수 있습니다. 토픽명과 URDF만 바꾸면 됩니다.

참고 자료

강의 코드 (DreamWaQ): go2_dreamwaq
강의 코드 (Sim2Sim): quadruped_sim2sim
Gazebo + ROS2: Gazebo Tutorials
ros2_control: ros2_control Docs
Sim2Real 서베이: Zhao et al., “Sim-to-Real Transfer in Deep Reinforcement Learning for Robotics: a Survey”, 2020
Domain Randomization: Tobin et al., “Domain Randomization for Transferring Deep Neural Networks from Simulation to the Real World”, 2017

Q & A

감사합니다!