# Gazebo 플러그인 활용

#### Gazebo 플러그인의 개요

Gazebo 플러그인은 로봇 시뮬레이션에서 특정 기능을 확장하거나, 기존의 기능을 세부적으로 제어할 수 있도록 하는 소프트웨어 컴포넌트이다. 플러그인은 로봇 모델의 물리적 속성, 센서 데이터의 수집 및 처리, 제어 시스템과의 통합 등을 구현하는 데 중요한 역할을 한다. URDF 또는 SDF 파일과 함께 사용되며, 로봇의 동작을 제어하거나 시뮬레이션 환경을 제어할 수 있다.

플러그인은 크게 두 가지 역할을 담당한다. 첫 번째는 **로봇의 동작을 제어**하는 것이고, 두 번째는 **시뮬레이션 환경과의 상호작용**을 관리하는 것이다. 이를 통해 사용자는 복잡한 시뮬레이션 환경에서 로봇의 정확한 동작을 모델링하고, 다양한 환경에서 로봇의 성능을 테스트할 수 있다.

#### Gazebo 플러그인의 주요 기능

1. **로봇 제어**
   * Gazebo 플러그인은 로봇의 모터 제어, 조인트 움직임, 센서의 데이터 수집 및 처리를 담당할 수 있다. 로봇의 동작 제어는 주로 PID 제어기를 통해 이루어지며, 이를 통해 사용자는 로봇의 조인트나 휠의 정확한 위치 및 속도를 제어할 수 있다.
   * PID 제어기의 수식은 다음과 같다:

$$
u(t) = K\_p \cdot e(t) + K\_i \cdot \int\_0^t e(\tau) d\tau + K\_d \cdot \frac{d e(t)}{dt}
$$

여기서:

* $u(t)$는 제어 신호
* $e(t)$는 목표값과 실제값의 차이(오차)
* $K\_p$, $K\_i$, $K\_d$는 각각 비례, 적분, 미분 게인이다.

2. **센서 데이터 처리**
   * Gazebo 플러그인을 통해 사용자는 로봇에 장착된 카메라, LiDAR, IMU와 같은 센서로부터 데이터를 수집하고, 해당 데이터를 실시간으로 처리할 수 있다.
   * 센서 데이터의 수집과 처리는 주로 센서 플러그인을 통해 이루어지며, 이를 통해 로봇의 위치, 속도, 가속도 등의 물리적 데이터를 분석하고 처리할 수 있다.
   * 예를 들어, IMU 센서에서 가속도 데이터를 수집하는 공식은 다음과 같다:

$$
\mathbf{a} = \frac{d\mathbf{v}}{dt}
$$

여기서:

* $\mathbf{a}$는 가속도 벡터
* $\mathbf{v}$는 속도 벡터이다.

#### Gazebo 플러그인의 구조

Gazebo 플러그인은 크게 **월드 플러그인**, **모델 플러그인**, **센서 플러그인**으로 구분할 수 있다.

1. **월드 플러그인**은 시뮬레이션 전체를 제어하는 데 사용된다. 이 플러그인을 통해 시뮬레이션 환경을 설정하고, 다양한 객체를 추가하거나 삭제할 수 있다.
2. **모델 플러그인**은 로봇 모델 자체를 제어한다. 이 플러그인을 사용하여 로봇의 동작, 조인트 제어, 충돌 처리 등을 구현할 수 있다.
3. **센서 플러그인**은 로봇에 장착된 센서를 제어하고, 센서 데이터를 처리한다. 이를 통해 사용자는 시뮬레이션 내에서 센서 데이터를 실시간으로 수집하고 분석할 수 있다.

#### URDF와 SDF에서 Gazebo 플러그인 설정

플러그인은 URDF 또는 SDF 파일에서 직접 설정할 수 있다. 각 플러그인은 XML 태그로 정의되며, 플러그인의 이름과 파라미터를 지정할 수 있다. 예를 들어, URDF에서 플러그인을 설정하는 방법은 다음과 같다:

```xml
<gazebo>
  <plugin name="my_plugin" filename="libmy_plugin.so">
    <param1>value1</param1>
    <param2>value2</param2>
  </plugin>
</gazebo>
```

