SDF를 활용한 로봇 시뮬레이션 예제

SDF 파일 구조 이해

SDF(Simulation Description Format)는 로봇 시뮬레이션을 위한 XML 기반의 포맷이다. SDF 파일은 로봇의 모델, 물리적 속성, 센서, 조인트, 링크 등을 기술하며, 다양한 시뮬레이션 소프트웨어에서 로봇의 시뮬레이션 환경을 설정할 수 있다. SDF 파일의 기본 구조는 다음과 같다.

<sdf version='1.6'>
  <model name='robot_model'>
    <link name='base_link'>
      <!-- 링크의 물리적 속성 정의 -->
      <inertial>
        <mass>1.0</mass>
        <inertia>
          <ixx>0.1</ixx>
          <iyy>0.1</iyy>
          <izz>0.1</izz>
        </inertia>
      </inertial>
      <!-- 충돌 설정 -->
      <collision name='collision'>
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </collision>
      <!-- 시각적 속성 정의 -->
      <visual name='visual'>
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </visual>
    </link>
    <!-- 조인트 정의 -->
    <joint name='joint_1' type='revolute'>
      <parent>base_link</parent>
      <child>link_1</child>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1.57</lower>
          <upper>1.57</upper>
        </limit>
      </axis>
    </joint>
  </model>
</sdf>

위 예제는 기본적인 SDF 파일의 구조로, 로봇 모델의 링크, 관성(inertia), 충돌, 시각적 속성(visual), 조인트 등을 정의하고 있다.

링크의 관성 및 질량 설정

로봇의 움직임 시뮬레이션에서 중요한 요소 중 하나는 각 링크의 관성과 질량이다. SDF 파일에서 이를 정의하기 위해 <inertial> 태그를 사용한다. 관성 행렬 $\mathbf{I}$ 는 다음과 같은 3x3 행렬로 나타낼 수 있다.

$\mathbf{I} = \begin{bmatrix} I_{xx} & I_{xy} & I_{xz} \\ I_{yx} & I_{yy} & I_{yz} \\ I_{zx} & I_{zy} & I_{zz} \end{bmatrix}$

SDF 파일에서 각 성분은 다음과 같이 설정할 수 있다.

<inertial>
  <mass>1.0</mass>
  <inertia>
    <ixx>0.1</ixx>
    <iyy>0.1</iyy>
    <izz>0.1</izz>
  </inertia>
</inertial>

여기서, $I_{xx}, I_{yy}, I_{zz}$ 는 각 링크의 주요 관성 모멘트이며, 링크가 회전할 때 관성의 영향을 계산하는 데 사용된다.

링크의 충돌 및 시각적 요소 설정

링크의 충돌 속성은 시뮬레이션에서 물체 간의 상호작용을 정의하는데 사용된다. 이는 주로 충돌 시 발생하는 힘과 물체의 반응을 시뮬레이션하는데 중요한 역할을 한다. 시각적 요소는 실제로 화면에 보여질 때의 로봇의 외관을 정의한다.

충돌 및 시각적 요소는 각각 <collision>과 <visual> 태그로 정의된다. 예를 들어, 링크의 충돌 영역을 정의할 때는 다음과 같이 정의할 수 있다.

<collision name='collision'>
  <geometry>
    <box>
      <size>1 1 1</size>
    </box>
  </geometry>
</collision>

이와 유사하게 시각적 요소도 동일한 구조를 따른다.

<visual name='visual'>
  <geometry>
    <box>
      <size>1 1 1</size>
    </box>
  </geometry>
</visual>

이 예제에서는 큐브(box) 형태의 링크를 정의했으며, 각 면의 크기는 1x1x1로 설정되어 있다.

조인트 설정

로봇의 움직임을 정의하는 중요한 요소 중 하나는 각 링크 간의 관계를 설정하는 조인트(joint)이다. SDF 파일에서 조인트는 두 링크 사이의 연결을 정의하며, 회전 또는 직선 운동 등의 움직임을 가능하게 한다. 조인트는 다양한 유형이 있으며, 여기서는 회전 조인트(revolute joint)를 예로 들어 설명한다.

