AMR Articulation Body
Unity Technologies
이번 포스팅에서는 Unity에서 AMR 로봇 제어를 진행할 예정입니다. 기존 작성한 포스팅과 겹치는 부분은 별도의 설명없이 링크를 남기도록 하겠습니다.
모델링 선정
본 포스팅에서 모델링은 별도로 진행하지 않겠습니다. 따라서 GrabCAD에서 적절한 모델링 파일을 선정 후, Top-Down 형식으로 수정만 진행하겠습니다. 모델은 Track Wheel Bot 모델을 사용하였습니다.
Caterpillar, 무한궤도의 경우 메쉬 설정에서 시간이 많이 소요되기 때문에 삭제하고 진행하겠습니다.
Unity 모델 설정
저는 Unity내에서 CaterPillar 모델을 삭제하였고, 이 후, 가독성을 위해 하이어리키에서 모델명을 수정하였습니다.
바퀴형상이 조금 신경쓰여서, Fusion 360-Extrude-Join기능으로 원형으로 수정하였습니다.
해당 부분은 단순 그래픽이고, 실제 충돌 영역은 Mesh로 설정하였으니 신경쓰지 않으셔도 됩니다.
Imported Model Mesh Decomposer
유니티에서 Mesh를 설정하는 방법은 크게 3가지가 있습니다.
1. Primitive 방식
즉 기존에 유니티 내부에 있는 Unity Mesh Collider를 이용해서 하는 방법입니다. 이 방법의 경우, 균일한 Mesh를 이용함으로써 다소 균일한 물리해석 결과를 기대할 수 있습니다.
2. VHACD 방식
이 방식은 복잡한 형상 또는 Primitive가 없는 모델의 경우 사용할 수 있습니다.
3. Convex Mesh Collider
세번째는 소유한 Mesh Collider의 Convex를 이용하는 방법입니다.
본 포스팅에서는 1번과 3번으로 진행하도록 하겠습니다.
Primitive Mesh
Primitive 방식으로 생성한 Mesh 입니다. Cylinder 모델을 사용했고, 기존 Cylinder 모델은 반지름이 1m 높이가 2m임으로 별도의 하위 오브젝트를 만들고 스케일을 조정하였습니다. 오른쪽 정면 바퀴 기준 설정한 스케일은 다음과 같습니다.
스케일 설정은 Fusion에서 실측을 계산하고, 유니티 단위 (m)에 맞춰서 변환하였습니다.
| 구분 | Y_Rotation | X_Scale | Y_Scale | Z_Scale |
| 값 | 90 | 0.075 | 0.009227 | 0.075 |
Convex Collider
간단한 AMR 튜토리얼임으로 Mesh Collider를 컴포넌트에 추가 후, Convex 하도록 하겠습니다. Wheel 오브젝트, 즉 바퀴를 제외한 부분은 모두 Mesh Collider를 설정하지 않았고, 대신 Mesh Collider의 연산을 줄이기 위해 전체 AMR에 Trigger가 Enabled된 Box Mesh를 설정하였습니다.
Mesh를 중점적으로 보기위해 Wireframe View로 확인해보도록 하겠습니다.
녹색으로 체크된 영역이 실제 충돌영역입니다. 큰 박스는 충돌 감지용 Trigger 그리고 4개의 바퀴는 제어를 위한 Collider로 구성하였습니다.
Articulation Body Hierachy 설정
Root body of the Articulation을 어떤걸로 할지 고민하다가, 링크 그리고 타이어가 이어져있는 시스템이 아니기 때문에 타이어를 제외한 나머지는 Collider도 없고 단순 오브젝트로 판단, 상단 덮개를 Root body로 나머지를 모두 하위에 넣어주었습니다.
하이어리키로 구성하면 다음과 같습니다. 이 후 Articulation Body를 컴포넌트로 추가하고 하이라이트 된 Wheel을 제외한 나머지 오브젝트(파트)는 모두 Joint Type을 Fixed로 설정합니다 (Default)
단순 형상을 위한 나머지 부품은 다음과 같이 Articulation Body 설정이 들어가고, 나머지 4개의 Wheel은 Revolute로 설정합니다.
Edit Joint Limit을 클릭하면 기존 오브젝트에 Gizmo가 나타납니다.
Gizmo가 이상한 곳으로 가있네요...^_^;; 자 이 모델은 꽝입니다...ㅎㅎ (사실 알고 있었음) 원래 유니티에 넘기기전에 각 오브젝트 파트의 원점을 지정해줘야합니다. 즉 Bottom-Up으로 모델링 후, 이 과정에서 각각의 오브젝트의 원점을 지정해줘야함. 이 때 지정하는 원점은 힘을 받을 원점입니다. 이번 다운로드 받은 모델은 전체가 같은 원점으로 하나의 Origin으로 잡힌 것 같습니다. Fusion 360에서도 한번 확인해보도록 하겠습니다.
한 눈에봐도 원점이 제 각각인 것을 알 수 있습니다. 여러 방법이 있는데 저는 Fusion 360의 Mesure를 이용해서 기존 Origin에서 지정해줘야 하는 원점의 거리를 계산하였습니다.
| 구분 | X | Y | Z |
| FL_Wheel | 0.12 | 0.047 | 0.106546 |
| FR_Wheel | 0.12 | 0.047 | -0.106546 |
| BL_Wheel | -0.087341 | 0.047 | 0.106546 |
| BR_Wheel | -0.087341 | 0.047 | -0.106546 |
이 후 Joint의 Anchor를 확인해보면 다음과 같은 것을 확인할 수 있습니다.
붉은 영역 즉 Joint Limit이 딱 중심점에 설정된 것을 확인할 수 있습니다.
Plane 하나 생성해주고 정상적으로 작동하는지 확인하도록 하겠습니다.
Model Test
간단하게 Target Velocity 1으로 설정하였습니다 (랜덤 값)
Code
전체 코드
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;
public class AMR_Physics : MonoBehaviour
{
public List<ArticulationBody> Wheels;
float maxLinearSpeed = 2f; // Units: m/s
float _wheelRadius = 0.375f; // Units: m
float _maxrotspeed; // Units: m/s
float _damping = 400; // Units: N*s/m
float _SpeedSensitivity = 1;
float _DampingSensitivity = 1;
float _Sensitivity = 0.2f;
private void Start()
{
_maxrotspeed = Mathf.Rad2Deg * maxLinearSpeed / _wheelRadius;
}
void Drive(float L_Speed, float R_Speed)
{
for (int i = 0; i < Wheels.Count; i++)
{
ArticulationDrive temp = new ArticulationDrive();
temp = Wheels[i].xDrive;
if (i % 2 == 0)
{
temp.targetVelocity = L_Speed;
}
else
{
temp.targetVelocity = R_Speed;
}
Wheels[i].xDrive = temp;
}
}
void RL(float _Forward, float _Rot)
{
float max_1 = Mathf.Max(Mathf.Abs(_Forward) + Mathf.Abs(_Rot), 1f);
float L_Rot = (_Forward + _Rot) * _maxrotspeed / max_1 * _SpeedSensitivity;
float R_Rot = (_Forward - _Rot) * _maxrotspeed / max_1 * _SpeedSensitivity;
Drive(L_Rot, R_Rot);
}
private void Update()
{
float Horizontal = Input.GetAxis("Horizontal");
float Vertical = Input.GetAxis("Vertical");
RL(Vertical, Horizontal);
if (Input.GetKeyDown(KeyCode.Space))
{
Speed_Up();
}
if (Input.GetKeyDown(KeyCode.LeftControl))
{
Speed_Down();
}
}
void Set_Properties(ArticulationBody joint)
{
ArticulationDrive drive = joint.xDrive;
drive.damping = _damping * _DampingSensitivity;
joint.xDrive = drive;
}
public void Speed_Up()
{
_DampingSensitivity += _Sensitivity;
_SpeedSensitivity += _Sensitivity;
foreach (var wheel in Wheels)
{
Set_Properties(wheel);
}
}
public void Speed_Down()
{
_DampingSensitivity -= _Sensitivity;
_SpeedSensitivity -= _Sensitivity;
foreach (var wheel in Wheels)
{
Set_Properties(wheel);
}
}
}
변수 선언
public List<ArticulationBody> Wheels;
float maxLinearSpeed = 2f; // Units: m/s
float _wheelRadius = 0.375f; // Units: m
float _maxrotspeed; // Units: m/s
float _damping = 400; // Units: N*s/m
float _SpeedSensitivity = 1;
float _DampingSensitivity = 1;
float _Sensitivity = 0.2f;
변수를 선언하는 부분입니다. ArticulationBody의 경우, Unity Inverse Kinematics 데모를 참가하여 리스트로 작성하였습니다. 추 후 중장비 시뮬레이션을 할 일이 생길 것 같아서 Public List로 작성하였습니다.
이 maxLinearSpeed의 경우 모터의 최대토크 값으로 계산한 선속도를 넣어주시면됩니다. 선속도와 CAD 데이터에서 바퀴의 반지름을 알 수 있음으로, 우리는 각속도를 계산할 수 있습니다.
$$w = \frac{v}{r}$$
최대 각속도 계산
void Start()
{
_maxrotspeed = Mathf.Rad2Deg * maxLinearSpeed / _wheelRadius;
}
이 때 해당 값은 rad으로 나오게 됨으로 아래와 같이 Mathf.Rad2Deg를 사용해서 Deg 각도로 변환해줍니다.
키보드 입력 구현
private void Update()
{
float Horizontal = Input.GetAxis("Horizontal");
float Vertical = Input.GetAxis("Vertical");
RL(Vertical, Horizontal);
if (Input.GetKeyDown(KeyCode.Space))
{
Speed_Up();
}
if (Input.GetKeyDown(KeyCode.LeftControl))
{
Speed_Down();
}
}
코드를 조금 넘겨서 Update부분을 먼저 보도록 하겠습니다. 익숙한 GetAxis입니다. Cardinal (키보드의 화살표) 또는 WASD Input을 받습니다. 이렇게 받은 Input Value를 RL 함수로 넘깁니다. 이 Input Value는 -1부터 1까지의 값을 가집니다.
강화학습을 고려한 물리식 구현
void RL(float _Forward, float _Rot)
{
float max_1 = Mathf.Max(Mathf.Abs(_Forward) + Mathf.Abs(_Rot), 1f);
float L_Rot = (_Forward + _Rot) * _maxrotspeed / max_1 * _SpeedSensitivity;
float R_Rot = (_Forward - _Rot) * _maxrotspeed / max_1 * _SpeedSensitivity;
Drive(L_Rot, R_Rot);
}메소드 명을 RL로 한 것은 추 후 ML-Agents를 적용할 수 있게 코드를 구성하기 위해 RL로 정하였습니다. Input은 _Forward와 _Rot이고 각각 -1부터 1까지 값을 가짐으로써 Continuous Actions 그리고 Action은 2가 될 것 입니다.
좌회전을 기준으로 설명드리겠습니다. AMR이 좌회전을 하기 위해서는 왼쪽 바퀴는 뒤로 오른쪽 바퀴는 앞으로 전진해야합니다. 이 경우 AMR에 토크가 가해지면서 Spin Turn을 할 수 있습니다.
따라서, 키보드에서 A를 누를 때 (즉 좌회전) 왼쪽 바퀴에 가해지는 Input Value는 -1값 그리고 오른쪽 바퀴에 가해지는 Input Value는 1값을 가져야합니다. 공식으로 표현하면 전진하면서 좌회전 또는 전진하면서 우회전 등 다양한 상황에도 마찬가지로 해당 값을 가져야 함으로 위와 같이 코드를 작성하였습니다.
다만 이렇게 코딩할 경우 모터의 출력에 문제가 발생합니다. 따라서 Input Value인 _Forward와 _Rot 변수의 값이 1보다 작을 경우 (즉 모터의 출력을 최대치로 사용하지 않을 경우) 해당 값을 그대로 사용해야 하고 (사용 출력 / 최대출력), 1이 넘어갈 경우 (직선운동과 회전운동이 같이 입력될 경우) 그 값을 최대출력인 1로 고정시켜야합니다.
이를 위해 Mathf.Max를 사용해서 _Forward와 _Rot변수의 합이 1보다 작을 경우는 이 값을 택하고, 1을 넘어갈 경우는 1을 택하도록 코딩하였습니다.
xDrive 값 할당
void Drive(float L_Speed, float R_Speed)
{
for (int i = 0; i < Wheels.Count; i++)
{
ArticulationDrive temp = new ArticulationDrive();
temp = Wheels[i].xDrive;
if (i % 2 == 0) //L, R 번갈아가면서 넣어야 함
{
temp.targetVelocity = L_Speed;
}
else
{
temp.targetVelocity = R_Speed;
}
Wheels[i].xDrive = temp;
}
}
이후 Drive 메소드에서 각각의 Wheel 안에 있는 Articulation Body의 xDrive 즉 ArticulationDrive의 Target Velocity에 구한 각속도를 넣어줍니다.
+) ML-Agents를 적용할 수 있도록, Action을 고려하였습니다. 실제로는 x축을 기준으로 회전하고 있지 않지만, xDrive의 각도를 조절하였음으로 xDrive를 사용할 수 있습니다. Articulation Body의 Wheel 리스트의 Index를 왼쪽 그리고 오른쪽 바퀴를 번갈아가면서 넣어주어야 정상적으로 작동합니다.
Sensitivity 추가
void Set_Properties(ArticulationBody joint)
{
ArticulationDrive drive = joint.xDrive;
drive.damping = _damping * _DampingSensitivity;
joint.xDrive = drive;
}
public void Speed_Up()
{
_DampingSensitivity += _Sensitivity;
_SpeedSensitivity += _Sensitivity;
foreach (var wheel in Wheels)
{
Set_Properties(wheel);
}
}
public void Speed_Down()
{
_DampingSensitivity -= _Sensitivity;
_SpeedSensitivity -= _Sensitivity;
foreach (var wheel in Wheels)
{
Set_Properties(wheel);
}
}
Sensitivity 값은 물리 값이 정확한지 확인하기 위해 디버깅 용도로 작성하였습니다. Space 그리고 왼쪽 Control을 누름으로써 속도를 조절할 수 있습니다. 간단한 부분으로 설명은 넘어가도록 하겠습니다.
Result
댓글