SDF에서도 유사하게 플러그인을 설정할 수 있으며, SDF의 플러그인 구조는 URDF보다 더 유연하고 확장성이 높다.

#### Gazebo 플러그인의 종류와 사용 사례

1. **월드 플러그인**
   * 월드 플러그인은 전체 시뮬레이션 환경을 제어한다. 예를 들어, 특정 시간이 경과한 후 시뮬레이션에 새로운 객체를 추가하거나, 기존 객체를 제거할 수 있다. 또한, 시뮬레이션 중에 중력이나 마찰 계수와 같은 물리적 특성을 동적으로 변경할 수 있다.
   * 월드 플러그인의 일반적인 사용 사례는 다음과 같다:
     * 시뮬레이션 시간에 따른 환경 변화
     * 로봇의 동작에 따른 실시간 환경 반응
     * 다중 로봇 시스템의 환경 내 상호작용
2. **모델 플러그인**
   * 모델 플러그인은 로봇 자체의 동작을 제어하는 데 사용된다. 이 플러그인을 통해 사용자는 로봇의 특정 부위를 움직이거나, 센서 데이터를 분석하여 로봇의 행동을 결정할 수 있다.
   * 예를 들어, 6자유도 로봇 팔의 조인트를 제어하는 플러그인을 작성할 수 있다. 플러그인은 PID 제어기와 결합하여 각 조인트의 각도를 제어하고, 목표 위치에 도달할 수 있도록 도와준다.
   * 이때, 각 조인트의 회전 행렬은 다음과 같이 표현될 수 있다:

$$
\mathbf{R}\_z(\theta) = \begin{bmatrix} \cos{\theta} & -\sin{\theta} & 0 \ \sin{\theta} & \cos{\theta} & 0 \ 0 & 0 & 1 \end{bmatrix}
$$

여기서:

* $\theta$는 회전각이다.

이 회전 행렬은 $z$-축을 기준으로 로봇 조인트의 회전을 설명한다. 이를 통해 로봇의 조인트가 주어진 각도로 회전하며, 플러그인은 로봇의 동작을 제어할 수 있다.

3. **센서 플러그인**
   * 센서 플러그인은 시뮬레이션 내에서 로봇에 장착된 센서들을 제어한다. 예를 들어, 카메라 센서를 사용하여 로봇 주변의 이미지를 수집하거나, LiDAR 센서를 사용하여 로봇 주변의 거리 데이터를 측정할 수 있다.
   * 카메라 센서에서 이미지를 수집하는 경우, 플러그인은 특정 주기로 이미지를 캡처하고 이를 시뮬레이션 내에서 사용할 수 있도록 설정한다. LiDAR 센서의 경우, 다음과 같은 거리 측정 공식을 사용할 수 있다:

$$
d = \sqrt{(x\_2 - x\_1)^2 + (y\_2 - y\_1)^2 + (z\_2 - z\_1)^2}
$$

여기서:

* $d$는 두 점 사이의 거리
* $(x\_1, y\_1, z\_1)$과 $(x\_2, y\_2, z\_2)$는 두 점의 좌표이다.

센서 플러그인은 이러한 데이터를 실시간으로 수집하고, 로봇의 동작에 반영하거나 외부 시스템에 전달할 수 있다.

#### 플러그인 개발 및 코드 구조

Gazebo 플러그인을 개발할 때는 C++ 언어를 주로 사용하며, Gazebo API를 활용하여 플러그인의 동작을 정의한다. 플러그인 개발은 주로 세 가지 단계로 나눌 수 있다:

1. **플러그인 초기화**
   * 플러그인이 처음 로드될 때, 초기화 과정을 거친다. 이 과정에서 플러그인은 필요한 파라미터를 설정하고, 시뮬레이션과 상호작용할 준비를 한다.
   * 초기화 단계에서는 URDF나 SDF 파일에서 전달된 파라미터 값을 읽어들이다. 예를 들어, 조인트의 초기 위치나 속도를 설정할 수 있다.
