강체 객체와 상호작용하기

# 필수 라이브러리 및 모듈 가져오기
import argparse
from omni.isaac.lab.app import AppLauncher

# ArgumentParser() : 명령줄 인수를 처리하는데 사용되는 ArgumentParser 인스턴스 생성
# description은 --help 인수를 사용할때 표시됨. 스크립트에 대한 간략한 설명이 포함되어있음
parser = argparse.ArgumentParser(description="Tutorial on spawning and interacting with a rigid object.")

# AppLauncher 클래스에 필요한 명령줄 인수 parser을 추가
AppLauncher.add_app_launcher_args(parser)

# parser.parse_args() 스크립트 실행 시 제공된 명령줄 인수를 처리
args_cli = parser.parse_args()

# launch omniverse app
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app


# 텐서 계산에 사용. 텐서는 다차원 배열이며 여기에서는 시뮬레이션 내의 객체의 위치 및 방향에 대한 변환 및 작업을 처리하는데 사용
import torch

# prim_utils : scene에 생성할 다양한 객체를 생성하고 관리하기 위한 도구
import omni.isaac.core.utils.prims as prim_utils

# sim_utils : 물리 구성, 조명 및 객체 생성을 포함한 여러 시뮬레이션 유틸리티가 포함된 도구
import omni.isaac.lab.sim as sim_utils

# math_utils : 시뮬레이션에서 객체를 조작하는 사용할 수 있는 난수 생성 및 기하학적 변환과 같은 수학적 유틸리티 제공
# 여기에서는 특정 영역 내 위치를 샘플링하는데 사용됨
import omni.isaac.lab.utils.math as math_utils

# RigidObject : 강체 객체를 표현하는데 사용되는 클래스. 강체는 힘이 가해질때 변형되지 않는 단단한 물체임
# RigidObjectCfg : 초기 위치, 모양, 질량, 충돌 속성 등 강체의 속성을 정의하는 클래스
from omni.isaac.lab.assets import RigidObject, RigidObjectCfg

# SimulationContext : 각 프레임 단계별 실행, 물리 관리, 장면의 개체 제어 등 시뮬레이션의 모든 측면을 처리하는데 사용
from omni.isaac.lab.sim import SimulationContext


def design_scene():
    
    # 지면 생성
    cfg = sim_utils.GroundPlaneCfg()
    cfg.func("/World/defaultGroundPlane", cfg)
    
    # 돔광원 생성
    cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.8, 0.8, 0.8))
    cfg.func("/World/Light", cfg)

    # origins : scene의 4개의 위치를 정의
    origins = [[0.25, 0.25, 0.0], [-0.25, 0.25, 0.0], [0.25, -0.25, 0.0], [-0.25, -0.25, 0.0]]
    
    # for문을 돌면서 origin의 지정된 원점이 Xform 노드를 생성. Origin0~3 까지 생성.
    for i, origin in enumerate(origins):
        prim_utils.create_prim(f"/World/Origin{i}", "Xform", translation=origin)



    
    cone_cfg = RigidObjectCfg(
        prim_path="/World/Origin.*/Cone",
        spawn=sim_utils.ConeCfg(
            radius=0.1,
            height=0.2,
            rigid_props=sim_utils.RigidBodyPropertiesCfg(),
            mass_props=sim_utils.MassPropertiesCfg(mass=1.0),
            collision_props=sim_utils.CollisionPropertiesCfg(),
            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2),
        ),
        init_state=RigidObjectCfg.InitialStateCfg(),
    )
    cone_object = RigidObject(cfg=cone_cfg)
    

RigidObjectCfg()

1.RigidObjectCfg()
- 원뿔 강체를 만듦
- 강체는 변형되지 않고 시뮬레이션에서 힘 및 충돌과 상호작용이 가능
2. prim_path="/World/Origin./Cone"
- 원뿔이 생성될 경로
- 경로의 ''는 원뿔이 Origin0~3 아래에 배치될 수 있음을 의미
3. spawn=sim_utils.ConeCfg()
- radius=0.1 : 반지름 0.1
- height=0.2 : 높이 0.2
- rigid_props=sim_utils.RigidBodyPropertiesCfg() : 원뿔에 물리학을 추가하여 다른 개체와 충돌 가능케함
- mass_props=sim_utils.MassPropertiesCfg(mass=1.0) : 원뿔의 질량이 1kg 이며 중력 및 충돌과 같은 힘에 반응
- collision_props=sim_utils.CollisionPropertiesCfg() : 원뿔에 충돌 감지를 추가해 scene의 다른 객체와 물리적으로 상호작용할 수 있게함
- PreviewSurfaceCfg : diffuse_color 는 색상. metallic 는 약간의 금속성 느낌을 줌을 의미