회전 조인트 정의

회전 조인트는 두 링크 사이에서 특정 축을 기준으로 회전하는 움직임을 허용한다. SDF 파일에서 회전 조인트를 설정할 때는 type='revolute'로 정의하며, 회전 축과 회전 범위를 설정할 수 있다. 예제는 다음과 같다.

<joint name='joint_1' type='revolute'>
  <parent>base_link</parent>
  <child>link_1</child>
  <axis>
    <xyz>0 0 1</xyz>
    <limit>
      <lower>-1.57</lower>
      <upper>1.57</upper>
    </limit>
  </axis>
</joint>

이 조인트는 base_link와 link_1 사이에서 회전하는 조인트를 정의한다. 회전 축은 xyz로 정의되며, 여기서는 $z$ 축을 기준으로 회전하게 설정되어 있다.

회전 축과 범위

조인트의 회전 축은 3차원 벡터로 정의되며, 위 예제에서는 $(0, 0, 1)$ 로 설정되어 있으므로 $z$ 축을 기준으로 회전한다. 조인트가 회전할 수 있는 범위는 각도 제한으로 설정할 수 있으며, 이 값은 라디안(radians)으로 입력된다.

하한(최소 회전 각도): $-1.57$ 라디안은 약 $-90^\circ$ 에 해당한다.
상한(최대 회전 각도): $1.57$ 라디안은 약 $90^\circ$ 에 해당한다.

따라서, 이 조인트는 $z$ 축을 기준으로 $-90^\circ$ 에서 $90^\circ$ 까지 회전할 수 있다.

Unity에서 SDF 파일 불러오기

Unity는 기본적으로 SDF 파일을 직접 불러오는 기능을 제공하지 않으므로, 이를 위해 추가적인 변환 또는 플러그인이 필요하다. SDF 파일을 Unity에서 로봇 시뮬레이션으로 변환하는 과정은 다음과 같다.

SDF 파일 변환 도구 사용: SDF 파일을 URDF로 변환하거나, Unity에서 사용할 수 있는 포맷으로 변환해야 한다. 이를 위해 SDF를 URDF로 변환하는 도구 또는 플러그인을 사용할 수 있다.
Unity 로봇 플러그인 설치: Unity에서 로봇 시뮬레이션을 위한 플러그인을 설치해야 한다. 이를 통해 변환된 URDF 또는 SDF 파일을 Unity 환경에 통합할 수 있다.
SDF 파일 로딩 및 설정: 변환된 파일을 Unity에 불러온 후, 물리 엔진과 연동하여 로봇의 시뮬레이션을 설정한다. 로봇의 조인트, 링크, 센서 등이 Unity 물리 엔진과 상호작용하도록 설정하는 것이 중요하다.

SDF 파일 변환 코드 예제

SDF 파일을 Unity에서 사용할 수 있는 URDF 포맷으로 변환하는 Python 스크립트 예제는 다음과 같다.

import os
import subprocess

def convert_sdf_to_urdf(sdf_file, urdf_file):
    command = f"sdf2urdf {sdf_file} -o {urdf_file}"
    subprocess.run(command, shell=True, check=True)

if __name__ == "__main__":
    sdf_file = "robot_model.sdf"
    urdf_file = "robot_model.urdf"
    convert_sdf_to_urdf(sdf_file, urdf_file)

이 스크립트는 SDF 파일을 URDF로 변환하여 Unity에서 사용할 수 있게 한다.

Unity에서 URDF로 변환된 로봇 모델 사용

SDF 파일을 URDF로 변환한 후, Unity에서 이를 로딩하고 물리 기반 시뮬레이션을 설정하는 과정은 다음과 같다.

Unity에서 URDF Import