2. **플러그인 업데이트**
   * 플러그인은 시뮬레이션이 진행되는 동안 주기적으로 업데이트된다. 업데이트 함수는 주어진 주기마다 호출되며, 이때 로봇의 동작을 제어하거나, 센서 데이터를 수집하고 처리할 수 있다.
   * 업데이트 주기는 주로 시뮬레이션의 시간 단위로 설정되며, 사용자는 이를 통해 로봇의 동작 속도를 조정할 수 있다. 일반적으로 1ms 또는 10ms 단위로 설정된다.
3. **플러그인 종료**
   * 시뮬레이션이 종료되면, 플러그인은 이를 감지하고 종료 절차를 밟습니다. 이때 로봇의 상태를 저장하거나, 필요한 데이터를 기록할 수 있다. 또한, 시뮬레이션 중에 사용된 자원을 해제하고, 메모리 누수가 발생하지 않도록 관리한다.

#### Gazebo 플러그인 개발 예제

다음은 간단한 Gazebo 모델 플러그인을 개발하는 예제이다. 이 플러그인은 로봇의 조인트를 제어하는 역할을 한다. 플러그인은 기본적으로 C++로 작성되며, Gazebo API를 사용하여 구현된다.

**1. 플러그인 헤더 파일**

먼저 플러그인의 헤더 파일을 정의한다. 여기서는 플러그인 클래스의 선언과 주요 변수 및 함수들을 정의한다.

```cpp
#ifndef _MY_PLUGIN_HH_
#define _MY_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ros/ros.h>
#include <std_msgs/Float64.h>

namespace gazebo
{
  class MyPlugin : public ModelPlugin
  {
  public:
    MyPlugin() : ModelPlugin() {}

    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

  private:
    void OnUpdate();

    // ROS 노드 핸들러
    ros::NodeHandle nh_;

    // ROS Subscriber
    ros::Subscriber joint_sub_;

    // 업데이트 이벤트 연결
    event::ConnectionPtr updateConnection_;

    // 조인트 포인터
    physics::JointPtr joint_;

    // 제어 입력 값
    double input_;
  };
}
#endif
```

**2. 플러그인 구현 파일**

플러그인의 동작을 구현하는 `.cpp` 파일이다. 여기서 조인트 제어 로직을 작성한다.

```cpp
#include "my_plugin.hh"
#include <functional>

namespace gazebo
{
  void MyPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
  {
    // Gazebo 모델의 조인트 가져오기
    this->joint_ = _model->GetJoint("joint_name");

    // ROS Subscriber 설정
    this->joint_sub_ = nh_.subscribe("/joint_input", 1, &MyPlugin::OnUpdate, this);

    // 업데이트 이벤트 연결
    this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
      std::bind(&MyPlugin::OnUpdate, this));
  }

  void MyPlugin::OnUpdate()
  {
    // ROS로부터 입력값 받기
    ros::spinOnce();

    // 조인트 제어
    this->joint_->SetForce(0, this->input_);
  }
}

GZ_REGISTER_MODEL_PLUGIN(MyPlugin)
```

**3. 플러그인 동작 원리**

* **Load 함수**: 이 함수는 플러그인이 처음 로드될 때 호출된다. 로봇 모델에서 특정 조인트를 가져와 제어할 준비를 한다. 또한 ROS 노드를 초기화하고, ROS 메시지를 수신할 수 있도록 Subscriber를 설정한다.
* **OnUpdate 함수**: 이 함수는 Gazebo 시뮬레이션이 업데이트될 때마다 호출된다. ROS로부터 입력값을 수신하고, 이를 사용하여 로봇 조인트에 힘을 가한다. 이때, 조인트에 가해지는 힘은 $\mathbf{F}$로 표현할 수 있으며, 다음과 같이 정의된다:

$$
\mathbf{F} = m \mathbf{a}
$$

여기서:

* $\mathbf{F}$는 조인트에 가해지는 힘
* $m$은 조인트의 질량
* $\mathbf{a}$는 조인트의 가속도이다.

이 예제에서는 조인트에 직접 힘을 가하는 방식으로 동작을 제어한다. 실제 시뮬레이션에서는 PID 제어기나 더욱 정밀한 제어 방법을 사용할 수 있다.

**4. 플러그인 컴파일 및 실행**