4. RigidObjectCfg.InitialStateCfg()
   • 강체의 초기 상태를 정의하는 클래스
   • 특정한 매개변수를 제공하지 않으면 위치는 (0,0,0) 방향은 회전 없이, 속도도 없이 즉 객체가 정지된 상태임

5.RigidObject(cfg=cone_cfg)
- cone_cfg 에 정의된 구성을 기반으로 원뿔 object를 생성

    # scene_entities : 원뿔 객체를 저장하는 dict. 나중에 시뮬레이션에서 상호 작용이나 업데이트를 위채 원뿔을 참조하는 기능을 제공
    scene_entities = {"cone": cone_object}
    
    # 원뿔 객체와 원점을 return
    return scene_entities, origins

"""
run_simulator 함수의 인자들
sim: sim_utils.SimulationContext : 물리학(충돌, 중력, 힘), 시간(시뮬레이션이 시간에 따라 어떻게 진행되는지), 개체간의 상호작용 을 포함한 전체 시뮬레이션 환경을 관리

entities: dict[str, RigidObject]: entities dict를 활용하여 시뮬레이션 중 cone을 액세스하고 그 상태를 조작함

origins: torch.Tensor : origins의 (x,y,z) 좌표들을 나타내는 2D 텐서(행렬)을 의미. 텐서는 병렬로 처리될 수 있어 작업 속도가 빨라져 사용

"""