Unity 프로젝트 설정: 먼저 Unity 프로젝트를 새로 생성하거나 기존 프로젝트에 URDF Import 플러그인을 설치한다. Unity Asset Store에서 ROS와의 통합을 위한 ROS#이나 Robot SDK와 같은 플러그인을 사용할 수 있다. 이러한 플러그인은 URDF 형식의 파일을 Unity로 불러오는 기능을 제공한다.
URDF 파일 로딩: 변환된 URDF 파일을 Unity에 임포트한다. 이 과정에서 로봇의 링크와 조인트가 자동으로 인식되어 Unity의 물리 엔진과 상호작용할 수 있다.
물리 설정 조정: Unity에서 URDF 파일이 임포트된 후, 로봇 모델의 물리 설정을 조정한다. 링크 간의 충돌 설정, 관성 설정, 조인트의 회전 및 이동 범위 설정 등을 Unity 내에서 조정할 수 있다.

URDF로 로봇 모델 불러오기 예제

다음은 Unity에서 URDF 파일을 불러오는 C# 코드의 예제이다. 이 코드는 URDF Import 플러그인을 사용하여 로봇 모델을 로드하고, 로봇의 움직임을 시뮬레이션한다.

using UnityEngine;
using RosSharp.Urdf;

public class RobotLoader : MonoBehaviour
{
    public string urdfFilePath;

    void Start()
    {
        LoadURDF();
    }

    void LoadURDF()
    {
        GameObject robot = UrdfRobotExtensions.Load(urdfFilePath);
        if (robot != null)
        {
            Debug.Log("URDF 파일 로딩 성공!");
            robot.transform.position = Vector3.zero;
        }
        else
        {
            Debug.LogError("URDF 파일 로딩 실패!");
        }
    }
}

위 코드는 URDF 파일을 로드하고, 로봇 모델을 Unity의 환경에서 초기화한다. URDF 파일 경로를 입력하면 로봇이 Unity 환경에 생성되며, 물리 엔진과 상호작용하게 된다.

Unity에서 로봇의 움직임 제어

로봇을 Unity 환경에 불러온 후, 로봇의 움직임을 제어할 수 있다. 로봇의 조인트(joint)을 제어하기 위해서는 Unity의 물리 엔진을 활용하여 직접적인 힘(force) 또는 토크(torque)를 적용할 수 있다.

C# 코드로 로봇 조인트 제어

Unity에서 로봇의 조인트를 제어하려면 HingeJoint 또는 ArticulationBody를 사용하여 회전 조인트에 토크를 적용할 수 있다. 예를 들어, 로봇의 팔을 회전시키는 예제는 다음과 같다.

using UnityEngine;

public class RobotArmController : MonoBehaviour
{
    public HingeJoint joint;
    public float torque = 10f;

    void Update()
    {
        JointMotor motor = joint.motor;
        motor.force = torque;
        motor.targetVelocity = 50;
        joint.motor = motor;
    }
}

이 코드에서는 HingeJoint를 사용하여 특정 조인트에 회전 운동을 적용하고, 일정한 힘을 가해 로봇 팔이 회전하도록 설정한다.

센서 시뮬레이션 및 데이터 처리

로봇 시뮬레이션에서 중요한 요소 중 하나는 센서 데이터를 생성하고 처리하는 것이다. Unity에서는 다양한 센서(예: 카메라, IMU, 라이다(LiDAR))를 시뮬레이션할 수 있으며, 이를 통해 로봇의 주변 환경을 감지하고 제어 알고리즘에 반영할 수 있다.

Unity에서 IMU 센서 시뮬레이션

IMU(Inertial Measurement Unit)는 가속도, 각속도, 그리고 때로는 자기장을 측정하는 센서이다. Unity에서는 물리 엔진을 사용하여 IMU 데이터를 생성할 수 있다. IMU 시뮬레이션을 위해서는 Unity의 물리 엔진에서 가속도와 각속도를 계산하고, 센서 노이즈를 추가하여 보다 현실적인 데이터를 생성한다.

IMU 센서 데이터를 생성하는 C# 코드 예제는 다음과 같다.

using UnityEngine;

public class ImuSensor : MonoBehaviour
{
    public Vector3 acceleration;
    public Vector3 angularVelocity;
    public float noiseFactor = 0.02f;