플러그인을 컴파일하려면 `CMakeLists.txt` 파일을 작성하고, 이를 통해 C++ 파일을 빌드해야 한다. 플러그인 컴파일 후, URDF나 SDF 파일에서 플러그인을 호출하여 시뮬레이션에서 실행할 수 있다.

```xml
<gazebo>
  <plugin name="my_plugin" filename="libmy_plugin.so"/>
</gazebo>
```

URDF나 SDF 파일에 위와 같이 플러그인을 정의한 후, Gazebo에서 시뮬레이션을 실행하면 플러그인이 로드되고, 로봇의 조인트 제어가 가능한다.

#### 5. Gazebo 플러그인의 주요 함수 및 이벤트

Gazebo 플러그인은 다양한 이벤트와 함수를 제공하여 시뮬레이션의 여러 측면을 제어할 수 있다. 이러한 함수와 이벤트는 플러그인 내부에서 시뮬레이션의 특정 상태나 동작을 감지하고 처리하는 데 사용된다.

**주요 함수**

1. **Load()**

   * 플러그인이 로드될 때 호출되는 함수이다. 시뮬레이션이 시작될 때 로봇 모델과 관련된 파라미터를 초기화하고 필요한 요소들을 설정한다.
   * 예를 들어, 로봇 모델에서 특정 링크나 조인트를 가져와 제어할 수 있으며, 센서를 초기화할 수 있다.

   ```cpp
   void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
   {
       // 모델의 특정 조인트를 가져오기
       this->joint_ = _model->GetJoint("joint_name");
   }
   ```
2. **Init()**
   * 플러그인이 초기화된 후, 시뮬레이션이 시작되기 전에 호출된다. 시뮬레이션 중 사용할 변수를 초기화하거나, 첫 번째 업데이트 전에 필요한 계산을 수행할 수 있다.
3. **Reset()**

   * 시뮬레이션이 다시 시작될 때 호출된다. 이 함수는 시뮬레이션이 중단되었다가 재시작될 때나, 시뮬레이션이 리셋될 때 사용된다. 플러그인의 상태를 초기화하고, 변수나 상태 값을 리셋하는 데 사용된다.

   ```cpp
   void Reset()
   {
       // 변수를 초기 상태로 리셋
       this->input_ = 0.0;
   }
   ```
4. **OnUpdate()**

   * 시뮬레이션이 업데이트될 때마다 주기적으로 호출된다. 이 함수는 플러그인의 주된 제어 로직이 실행되는 곳이다. 주기적으로 호출되기 때문에, 로봇의 동작을 실시간으로 제어하거나, 센서 데이터를 수집하고 처리할 수 있다.
   * 시뮬레이션에서의 시간은 고정된 주기로 흘러가며, 일반적으로 1ms 또는 10ms 주기로 이 함수가 호출된다.

   ```cpp
   void OnUpdate()
   {
       // 주기적으로 조인트 제어
       this->joint_->SetForce(0, this->input_);
   }
   ```

**주요 이벤트**

1. **ConnectWorldUpdateBegin()**

   * 시뮬레이션이 업데이트되기 시작할 때 발생하는 이벤트이다. 플러그인은 이 이벤트에 연결되어 주기적으로 OnUpdate 함수를 호출할 수 있다.

   ```cpp
   this->updateConnection = event::Events::ConnectWorldUpdateBegin(
       std::bind(&MyPlugin::OnUpdate, this));
   ```
2. **ConnectWorldUpdateEnd()**
   * 시뮬레이션이 업데이트를 마친 후 발생하는 이벤트이다. 이를 통해 시뮬레이션의 프레임이 종료된 후 추가적인 작업을 수행할 수 있다.
3. **ConnectBeforePhysicsUpdate()**
   * 물리 엔진이 업데이트되기 직전에 발생하는 이벤트이다. 이를 통해 물리 연산이 적용되기 전에 로봇의 상태를 조정할 수 있다. 예를 들어, 로봇의 위치나 속도를 조정할 수 있다.
4. **ConnectAfterPhysicsUpdate()**
   * 물리 엔진이 업데이트된 후 발생하는 이벤트이다. 이 이벤트는 물리 엔진이 계산된 데이터를 기반으로 로봇의 상태를 업데이트한 후, 추가적인 작업을 수행하는 데 사용된다.

