Gazebo 플러그인 활용 - 실험 도서관

Gazebo 플러그인의 개요

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

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

Gazebo 플러그인의 주요 기능

로봇 제어
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$ 는 각각 비례, 적분, 미분 게인이다.

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

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

여기서: - $\mathbf{a}$ 는 가속도 벡터 - $\mathbf{v}$ 는 속도 벡터이다.

Gazebo 플러그인의 구조

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

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

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

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

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

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

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

월드 플러그인
월드 플러그인은 전체 시뮬레이션 환경을 제어한다. 예를 들어, 특정 시간이 경과한 후 시뮬레이션에 새로운 객체를 추가하거나, 기존 객체를 제거할 수 있다. 또한, 시뮬레이션 중에 중력이나 마찰 계수와 같은 물리적 특성을 동적으로 변경할 수 있다.
월드 플러그인의 일반적인 사용 사례는 다음과 같다:
- 시뮬레이션 시간에 따른 환경 변화
- 로봇의 동작에 따른 실시간 환경 반응
- 다중 로봇 시스템의 환경 내 상호작용
모델 플러그인
모델 플러그인은 로봇 자체의 동작을 제어하는 데 사용된다. 이 플러그인을 통해 사용자는 로봇의 특정 부위를 움직이거나, 센서 데이터를 분석하여 로봇의 행동을 결정할 수 있다.
예를 들어, 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$ -축을 기준으로 로봇 조인트의 회전을 설명한다. 이를 통해 로봇의 조인트가 주어진 각도로 회전하며, 플러그인은 로봇의 동작을 제어할 수 있다.

센서 플러그인
센서 플러그인은 시뮬레이션 내에서 로봇에 장착된 센서들을 제어한다. 예를 들어, 카메라 센서를 사용하여 로봇 주변의 이미지를 수집하거나, 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를 활용하여 플러그인의 동작을 정의한다. 플러그인 개발은 주로 세 가지 단계로 나눌 수 있다:

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

Gazebo 플러그인 개발 예제

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

1. 플러그인 헤더 파일

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

#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 파일이다. 여기서 조인트 제어 로직을 작성한다.

#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 파일에서 플러그인을 호출하여 시뮬레이션에서 실행할 수 있다.

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

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

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

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

주요 함수

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

cpp void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { // 모델의 특정 조인트를 가져오기 this->joint_ = _model->GetJoint("joint_name"); }

Init()
플러그인이 초기화된 후, 시뮬레이션이 시작되기 전에 호출된다. 시뮬레이션 중 사용할 변수를 초기화하거나, 첫 번째 업데이트 전에 필요한 계산을 수행할 수 있다.
Reset()
시뮬레이션이 다시 시작될 때 호출된다. 이 함수는 시뮬레이션이 중단되었다가 재시작될 때나, 시뮬레이션이 리셋될 때 사용된다. 플러그인의 상태를 초기화하고, 변수나 상태 값을 리셋하는 데 사용된다.

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

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

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

주요 이벤트

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

cpp this->updateConnection = event::Events::ConnectWorldUpdateBegin( std::bind(&MyPlugin::OnUpdate, this));

ConnectWorldUpdateEnd()
시뮬레이션이 업데이트를 마친 후 발생하는 이벤트이다. 이를 통해 시뮬레이션의 프레임이 종료된 후 추가적인 작업을 수행할 수 있다.
ConnectBeforePhysicsUpdate()
물리 엔진이 업데이트되기 직전에 발생하는 이벤트이다. 이를 통해 물리 연산이 적용되기 전에 로봇의 상태를 조정할 수 있다. 예를 들어, 로봇의 위치나 속도를 조정할 수 있다.
ConnectAfterPhysicsUpdate()
물리 엔진이 업데이트된 후 발생하는 이벤트이다. 이 이벤트는 물리 엔진이 계산된 데이터를 기반으로 로봇의 상태를 업데이트한 후, 추가적인 작업을 수행하는 데 사용된다.

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

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

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

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

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

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

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

$\tau = I \alpha$

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

조인트 속도 제어
조인트의 속도를 제어하는 방법도 지원된다. 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 제어기의 구성 요소

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

$P = K_p \cdot e(t)$

여기서: - $P$ 는 비례 제어 신호 - $K_p$ 는 비례 게인 - $e(t)$ 는 목표값과 실제값의 차이 (오차)이다.

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

$I = K_i \cdot \int_0^t e(\tau) d\tau$

여기서: - $I$ 는 적분 제어 신호 - $K_i$ 는 적분 게인 - $\int_0^t e(\tau) d\tau$ 는 시간에 따른 오차의 누적이다.

미분 제어 (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의 업데이트 함수에서 주기적으로 계산되며, 각 조인트에 필요한 힘이나 토크를 적용하는 방식으로 구현된다.

// 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)}$

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