def run_simulator(sim: sim_utils.SimulationContext, entities: dict[str, RigidObject], origins: torch.Tensor):

    # entities dict에서 원뿔 객체를 추출해 cone_object에 저장. 
    # design_scene() 에서 생성된 원뿔 개체는 시뮬레이션 루프에서 업데이트 되고 상호작용함.
    cone_object = entities["cone"]
    
    '''
    sim.get_physics_dt() : 시뮬레이션의 물리 엔진에 대한 dt를 검색
    dt는 루프가 반복될 때마다 시간이 얼마나 진행되는지 정의함.
    값이 작을수록 시뮬레이션이 더 정확하지만 속도가 느려짐
    sim_time = 0.0 : 시뮬레이션 현재 시간을 저장함. 0.0에서 시작하며 시뮬레이션 진행됨에 따라 증가
    count = 0 : 시뮬레이션 단계 수를 계산. 특정 작업(객체 상태 재설정)이 발생해야 하는 시기를 제어하는 데 사용됨
    '''
    sim_dt = sim.get_physics_dt()
    sim_time = 0.0
    count = 0
    
    # isaac sim 애플리케이션이 계속 실행되는 동안 지속적으로 시뮬레이션 실행
    while simulation_app.is_running():
        # 시뮬레이션이 250단계를 완료했다면 객체 상태(위치, 속도)가 초기 상태로 리셋
        if count % 250 == 0:
           	# sim_time과 count도 0으로 초기화
            sim_time = 0.0
            count = 0
            
            # default_root_state.clone() : 객체의 초기 또는 기본 상태를 복제함. root_state = 기저의 상태 즉 처음의 상태라고 보면됨
            root_state = cone_object.data.default_root_state.clone()
            
            
            '''
            root_state 는 시뮬레이션에서 원뿔의 위치 및 방향을 저장하는 텐서이다.
            이 텐서는 위치, 속도, 방향, 등 객체의 상태와 관련된 데이터를 포함한다.
            특히 3D 공간에서의 위치를 포함하는 각 객체의 기본 상태가 저장되는 곳임
            
            [:, :3] 슬라이싱을 사용하여 root_state 텐서의 일부를 선택
            첫번째 콜론 은 모든 행을 포함(각 행은 원뿔 객체의 다른 인스턴스를 나타냄) 따라서 : 는 원뿔의 모든 인스턴스를 나타냄
            두번째 콜론 :3 은 3D 공간 위치의 x,y,z 좌표를 나타내는 텐서의 처음 세 열을 선택한다는 것임
            
            즉 root_state[:, :3]는 scene에 있는 모든 원뿔 객체 위치의 x,y,z 좌표를나타냄
            
            root_state[:, :3] += origins
            객체의 위치에 기존에 정의했던 origins를 추가함
            ex) [0.25, 0.25, 0.0] 인 경우 원뿔 객체의 위치를 x,y축을 따라 0.25 이동하고 z축은 변경되징 않는다. 즉 원점 근처에 배치된다.
            
            root_state[:, :3] += math_utils.sample_cylinder(
                radius=0.1, h_range=(0.25, 0.5), size=cone_object.num_instances, device=cone_object.device
                math_utils.sample_cylinder(): 이 함수는 원통 표면의 무작위 위치를 샘플링함.
                radius = 0.1 반지름을 의미
                h_range=(0.25, 0.5) 는 원통의 수직 범위가 0.25~0.5 사이 무작위로 선택됨
                size=cone_object.num_instances 는 생성할 무작위 샘플 수를 지정
                device=cone_object.device 는 텐서 작업이 GPU에 수행되도록함
                
                위 두가지 작업 후 원뿔 객체의 위치는 먼저 원점 지점을 기준으로 조정된 다음, 무작위로 배치됨
                
                이렇게 하면 원뿔이 모두 원점에 정확히 배치되지 않고 작은 원통형 영역 내에서 원뿔이 주위에 배치가 되어 더욱 현실감 있게 보임
            '''
            root_state[:, :3] += origins
            root_state[:, :3] += math_utils.sample_cylinder(
                radius=0.1, h_range=(0.25, 0.5), size=cone_object.num_instances, device=cone_object.device
            )
            
            # 새로운 무작위 위치가 시뮬레이션에 다시 기록되어 원뿔 개체의 위치가 업데이트됨
            cone_object.write_root_state_to_sim(root_state)
            
            # 임시 데이터를 지우고 다음 시뮬레이션 단계를 위해 객체를 준비
            cone_object.reset()
            
            print("----------------------------------------")
            print("[INFO]: Resetting object state...")
            
        # 업데이트된 데이터(위치, 속도, 방향등)가 원뿔 개체에 대한 시뮬레이션에 다시 기록
        cone_object.write_data_to_sim()
        
        # perform step
        sim.step()
        
        # 각 반복에 대해 sim_dt를 추가하여 업데이트
        sim_time += sim_dt
        count += 1
        
        # 현재 시뮬레이션 시간을 기준으로 원뿔 개체의 내부 버퍼와 상태를 업데이트
        cone_object.update(sim_dt)
        
        # 50 단계마다 원뿔의 위치를 print함
        if count % 50 == 0:
            print(f"Root position (in world): {cone_object.data.root_state_w[:, :3]}")

def main():
    # 시뮬레이션을 위한 구성 개체를 생성. device=args_cli.device : GPU 사용하겠다는 의미
    sim_cfg = sim_utils.SimulationCfg(device=args_cli.device)
    
    # 방금 생성된 sim_cfg 구성을 사용하여 SimulationContext의 새 인스턴스 생성
    # SimulationContext 는 시뮬레이션을 관리하고 실행하면서 물리 계싼부터 개체 업데이트까지 모든 것을 처리
    sim = SimulationContext(sim_cfg)
    
    
    # Set main camera
    sim.set_camera_view(eye=[1.5, 0.0, 1.0], target=[0.0, 0.0, 0.0])
    
    # Design scene
    scene_entities, scene_origins = design_scene()
    
    # 객체의 좌표를 tensor로 변환 후 gpu에서 사용하게 설정
    scene_origins = torch.tensor(scene_origins, device=sim.device)
    
    # 시뮬레이션 초기 상태로 재설정
    sim.reset()
    
    print("[INFO]: Setup complete...")
    run_simulator(sim, scene_entities, scene_origins)


if __name__ == "__main__":
    # run the main function
    main()
    # close sim app
    simulation_app.close()