#### Gazebo 플러그인에서 물리 엔진 제어

Gazebo에서 로봇의 물리적 동작을 제어하려면, 물리 엔진과의 상호작용이 필요하다. Gazebo는 ODE, Bullet, DART, Simbody와 같은 여러 물리 엔진을 지원하며, 각 엔진은 물리적 동작을 시뮬레이션하는 데 필요한 다양한 기능을 제공한다.

1. **조인트 제어**
   * Gazebo 플러그인은 조인트의 위치, 속도, 가속도 등을 제어할 수 있다. 조인트의 위치를 제어할 때는 다음과 같은 방식으로 각도를 설정할 수 있다:

$$
\theta(t) = \theta\_0 + \omega t + \frac{1}{2} \alpha t^2
$$

여기서:

* $\theta(t)$는 시간 $t$에서의 조인트 각도
* $\theta\_0$는 초기 각도
* $\omega$는 각속도
* $\alpha$는 각가속도이다.

2. **힘과 토크 제어**
   * 조인트에 힘이나 토크를 가하는 방식으로 동작을 제어할 수 있다. 이때 힘과 토크는 뉴턴의 운동 법칙에 따라 결정된다:

$$
\mathbf{F} = m \mathbf{a}
$$

$$
\tau = I \alpha
$$

여기서:

* $\mathbf{F}$는 힘
* $m$은 질량
* $\mathbf{a}$는 가속도
* $\tau$는 토크
* $I$는 관성 모멘트
* $\alpha$는 각가속도이다.

3. **조인트 속도 제어**
   * 조인트의 속도를 제어하는 방법도 지원된다. PID 제어기를 사용하여 목표 속도에 도달할 수 있도록 조인트에 힘을 가하는 방식이다. PID 제어기는 다음과 같이 정의된다:

$$
u(t) = K\_p \cdot e(t) + K\_i \cdot \int\_0^t e(\tau) d\tau + K\_d \cdot \frac{d e(t)}{dt}
$$

여기서:

* $u(t)$는 제어 신호
* $e(t)$는 목표값과 실제값의 차이 (오차)
* $K\_p$, $K\_i$, $K\_d$는 각각 비례, 적분, 미분 게인이다.

#### Gazebo 플러그인에서 PID 제어기의 구현

Gazebo에서 로봇의 조인트나 다른 물리적 요소를 제어할 때 PID 제어기를 자주 사용한다. PID 제어기는 목표값과 실제값의 차이를 줄이기 위한 제어 알고리즘으로, Gazebo 플러그인에서 중요한 역할을 한다. PID 제어기는 Proportional(비례), Integral(적분), Derivative(미분) 세 가지 요소로 이루어져 있다.

**PID 제어기의 구성 요소**

1. **비례 제어 (Proportional Control, $P$)**
   * 비례 제어는 목표값과 실제값 간의 오차에 비례하여 제어 신호를 생성한다. 오차가 클수록 더 큰 제어 신호가 발생하며, 오차가 작아질수록 제어 신호도 작아진다.

$$
P = K\_p \cdot e(t)
$$

여기서:

* $P$는 비례 제어 신호
* $K\_p$는 비례 게인
* $e(t)$는 목표값과 실제값의 차이 (오차)이다.

2. **적분 제어 (Integral Control, $I$)**
   * 적분 제어는 시간에 따라 누적된 오차를 기반으로 제어 신호를 생성한다. 이는 시스템이 목표값에 천천히 도달하는 경향이 있을 때, 오차를 줄이기 위한 신호를 제공한다.

$$
I = K\_i \cdot \int\_0^t e(\tau) d\tau
$$

여기서:

* $I$는 적분 제어 신호
* $K\_i$는 적분 게인
* $\int\_0^t e(\tau) d\tau$는 시간에 따른 오차의 누적이다.

3. **미분 제어 (Derivative Control, $D$)**
   * 미분 제어는 오차의 변화율에 따라 제어 신호를 생성한다. 이는 시스템의 과도한 반응을 줄이기 위한 요소로 작용하며, 오차가 빠르게 변화하는 경우 이를 억제한다.

