flowchart TD B(["몸통 base"]) --> FL["FL 다리"] & FR["FR 다리"] & RL["RL 다리"] & RR["RR 다리"] FL --> h["hip<br/>(좌우 벌림)"] --> t["thigh<br/>(앞뒤)"] --> c["calf<br/>(무릎)"] classDef leg fill:#e3f2fd,stroke:#1976d2; classDef joint fill:#fff3e0,stroke:#f57c00; class FL,FR,RL,RR leg; class h,t,c joint;
4족 보행 로봇 & IsaacLab 환경 구성
DreamWaQ on IsaacLab — 1차시
강의 로드맵
| 회차 | 주제 |
|---|---|
| 1차시 (오늘) | 4족 보행 로봇 & IsaacLab 환경 구성 |
| 2차시 | PPO & Actor-Critic — 보행 정책 학습 |
| 3차시 | VAE & DreamWaQ — 지형 인식 없이 걷기 (CENet) |
| 4차시 | Gazebo Sim2Sim — 배포 전 검증 |
본 시리즈는 4족 보행 로봇 Unitree Go2가 험지를 걷도록 학습시키는 전 과정을, 실제 구현 저장소 IsaacLab_DreamWaQ의 소스 코드와 함께 한 줄씩 따라갑니다.
- 4족 로봇의 12 자유도(DoF)와 관측/행동/보상의 구조를 안다.
- IsaacLab의 두 API — ManagerBased(선언적) / Direct(명시적) — 를 비교한다.
- 관측 조립·보상 계산·PD 변환·도메인 randomization 코드를 직접 읽는다.
💡 회색 동그라미 번호(1)에 마우스를 올리면 해당 코드 줄의 설명이 보입니다.
복잡해 보이지만 결국 세 가지를 설계하는 일입니다 — 로봇이 무엇을 보는지(관측), 무엇을 하는지(행동), 무엇을 잘하면 칭찬하는지(보상). 오늘은 이 셋과, 이를 흔들어 강건하게 만드는 도메인 randomization을 IsaacLab 코드에서 만듭니다. (학습 알고리즘 PPO는 2차시에서 다룹니다.)
1. 4족 보행 로봇 Go2
Go2는 4개 다리 × 3관절 = 12 DoF입니다. 각 다리는 hip(좌우 벌림), thigh(앞뒤 휘두름), calf(무릎 굽힘)으로 구성되고 순서는 FL → FR → RL → RR입니다. 정책의 출력은 절대각이 아니라 기본 자세(default_dof_pos)에 더해지는 오프셋입니다.
2. IsaacLab의 두 가지 환경 API
IsaacLab_DreamWaQ는 동일한 DreamWaQ 환경을 두 방식으로 구현해 비교할 수 있게 했습니다.
ManagerBased (dreamwaq_manager) |
Direct (dreamwaq_direct) |
|
|---|---|---|
| 핵심 클래스 | ManagerBasedRLEnv |
DirectRLEnv |
| 스타일 | 선언적 — @configclass로 항목 선언 |
명시적 — 메서드에 로직 직접 작성 |
| 관측/보상 | Manager가 항목 단위로 조합 | _get_observations() / _get_rewards() |
| 장점 | 코드 짧음, 조합 쉬움 | 흐름 투명, 논문 코드와 1:1 |
두 구현은 알고리즘이 아니라 표현 방식만 다릅니다. 아래부터 같은 로직을 두 코드로 나란히 봅니다.
3. 관측 조립 — 코드로 읽기
DreamWaQ는 비대칭 actor-critic입니다. Actor는 실제 로봇이 가질 수 있는 고유수용성 45차원, Critic은 시뮬레이터만 아는 특권 238차원을 받습니다.
flowchart LR
subgraph P["고유수용성 (실로봇도 측정 가능)"]
direction TB
A1["ang_vel 3"]; A2["gravity 3"]; A3["commands 3"]
A4["joint_pos 12"]; A5["joint_vel 12"]; A6["last_action 12"]
end
subgraph PR["특권 정보 (시뮬레이터만)"]
direction TB
B1["disturb_force 3"]; B2["height_scan 187"]
end
P -->|"+노이즈"| ACT(["Actor 관측<br/>45"])
P -->|"노이즈 없음"| CRI(["Critic 관측<br/>238"])
PR --> CRI
classDef act fill:#e8f5e9,stroke:#2e7d32,stroke-width:2px;
classDef cri fill:#fce4ec,stroke:#c2185b,stroke-width:2px;
class ACT act; class CRI cri;
Direct: _get_observations() 한 메서드에서 조립
Direct 구현은 관측이 어떻게 만들어지는지 가장 투명하게 보여줍니다.
# dreamwaq_direct/.../dreamwaq_env.py
def _get_observations(self) -> dict:
lin_vel_b = self._robot.data.root_lin_vel_b
ang_vel_b = self._robot.data.root_ang_vel_b
projected_gravity = self._robot.data.projected_gravity_b
joint_pos_rel = self._robot.data.joint_pos - self._robot.data.default_joint_pos
joint_vel = self._robot.data.joint_vel
self._true_lin_vel_b = lin_vel_b.clone()
if self.cfg.obs_noise:
ang_vel_b = ang_vel_b + torch.empty_like(ang_vel_b).uniform_(-0.05, 0.05)
projected_gravity = projected_gravity + torch.empty_like(projected_gravity).uniform_(-0.05, 0.05)
joint_pos_rel = joint_pos_rel + torch.empty_like(joint_pos_rel).uniform_(-0.01, 0.01)
joint_vel = joint_vel + torch.empty_like(joint_vel).uniform_(-0.075, 0.075)
obs_parts = [ang_vel_b, projected_gravity,
self._commands[:, :3],
joint_pos_rel, joint_vel, self._actions]
obs = torch.cat(obs_parts, dim=-1)
state_parts = [true_ang_vel_b, true_gravity, commands_3,
true_joint_pos_rel, true_joint_vel, self._actions,
disturb_force, height_data]
state = torch.cat(state_parts, dim=-1)
observations = {"policy": obs}
if self.cfg.state_space > 0:
observations["critic"] = state
return observations- 1
- 몸통 좌표계의 선속도. actor 관측에는 들어가지 않습니다 — 실제 로봇은 측정이 어렵기 때문이며, 3차시 CENet이 이 값을 추정하게 됩니다.
- 2
- 관절각을 기본 자세 기준 상대값으로. 절대각이 아니라 “기본자세에서 얼마나 벗어났나”를 봅니다.
- 3
- 진짜 선속도를 따로 저장 — critic의 정답이자 3차시 CENet 학습의 정답으로 쓰입니다.
- 4
- 관측 노이즈: 센서 불확실성을 모사해 sim-to-real 강건성 확보. 값(±0.05 각속도, ±0.01 관절각 등)은 원본 IsaacGym DreamWaQ와 동일.
- 5
-
actor 관측 6개 항목을 순서대로 나열:
ang_vel(3) + gravity(3) + commands(3) + joint_pos(12) + joint_vel(12) + actions(12). - 6
-
torch.cat으로 45차원 한 벡터로 합칩니다 (Oracle은 lin_vel 추가 → 48). - 7
-
critic은 노이즈 없는 값 + 특권 정보(
disturb_force,height_data)를 씁니다. - 8
- critic 관측 238차원(Waq는 lin_vel 제외 → 235).
- 9
-
결과는
policy/critic두 키의 dict. rsl_rl이 각각 actor/critic에 연결합니다.
height_data 는 187개 ray가 측정한 로봇높이 − 지형높이를 [-1,1]로 클리핑한 값으로, critic은 지형을 “봅니다”. Actor에는 없으므로 험지에서 불리하고, 이것이 3차시 CENet의 동기입니다.
ManagerBased: 같은 관측을 “선언”으로
ManagerBased는 위 조립 과정을 직접 쓰지 않고 항목만 선언하면 Manager가 합쳐줍니다.
# dreamwaq_manager/.../velocity_env_cfg.py (ObservationsCfg.PolicyCfg)
base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(-0.05, 0.05))
projected_gravity = ObsTerm(func=mdp.projected_gravity, noise=Unoise(-0.05, 0.05))
velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"})
joint_pos = ObsTerm(func=mdp.joint_pos_rel, noise=Unoise(-0.01, 0.01))
joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(-0.075, 0.075))
actions = ObsTerm(func=mdp.last_action)- 1
-
각 항목이
ObsTerm— 계산 함수와 노이즈를 선언만 합니다. Direct의uniform_(-0.05, 0.05)노이즈가 여기선Unoise(...)선언으로 표현됩니다. - 2
-
선언 순서가 곧 concat 순서. Manager가 6개 항목을 자동으로 45차원으로 합칩니다 — Direct의
obs_parts+torch.cat과 동일한 결과.
4. 행동 → PD 토크 — 코드로 읽기
정책 출력(12차원)은 관절 목표각 오프셋이고, PD 제어가 이를 토크로 바꿉니다. 핵심 수식은 다음 두 줄입니다.
q_{des} = q_{default} + \text{scale}\cdot a, \qquad \tau = K_p\,(q_{des}-q) - K_d\,\dot q
쉽게 말하면 — PD 제어는 관절에 용수철 + 완충기를 단 것과 같습니다. K_p(용수철)는 목표각 q_{des} 쪽으로 당기는 힘, K_d(완충기)는 너무 빨리 움직여 출렁이지 않게 잡아주는 힘입니다. 정책은 “목표각”만 정하고, 실제로 당기고 잡는 일은 PD가 합니다. (아래에서 직접 만져봅니다.)
Direct에서 앞 단계(목표각 계산)는 명시적으로 드러납니다.
# dreamwaq_direct/.../dreamwaq_env.py
def _pre_physics_step(self, actions):
self._prev_prev_actions = self._prev_actions.clone()
self._prev_actions = self._actions.clone()
self._actions = actions.clone().clamp(-1.0, 1.0)
self._processed_actions = (
self.cfg.action_scale * self._actions + self._robot.data.default_joint_pos
)
def _apply_action(self):
self._robot.set_joint_position_target(self._processed_actions)- 1
- 직전·직전전 행동을 보관 — 보상의 저크(jerk) 계산(§5)에 씁니다.
- 2
-
행동 클리핑
[-1, 1]. 없으면 큰 행동 → 관절 한계 초과 → 극단적 PD 토크 → 학습 붕괴. - 3
-
위 수식의 q_{des}=q_{default}+\text{scale}\cdot a.
action_scale=0.25(작은 값이 안정적). - 4
- 목표각만 설정하면, \tau=K_p(q_{des}-q)-K_d\dot q 변환은 IsaacLab 액추에이터가 200 Hz로 수행합니다.
decimation=4 때문에 정책은 50 Hz로 목표각만 갱신하고, 그 사이 PD 제어는 200 Hz로 4번 토크를 계산합니다. 아래 애니메이션에서 굵은 칸(정책 추론)마다 그 뒤로 가는 세 칸의 PD 스텝이 따라옵니다.
Go2 게인 K_p=20, K_d=0.5는 Go2 설정에서 덮어씁니다. 단일 관절 PD가 실제로 어떻게 진동을 잡는지는 아래 시뮬레이터에서 직접 만져봅니다.
단일 관절 PD를 손으로 — 행동이 곧 목표각
행동(정책 출력)이 곧 PD 목표각이므로, 관절 하나의 PD를 직접 보면 12관절 전체가 한눈에 들어옵니다. 1자유도 회전 모터 J\ddot\theta + b\dot\theta = \tau 위에서:
- P 제어 \tau = K_p(\theta_d-\theta) — 목표각에 용수철만 단 것. K_p를 키우면 빨라지지만 감쇠비 \zeta = b/(2\sqrt{K_p J})가 작아져 진동합니다.
- PD 제어 \tau = K_p(\theta_d-\theta) - K_d\dot\theta — 완충기(K_d)를 더해 \zeta = (b+K_d)/(2\sqrt{K_p J}). 이제 K_p로 속도, K_d로 진동을 따로 잡습니다.
DreamWaQ의 12개 관절은 이 PD가 12벌 동시에 도는 것뿐입니다 (K_p{=}20,\ K_d{=}0.5).
P / PD를 토글하고 K_p,\ K_d, 목표각을 움직여 응답을 관찰해 보세요.
목표 θ_d
90.0°
현재 θ
0.0°
오차 e
90.0°
제어 입력 τ
0.00 Nm
이렇게 해보세요 — ① P 모드에서 K_p를 1 → 4 → 8로 올리면 진동이 점점 심해집니다. ② PD 모드로 바꿔 K_p{=}8을 둔 채 K_d를 0 → 1.5로 올리면 같은 K_p에서도 진동이 사라집니다. ③ K_p만으로는 빠른 응답과 안정을 동시에 얻을 수 없다는 걸 체감하세요 — 그래서 K_d가 필요합니다.
5. 보상 계산 — 코드로 읽기
DreamWaQ 보상은 속도 추종(+) 2항 + 정규화 페널티(−) 11항 = 13개입니다. 핵심 수식은 추종 항입니다.
r_{track} = \exp\!\Big(-\frac{\lVert v_{cmd}-v\rVert^2}{\sigma}\Big), \qquad \sigma = 0.25
Direct는 13개 항을 _get_rewards()에서 직접 계산합니다.
# dreamwaq_direct/.../dreamwaq_env.py (_get_rewards, 발췌)
dt = self.step_dt
lin_vel_error = torch.sum(torch.square(self._commands[:, :2] - lin_vel_b[:, :2]), dim=1)
track_lin_vel = torch.exp(-lin_vel_error / self.cfg.tracking_sigma) * self.cfg.rew_track_lin_vel_xy
jerk = self._actions - 2.0 * self._prev_actions + self._prev_prev_actions
smoothness = torch.sum(torch.square(jerk), dim=1) * self.cfg.rew_smoothness
torques = self._robot.data.applied_torque
joint_power = torch.sum(torch.abs(torques) * torch.abs(self._robot.data.joint_vel), dim=1) \
* self.cfg.rew_joint_power
rewards = {"track_lin_vel_xy": track_lin_vel * dt,
"smoothness": smoothness * dt,
"joint_power": joint_power * dt, ...}
total_reward = torch.sum(torch.stack(list(rewards.values())), dim=0)- 1
-
한 스텝 시간(
step_dt). 모든 보상에 곱해 시간 정규화합니다. - 2
- 명령 속도와 실제 속도의 제곱오차.
- 3
- 위 수식 그대로: \exp(-\text{error}/\sigma)에 가중치(+1.0)를 곱함. 오차 0이면 1, 멀어질수록 0으로 부드럽게 감소.
- 4
- 저크 = 2차 차분 a_t - 2a_{t-1} + a_{t-2}. §4에서 보관한 직전 행동들이 여기 쓰입니다. 급격한 행동 변화를 벌함.
- 5
- 관절 출력 \sum|\tau||\omega| — 에너지 효율 페널티(DreamWaQ 고유 보상).
- 6
-
각 항에
* dt를 곱해 dict에 모음. - 7
- 모든 항을 합산 → 스텝당 총 보상.
ManagerBased는 동일 항목을 RewTerm(func=..., weight=...)로 선언하고, mdp/rewards.py에 함수가 있습니다 (예: joint_power_l1, action_smoothness_l2). * dt도 Manager가 자동 처리 — 수학적으로 Direct와 동일합니다.
6. 도메인 Randomization — 코드로 읽기
sim-to-real 격차를 줄이는 핵심입니다. 매 환경마다 물리 특성을 흔들어 과적합을 막습니다. Direct는 원본 IsaacGym과 1:1 대응하도록 메서드로 직접 구현합니다.
# dreamwaq_direct/.../dreamwaq_env.py (마찰 randomization)
def _randomize_friction(self):
materials = wp.to_torch(self._robot.root_physx_view.get_material_properties())
bucket_friction = torch.empty(self.cfg.friction_num_buckets,
device=materials.device).uniform_(0.2, 1.25)
bucket_ids = torch.randint(0, self.cfg.friction_num_buckets, (self.num_envs,))
per_env_friction = bucket_friction[bucket_ids]
materials[..., 0] = per_env_friction.unsqueeze(-1) # static friction
materials[..., 1] = per_env_friction.unsqueeze(-1) # dynamic friction
self._robot.root_physx_view.set_material_properties(...)- 1
-
현재 물리 머티리얼 텐서를 가져옴 (env × shape ×
[static, dynamic, restitution]). - 2
- 마찰값 후보 64개(bucket) 를 [0.2, 1.25]에서 미리 뽑음 — 매 shape마다 난수를 뽑지 않고 후보를 재사용(원본 방식).
- 3
- 각 환경에 bucket 하나를 무작위 배정.
- 4
- 정적·동적 마찰을 env별로 덮어씀.
- 5
- 바뀐 머티리얼을 시뮬레이터에 다시 기록.
다른 randomization도 같은 패턴입니다.
| 메서드 | 시점 | 흔드는 대상 | 범위 |
|---|---|---|---|
_randomize_friction |
startup | 마찰 | 0.2 ~ 1.25 |
_randomize_base_mass |
startup | 몸통 질량 | −1 ~ +2 kg |
_randomize_com |
startup | 질량중심 | ±0.05 m |
_randomize_pd_gains |
startup | PD 게인 | ×0.9 ~ ×1.1 |
_push_robots |
1초마다 | 속도 외란 | ±1.0 m/s |
_push_robots는 외란값을 self._disturb_force에 저장하고, 이 값이 §3의 critic 관측 disturb_force(3)로 들어갑니다.
- 1
- 무작위 속도 충격을 뽑아 저장 — critic 관측으로 노출하기 위함.
- 2
- 현재 속도에 충격을 더해 시뮬레이터에 기록 → 로봇이 “툭” 밀림.
ManagerBased는 같은 일을 EventCfg에 EventTerm으로 선언합니다(mode="startup"/"interval").
7. Go2 설정과 태스크 등록
기본 환경을 상속해 Go2 설정을 얹고 Gymnasium에 등록합니다. 세 변형이 강의 전체의 비교축입니다.
| 변형 | 태스크 ID (Manager) | Actor 입력 | 설명 |
|---|---|---|---|
| Base | DreamWaQ-Manager-Go2-Base-v0 |
45 | 추정 없는 맹목 보행 |
| Oracle | DreamWaQ-Manager-Go2-Oracle-v0 |
48 | 특권 정보 직접 사용 (상한선) |
| Waq | DreamWaQ-Manager-Go2-Waq-v0 |
64 | CENet 추정 (3차시) |
- 1
- 비대칭 학습용 critic 관측 그룹을 추가.
- 2
- Waq critic은 lin_vel을 제외 → 235차원 (actor도 없으므로 대칭 유지).
- 3
- 관측 노이즈를 끔 — CENet이 자체 정규화를 하기 때문. Actor를 45→64로 확장하는 일은 환경이 아니라 3차시 커스텀 runner가 합니다.
8. 핵심 정리
- Go2는 12 DoF, 정책은 목표각 오프셋을 출력하고 PD가 토크로 변환(K_p=20, K_d=0.5).
- 비대칭 actor-critic: actor 45 (선속도·지형 없음), critic 238 (특권). → 3차시 CENet의 출발점.
- 보상은 추종 \exp(-e/\sigma) + 페널티 13항. 도메인 randomization으로 강건성 확보.
- ManagerBased(선언) 와 Direct(명시) 는 같은 로직의 다른 표현 — 결과는 동일.
이 환경 위에서 정책을 어떻게 학습시키는지: PPO-clip 목적함수, GAE, rsl_rl의 롤아웃→갱신 루프, 실행 명령을 코드로 따라갑니다.