    void FixedUpdate()
    {
        // 가속도 및 각속도 계산
        acceleration = transform.InverseTransformDirection(GetComponent<Rigidbody>().velocity) / Time.fixedDeltaTime;
        angularVelocity = GetComponent<Rigidbody>().angularVelocity;

        // 노이즈 추가
        acceleration += Random.insideUnitSphere * noiseFactor;
        angularVelocity += Random.insideUnitSphere * noiseFactor;

        Debug.Log("가속도: " + acceleration + " / 각속도: " + angularVelocity);
    }
}

이 코드는 Unity의 Rigidbody 컴포넌트를 사용하여 로봇의 가속도와 각속도를 계산하고, IMU 센서 데이터를 시뮬레이션한다. Random.insideUnitSphere를 사용하여 센서 노이즈를 추가하고, 현실적인 IMU 데이터를 출력한다.

LiDAR 시뮬레이션

LiDAR(Light Detection and Ranging) 센서는 레이저를 사용하여 주변 환경의 깊이 정보를 측정한다. Unity에서는 레이캐스트(Raycast) 기능을 사용하여 LiDAR 데이터를 시뮬레이션할 수 있다. Raycast는 가상의 레이저를 특정 방향으로 발사하여 물체와의 충돌 지점을 감지한다.

LiDAR 시뮬레이션을 위한 C# 코드 예제는 다음과 같다.

using UnityEngine;

public class LidarSensor : MonoBehaviour
{
    public float maxDistance = 10f;
    public int rayCount = 360;
    public float noiseFactor = 0.01f;

    void FixedUpdate()
    {
        for (int i = 0; i < rayCount; i++)
        {
            float angle = (360f / rayCount) * i;
            Vector3 direction = Quaternion.Euler(0, angle, 0) * transform.forward;
            RaycastHit hit;

            if (Physics.Raycast(transform.position, direction, out hit, maxDistance))
            {
                float distance = hit.distance + Random.Range(-noiseFactor, noiseFactor);
                Debug.Log("Ray " + i + ": Distance = " + distance);
            }
        }
    }
}

이 코드는 360개의 레이캐스트를 발사하여 주변 물체의 거리 정보를 측정한다. 각 레이캐스트는 로봇의 주변 환경에서 충돌 지점을 감지하며, 센서 노이즈를 추가하여 현실적인 LiDAR 데이터를 시뮬레이션한다.

센서 데이터 처리

센서 데이터를 시뮬레이션한 후, 해당 데이터를 로봇의 제어 알고리즘에 반영하는 과정이 필요하다. Unity에서 생성된 IMU, LiDAR 등의 데이터를 사용하여 로봇의 경로 계획, 충돌 회피, 또는 환경 인식 등의 알고리즘을 구현할 수 있다. 이를 위해 센서 데이터를 실시간으로 처리하고, Unity의 물리 엔진과 상호작용하여 로봇의 동작을 제어한다.

센서 데이터를 제어 알고리즘에 반영하는 예제

다음은 센서 데이터를 활용하여 로봇의 간단한 장애물 회피 알고리즘을 구현하는 예제이다.

using UnityEngine;

public class ObstacleAvoidance : MonoBehaviour
{
    public float speed = 5f;
    public float turnSpeed = 90f;
    public float obstacleDistanceThreshold = 1f;

    private LidarSensor lidar;

    void Start()
    {
        lidar = GetComponent<LidarSensor>();
    }

    void Update()
    {
        bool obstacleDetected = false;

        for (int i = 0; i < lidar.rayCount; i++)
        {
            float distance = lidar.GetRayDistance(i);

            if (distance < obstacleDistanceThreshold)
            {
                obstacleDetected = true;
                break;
            }
        }

        if (obstacleDetected)
        {
            // 장애물이 있으면 회전
            transform.Rotate(0, turnSpeed * Time.deltaTime, 0);
        }
        else
        {
            // 장애물이 없으면 전진
            transform.Translate(Vector3.forward * speed * Time.deltaTime);
        }
    }
}

이 코드는 LiDAR 센서 데이터를 사용하여 로봇이 장애물을 감지하면 회전하고, 장애물이 없을 경우 전진하는 간단한 장애물 회피 알고리즘을 구현한 예제이다.