$$
D = K\_d \cdot \frac{d e(t)}{dt}
$$

여기서:

* $D$는 미분 제어 신호
* $K\_d$는 미분 게인
* $\frac{d e(t)}{dt}$는 오차의 변화율이다.

**PID 제어기 전체 수식**

PID 제어기의 전체 제어 신호는 비례, 적분, 미분 제어의 합으로 이루어진다:

$$
u(t) = K\_p \cdot e(t) + K\_i \cdot \int\_0^t e(\tau) d\tau + K\_d \cdot \frac{d e(t)}{dt}
$$

여기서:

* $u(t)$는 전체 제어 신호이다.
* $K\_p$, $K\_i$, $K\_d$는 각각 비례, 적분, 미분 게인이다.
* $e(t)$는 목표값과 실제값 간의 오차이다.

**Gazebo에서 PID 제어기 사용하기**

Gazebo 플러그인에서 PID 제어기를 사용하려면, 조인트 또는 링크에 PID 제어기를 설정하고 주기적으로 오차를 계산하여 제어 신호를 생성한다. 이는 Gazebo의 업데이트 함수에서 주기적으로 계산되며, 각 조인트에 필요한 힘이나 토크를 적용하는 방식으로 구현된다.

```cpp
// PID 제어기 초기화
common::PID pid(0.1, 0.01, 0.001);

// 목표 각도 설정
double target_position = 1.57;  // 라디안 단위 (90도)

// 현재 조인트의 각도 가져오기
double current_position = this->joint_->Position(0);

// 오차 계산
double error = target_position - current_position;

// PID 제어기를 사용하여 힘 계산
double force = pid.Update(error, dt);

// 계산된 힘을 조인트에 적용
this->joint_->SetForce(0, force);
```

위 예제에서는 PID 제어기를 사용하여 조인트의 목표 각도와 실제 각도 간의 오차를 계산하고, 이를 기반으로 조인트에 힘을 가하는 방식으로 제어가 이루어진다.

**PID 튜닝**

PID 제어기의 성능은 $K\_p$, $K\_i$, $K\_d$ 값을 어떻게 설정하느냐에 따라 크게 달라진다. 이러한 값을 설정하는 과정을 **PID 튜닝**이라고 하며, 시스템의 반응 특성에 맞게 조정해야 한다.

* **$K\_p$ (비례 게인)**: 너무 크면 시스템이 불안정해질 수 있고, 너무 작으면 시스템이 느리게 반응한다.
* **$K\_i$ (적분 게인)**: 적분 제어는 오차의 누적을 처리하므로, $K\_i$ 값이 너무 크면 시스템이 과도하게 반응할 수 있다.
* **$K\_d$ (미분 게인)**: 미분 제어는 빠른 오차 변화를 억제하므로, $K\_d$ 값이 너무 크면 시스템이 반응을 너무 억제하게 되어 느려질 수 있다.

**PID 제어기의 안정성 분석**

PID 제어기의 안정성을 분석할 때, \*\*루트 궤적(root locus)\*\*을 사용할 수 있다. 루트 궤적은 시스템의 특성 방정식의 근이 제어 파라미터에 따라 어떻게 변하는지를 나타낸다. 루트 궤적 분석을 통해 시스템의 안정성을 보장하는 $K\_p$, $K\_i$, $K\_d$ 값을 찾을 수 있다.

시스템의 특성 방정식은 일반적으로 다음과 같은 형태를 갖는다:

$$
P(s) = \frac{1}{s^2 + 2 \zeta \omega\_n s + \omega\_n^2}
$$

여기서:

* $P(s)$는 시스템의 전달 함수
* $\zeta$는 감쇠비
* $\omega\_n$은 자연 진동수이다.

PID 제어기를 적용하면 시스템의 폐루프 전달 함수는 다음과 같이 표현된다:

$$
P\_{closed}(s) = \frac{K\_p + K\_i/s + K\_d s}{s^2 + 2 \zeta \omega\_n s + \omega\_n^2 + (K\_p + K\_i/s + K\_d s)}
$$

이 방정식을 기반으로 루트 궤적을 분석하여 시스템의 안정성을 확인할 수 있다.