ผู้จัดทำ : นายธีร์ธวัช กมลทกาภัย 6634050021
Objective
โครงการนี้จัดทำขึ้นเพื่อให้ผู้ที่ยังไม่รู้จักว่า UR3e คืออะไร ได้เข้าใจพื้นฐานของแขนหุ่นยนต์ โดยผู้ใช้งานจะได้เรียนรู้ว่าแขนหุ่นยนต์มีทั้งหมดกี่ข้อต่อ (joint) และสามารถทดลองขยับแขนหุ่นยนต์ผ่าน UI Panel ที่แสดงผลบนแว่น Meta Quest 3s ซึ่งทำงานในรูปแบบ passthrough ทำให้สามารถมองเห็นการเคลื่อนไหวของหุ่นยนต์ได้แบบเรียลไทม์ นอกจากนี้ยังมีระบบ popup แสดงชื่อของแต่ละข้อต่อของแขนกล เพื่อช่วยให้เข้าใจโครงสร้างของหุ่นยนต์ได้ชัดเจนยิ่งขึ้น
System Scenario
การทำงานเริ่มต้นจากผู้ใช้งานที่ควบคุมแขนหุ่นยนต์ผ่าน UI Panel บนแว่น Meta Quest 3s ซึ่งทำงานในรูปแบบ passthrough โดยภายใน UI Panel จะมีปุ่มควบคุมต่าง ๆ ได้แก่
- การเลือกข้อต่อ (Joint X)
- การปรับค่าองศา (เพิ่ม +10 / ลด -10 degree)
- ปุ่ม Home config สำหรับรีเซ็ตแขนหุ่นไปยังตำแหน่งเริ่มต้น
- ปุ่ม confirm เพื่อยืนยันค่าที่ตั้ง
- ปุ่ม reset เพื่อย้อนกลับไปยังค่าล่าสุดที่ส่งไป
UX/UI Design
ส่วน UI ของระบบนี้ถูกออกแบบให้สามารถใช้ Hand Interaction ได้โดยผู้ใช้งานสามารถใช้การ grabbing หรือ touching กับตัวของ UI Panel ในสภาพแวดล้อมได้ ทั้งนี้ระบบพัฒนาขึ้นโดยอาศัยเครื่องมือ Meta XR Interaction SDK ซึ่งเป็นชุดเครื่องมือสำหรับการพัฒนาแอปพลิเคชันบน Unity ที่รองรับการโต้ตอบแบบ Mixed Reality และ Virtual Reality
SDK ดังกล่าวรองรับ hand tracking และ gesture ต่าง ๆ เช่น grab, poke, drag ทำให้ผู้ใช้งานสามารถโต้ตอบกับวัตถุเสมือนได้โดยไม่ต้องใช้ controller ส่งผลให้ประสบการณ์การใช้งานมีความเป็นธรรมชาติ และช่วยลด learning curve ของผู้ใช้
System Block Diagram
Diagram นี้แสดงระบบ Digital Twin ของหุ่นยนต์ UR3e โดยเริ่มจากผู้ใช้ควบคุมผ่าน UI ใน Unity ซึ่งจะปรับค่ามุมของข้อต่อต่างๆ จากนั้น Unity จะส่งค่ามุมนี้ผ่าน MQTT ไปยังโปรแกรม Python ที่ทำหน้าที่ตรวจสอบความถูกต้องของค่า แปลงหน่วย และสั่งหุ่นยนต์จริงให้เคลื่อนที่ผ่าน RTDE ขณะเดียวกัน Python จะอ่านตำแหน่งจริงของหุ่นยนต์แล้วส่งกลับไปยัง Unity ผ่าน MQTT เพื่อให้ UI แสดงผลตรงกับสถานะของหุ่นยนต์จริง และในส่วนของ Digital Twin ตัวของมันนั้นจะเปลี่ยนตามค่าใน UI ทำให้มันจะเป็นต้นแบบของหุ่นยนต์จริง เพื่อตรวจสอบก่อนสั่งให้หุ่นยนต์จริงทำงาน
RTDE Python code
ทำหน้าที่ส่งและรับข้อมูล จาก unity แปลงเป็น Radians แล้วค่อยส่ง Radians ของแต่ละ joint ไปให้กับ UR3e ด้วย RTDE
ส่วน IMPORT
import time
import math
import threading
import paho.mqtt.client as mqtt
from rtde_control import RTDEControlInterface as RTDEControl
from rtde_receive import RTDEReceiveInterface as RTDEReceive
from rtde_io import RTDEIOInterface as RTDEIOส่วนนี้นำเข้าไลบรารีที่จำเป็นต่อการทำงานของโปรแกรม เช่น time สำหรับหน่วงเวลา, math สำหรับแปลงองศาเป็นเรเดียน, threading สำหรับจัดการข้อมูลร่วมกันระหว่างหลายงาน, paho.mqtt.client สำหรับรับส่งข้อมูลผ่าน MQTT และ rtde_control, rtde_receive, rtde_io สำหรับควบคุมและอ่านค่าจากหุ่นยนต์ UR โดยตรง
ส่วน CONFIG
ROBOT_IP = "172.20.10.100"
RTDE_FREQUENCY = 500.0
UR_CAP_PORT = 50002
MQTT_BROKER = "10.22.253.231"
MQTT_PORT = 1883
TOPIC_SUB = "fibo/log/Bless/UnityToPython"
TOPIC_PUB = "fibo/log/Bless/PythonToUnity"
MOVE_SPEED = 0.5 # rad/s
MOVE_ACCEL = 0.5 # rad/s²ส่วนนี้เป็นการกำหนดค่าพื้นฐานของระบบ เช่น IP ของหุ่นยนต์, ความถี่ที่ใช้สื่อสารกับ RTDE, หมายเลขพอร์ต, ที่อยู่ของ MQTT broker, พอร์ต MQTT, ชื่อ topic สำหรับรับข้อมูลจาก Unity และชื่อ topic สำหรับส่งข้อมูลกลับไป Unity รวมถึงกำหนดความเร็วและความเร่งของการเคลื่อนที่ของหุ่นยนต์ด้วย
ส่วน JOINT LIMITS
JOINT_LIMITS_DEG = [
(-360.0, 360.0), # 0: Shoulder
(-360.0, 360.0), # 1: Upper arm
(-160.0, 160.0), # 2: Forearm
(-360.0, 360.0), # 3: Wrist 1
(-360.0, 360.0), # 4: Wrist 2
(-360.0, 360.0), # 5: Wrist 3
]
JOINT_NAMES = [
"Shoulder", "Upper arm", "Elbow", "Wrist 1", "Wrist 2", "Wrist 3"
]ส่วนนี้กำหนดขอบเขตการหมุนของข้อต่อแต่ละแกนของหุ่นยนต์ โดยในโค้ดระบุว่าข้อศอกหรือ joint ที่ 2 หมุนได้ระหว่าง -160 ถึง 160 องศา ส่วนข้อต่ออื่นหมุนได้ระหว่าง -360 ถึง 360 องศา และยังมี JOINT_NAMES เพื่อเก็บชื่อของแต่ละข้อต่อไว้ใช้แสดงข้อความเตือนให้อ่านง่ายขึ้น
ฟังก์ชัน validate_joint_limits
def validate_joint_limits(degrees: list[float]) -> tuple[bool, list[float], list[str]]:
ok = True
clamped = []
warnings = []
for i, (val, (lo, hi)) in enumerate(zip(degrees, JOINT_LIMITS_DEG)):
if val < lo:
warnings.append(
f" {JOINT_NAMES[i]} (joint {i}): {val:.1f}° ต่ำกว่า limit"
)
clamped.append(lo)
ok = False
elif val > hi:
warnings.append(
f" {JOINT_NAMES[i]} (joint {i}): {val:.1f}° เกิน limit"
)
clamped.append(hi)
ok = False
else:
clamped.append(val)
return ok, clamped, warnings
pending_target: list[float] | None = None # องศา 6 ค่า (degree)
state_lock = threading.Lock()
stop_event = threading.Event()ฟังก์ชันนี้มีหน้าที่ตรวจสอบค่ามุมที่รับเข้ามาว่าเกินขอบเขตของแต่ละข้อต่อหรือไม่ ถ้าค่าต่ำกว่าขอบเขตล่างหรือสูงกว่าขอบเขตบน ฟังก์ชันจะสร้างข้อความเตือนและปรับค่ามุมนั้นให้อยู่ในช่วงที่ปลอดภัย จากนั้นคืนค่าออกมา 3 อย่างคือ สถานะว่าทุกค่าปลอดภัยหรือไม่, รายการค่าที่ถูกปรับแล้ว, และรายการข้อความเตือนที่เกิดขึ้น
ตัวแปร pending_target ใช้เก็บชุดมุมเป้าหมายล่าสุดที่รับมาจาก Unity โดยยังไม่ถูกนำไปสั่งหุ่นยนต์ state_lock ใช้ล็อกการเข้าถึงตัวแปรนี้เพื่อป้องกันข้อมูลชนกันระหว่าง callback ของ MQTT กับลูปหลักของโปรแกรม ส่วน stop_event ใช้เป็นตัวบอกว่าควรหยุดลูปการทำงานหรือยัง แม้ในโค้ดนี้จะยังไม่ได้สั่ง set ไว้ในจุดอื่นก็ตาม
ฟังก์ชัน on_connect
def on_connect(client, userdata, flags, rc):
if rc == 0:
print(f"[MQTT] Connected to broker {MQTT_BROKER}:{MQTT_PORT}")
client.subscribe(TOPIC_SUB)
print(f"[MQTT] Subscribed to '{TOPIC_SUB}'")
else:
print(f"[MQTT] Connection failed, return code {rc}")ฟังก์ชันนี้จะถูกเรียกเมื่อ MQTT client เชื่อมต่อกับ broker สำเร็จ ถ้าเชื่อมต่อได้จะพิมพ์ข้อความยืนยันและสั่ง subscribe ไปยัง topic ที่ใช้รับคำสั่งจาก Unity แต่ถ้าเชื่อมต่อไม่สำเร็จก็จะแสดงรหัสข้อผิดพลาดออกมา
ฟังก์ชัน on_message
def on_message(client, userdata, msg):
"""
Ex : "0.0,-90.0,0.0,-5.0,0.0,359.0"
"""
global pending_target
try:
payload = msg.payload.decode("utf-8").strip()
parts = payload.split(",")
if len(parts) != 6:
print(f"[MQTT] ข้อมูลไม่ครบ 6 ค่า: '{payload}'")
return
degrees = [float(p) for p in parts]
with state_lock:
pending_target = degrees
print(f"[MQTT] Received target (deg): {degrees}")
except Exception as e:
print(f"[MQTT] Parse error: {e}")ฟังก์ชันนี้จะถูกเรียกเมื่อมีข้อความใหม่เข้ามาจาก Unity โดยโปรแกรมคาดหวังว่าข้อความจะเป็นตัวเลข 6 ค่าในรูปแบบคั่นด้วยเครื่องหมายจุลภาค เช่น 0.0,-90.0,0.0,-5.0,0.0,359.0 จากนั้นจะแยกข้อความออกเป็น 6 ส่วน ตรวจว่ามาครบหรือไม่ แล้วแปลงแต่ละส่วนเป็น float ถ้าสำเร็จก็จะเก็บไว้ใน pending_target เพื่อให้ลูปหลักนำไปใช้ต่อ
ฟังก์ชัน publish_joint_state
def publish_joint_state(mqtt_client, rtde_r):
try:
q_rad = rtde_r.getActualQ()
q_deg = [math.degrees(v) for v in q_rad]
payload = ",".join(f"{v:.1f}" for v in q_deg)
mqtt_client.publish(TOPIC_PUB, payload)
print(f"[RTDE→Unity] {payload}")
except Exception as e:
print(f"[Publish] Error: {e}")
ฟังก์ชันนี้ใช้สำหรับอ่านค่ามุมปัจจุบันของหุ่นยนต์จาก rtde_r.getActualQ() ซึ่งค่าที่ได้จะอยู่ในหน่วยเรเดียน จากนั้นจะแปลงเป็นองศา จัดรูปแบบเป็นข้อความคั่นด้วยจุลภาค และ publish กลับไปยัง Unity ผ่าน topic ที่กำหนดไว้ เพื่อให้ Unity รู้สถานะล่าสุดของหุ่นยนต์
ฟังก์ชัน main ช่วงเชื่อมต่อ RTDE
def main():
global pending_target
# ── เชื่อมต่อ RTDE ──────────────────────
flags = RTDEControl.FLAG_VERBOSE | RTDEControl.FLAG_UPLOAD_SCRIPT
print("[RTDE] Connecting to robot...")
rtde_r = RTDEReceive(ROBOT_IP, RTDE_FREQUENCY, [])
rtde_c = RTDEControl(ROBOT_IP, RTDE_FREQUENCY, flags, UR_CAP_PORT)
rtde_i = RTDEIO(ROBOT_IP)
rtde_i.setSpeedSlider(0.5) ##50%
time.sleep(0.1)
q_init = rtde_r.getActualQ()
print(f"[RTDE] Current joint (deg): {[round(math.degrees(v),2) for v in q_init]}")ในช่วงเริ่มต้นของ main() โปรแกรมจะสร้างการเชื่อมต่อกับหุ่นยนต์ผ่าน RTDE โดยแยกเป็นตัวอ่านค่า (RTDEReceive), ตัวควบคุม (RTDEControl) และตัวจัดการ I/O (RTDEIO) จากนั้นตั้ง speed slider ของหุ่นยนต์ไว้ที่ 0.5 แล้วอ่านค่ามุมปัจจุบันของหุ่นยนต์มาแสดงบนหน้าจอเพื่อบอกตำแหน่งเริ่มต้น
ฟังก์ชัน main ช่วงเชื่อม MQTT
mqtt_client = mqtt.Client()
mqtt_client.on_connect = on_connect
mqtt_client.on_message = on_message
mqtt_client.connect(MQTT_BROKER, MQTT_PORT, keepalive=60)
mqtt_client.loop_start()
time.sleep(1.0)
publish_joint_state(mqtt_client, rtde_r)หลังจากเชื่อมต่อหุ่นยนต์แล้ว โปรแกรมจะสร้าง MQTT client กำหนด callback สำหรับ on_connect และ on_message จากนั้นเชื่อมต่อไปยัง broker และเริ่ม loop ของ MQTT แบบ background ด้วย loop_start() เพื่อให้โปรแกรมสามารถรับข้อความจาก Unity ได้ตลอดเวลาโดยไม่บล็อกลูปหลัก
หลังจากเริ่ม MQTT แล้ว โปรแกรมจะรอประมาณ 1 วินาที แล้วเรียก publish_joint_state() เพื่อส่งค่ามุมปัจจุบันของหุ่นยนต์กลับไปยัง Unity ทันทีหนึ่งครั้ง ซึ่งช่วยให้ Unity รู้สถานะเริ่มต้นของหุ่นยนต์ตั้งแต่ก่อนมีคำสั่งใหม่เข้ามา
ฟังก์ชัน main ช่วง main loop
try:
while not stop_event.is_set():
target_deg = None
with state_lock:
if pending_target is not None:
target_deg = pending_target
pending_target = None
if target_deg is not None:
ok, clamped_deg, warnings = validate_joint_limits(target_deg)
if not ok:
print("[Limit] ค่าบางข้อเกิน joint limit ของ UR3e:")
for w in warnings:
print(w)
print(f"[Limit] ใช้ค่าหลัง clamp: {[round(v,1) for v in clamped_deg]}")
else:
print(f"[Limit] ค่าทุก joint อยู่ใน limit ✓")
q_target = [math.radians(d) for d in clamped_deg]
print(f"[RTDE] Moving to (rad): {[round(v,4) for v in q_target]}")
try:
rtde_c.moveJ(q_target, MOVE_SPEED, MOVE_ACCEL)
print("[RTDE] Move complete.")
except Exception as e:
print(f"[RTDE] moveJ error: {e}")
publish_joint_state(mqtt_client, rtde_r)pending_target ภายใต้การล็อกด้วย state_lock และเมื่อนำค่าออกมาแล้วก็จะล้าง pending_target ทิ้งเพื่อไม่ให้คำสั่งเดิมถูกใช้ซ้ำอีกรอบ
ถ้ามีค่ามุมใหม่เข้ามา ลูปหลักจะเรียก validate_joint_limits() เพื่อตรวจสอบว่ามุมของแต่ละ joint อยู่ในช่วงที่ปลอดภัยหรือไม่ ถ้ามีค่าที่เกิน โปรแกรมจะแสดงข้อความเตือนทีละ joint และบอกค่าหลังจาก clamp แล้ว แต่ถ้าทุกค่าปลอดภัยก็จะแสดงข้อความยืนยันว่าทุก joint อยู่ใน limit
หลังจากได้ค่าที่ปลอดภัยแล้ว โปรแกรมจะแปลงมุมจากองศาเป็นเรเดียนด้วย math.radians() เพราะคำสั่ง moveJ ของหุ่นยนต์ต้องใช้เรเดียน จากนั้นจะแสดงค่าที่จะใช้เคลื่อนที่และเรียก rtde_c.moveJ(q_target, MOVE_SPEED, MOVE_ACCEL) เพื่อให้หุ่นยนต์เคลื่อนที่ไปยังตำแหน่งเป้าหมายด้วยความเร็วและความเร่งที่ตั้งไว้
เมื่อสั่งเคลื่อนที่เสร็จแล้ว โปรแกรมจะเรียก publish_joint_state() อีกครั้งเพื่ออ่านค่าปัจจุบันของหุ่นยนต์และส่งกลับไปยัง Unity ทำให้ฝั่ง Unity ได้รับสถานะล่าสุดหลังการเคลื่อนที่จริง
Transform position of Digital Twin
โค้ดนี้เป็นสคริปต์ C# ใน Unity ที่ใช้สำหรับ หมุนข้อต่อตามค่ามุมที่แสดงอยู่ใน UI (TMP_Text) โดยไม่ใช้ IK (Inverse Kinematics) แต่ใช้การหมุนตรง ๆ จากค่าองศาที่อ่านมา
using TMPro;
using UnityEngine;
public class NO_IK_PLS : MonoBehaviour
{
[System.Serializable]
public class JointMap
{
public Transform joint;
public TMP_Text degreeText;
public Vector3 axis = Vector3.up;
[HideInInspector] public Quaternion initialRotation;
}
public JointMap[] joints;
void Start()
{
foreach (var j in joints)
{
if (j.joint != null)
{
j.initialRotation = j.joint.localRotation;
j.axis = j.axis.normalized;
}
}
}
void Update()
{
foreach (var j in joints)
{
if (j.joint == null || j.degreeText == null) continue;
string raw = j.degreeText.text.Trim();
raw = raw.Replace("°", "").Replace("degree", "").Trim();
if (float.TryParse(raw, out float angle))
{
Quaternion rot = Quaternion.AngleAxis(angle, j.axis);
j.joint.localRotation = j.initialRotation * rot;
}
}
}
}Start()
ฟังก์ชันนี้ทำงานแค่ครั้งเดียวตอนเริ่มเกม โดยมีหน้าที่ “ตั้งค่าเริ่มต้น” ให้แต่ละข้อต่อ เช่น เก็บ rotation เดิมของ joint ไว้เป็นค่าอ้างอิง และปรับแกนหมุนให้เป็นเวกเตอร์หน่วย เพื่อให้การคำนวณหมุนในภายหลังถูกต้องและเสถียร
Update()
ฟังก์ชันนี้จะทำงานทุก frame โดยมีหน้าที่ “อัปเดตการหมุนตลอดเวลา” คืออ่านค่ามุมจาก UI (TMP_Text) ของแต่ละ joint แปลงเป็นตัวเลข แล้วนำไปหมุน Transform ของ joint ตามแกนที่กำหนด ทำให้วัตถุใน Unity เคลื่อนไหวตามค่าที่เปลี่ยนแบบ real-time
System Implementation Demo
System Evaluation
การวิเคราะห์ ประเมินประสิทธิภาพของระบบ
System Performance
Refresh Rate
ทดลองโดยการเล่นแอปพลิเคชัน เป็นเวลา 3 นาทีแล้วดูผลลัพธ์
| Uniy Link in VR (Meta quest 3s) | 69.2 – 73.2 FPS |
| Build in VR (Meta quest 3s) | 68 – 72 FPS |
Latency ของ MQTT
ทดลองโดยส่งข้อมูลผ่าน MQTT แล้วดูความต่างของเวลาที่ส่ง
| 10 msg/sec | ≈ 1.59 ms |
| 33 msg/sec | ≈ 1.44 ms |
| 65 msg/sec | ≈ 1.25 ms |
Usability
หลังจากผู้ทดลองใช้งานเสร็จแล้วจะให้ทำแบบสอบถามโดยจะประกอบด้วยแบบสอบถาม System Usability Scale (SUS) ซึ่งใช้ประเมินความง่ายและประสิทธิภาพในการใช้งานของระบบ และ NASA Task Load Index (NASA-TLX) ซึ่งใช้ประเมินภาระงานของผู้ใช้งานทั้งด้านร่างกายและจิตใจ เพื่อให้ได้ข้อมูลครอบคลุมทั้งด้านคุณภาพการใช้งานและระดับความเหนื่อยล้าหรือความเครียดจากการใช้งานระบบ
| SUS | NASA-TLX | ||
| Mean | SD | Mean | SD |
| 80.60 | 10.66 | 36.70 | 9.17 |
จากการประเมิน System Usability Scale (SUS) พบว่าระบบมีค่าเฉลี่ยเท่ากับ 80.6 ซึ่งอยู่ในระดับดีมาก (Excellent) และสูงกว่าค่ามาตรฐานอย่างชัดเจน
ในขณะเดียวกัน การประเมินภาระงานด้วย NASA-TLX พบว่ามีค่าเฉลี่ยเท่ากับ 36.7 ซึ่งอยู่ในระดับปานกลางค่อนไปทางต่ำ แสดงให้เห็นว่าผู้ใช้ไม่ได้รู้สึกเหนื่อยหรือมีภาระในการใช้งานสูง
เมื่อพิจารณาร่วมกัน พบว่าระบบมีทั้งความสามารถในการใช้งานที่ดี และไม่สร้างภาระให้ผู้ใช้มาก ซึ่งสะท้อนถึงคุณภาพของประสบการณ์ผู้ใช้ (User Experience) ในภาพรวมที่ดีอย่างไรก็ตาม ยังมีผู้ใช้บางส่วนที่มีคะแนน SUS อยู่ในระดับปานกลาง (~70) และ NASA-TLX ในระดับใกล้เคียง 45 ซึ่งบ่งชี้ว่าบางขั้นตอนของระบบอาจยังมีความซับซ้อนหรือใช้ความพยายามในการใช้งานอยู่
Task Completion Efficiency
โดยภาพรวมแล้ว แอปพลิเคชันนี้ช่วยให้ผู้ใช้งานที่ไม่เคยมีประสบการณ์กับหุ่นยนต์ UR3e สามารถทำความเข้าใจการใช้งานเบื้องต้นได้ และสามารถควบคุมหุ่นยนต์ให้เคลื่อนที่ไปยังตำแหน่งเป้าหมายได้
Discussion
เนื่องจากผู้จัดทำได้ออกแบบระบบเพิ่มเติมเพื่อให้สามารถใช้งานหุ่นยนต์ UR3e ได้ง่ายขึ้น โดยระบบดังกล่าวเปิดโอกาสให้ผู้ใช้งานสามารถจับที่ปลายหุ่นยนต์ (end-effector) และเคลื่อนย้ายได้โดยตรง จากนั้นหุ่นยนต์จะเคลื่อนที่ตามการกระทำของผู้ใช้งานผ่านการคำนวณ Inverse Kinematics ด้วยอัลกอริทึม Cyclic Coordinate Descent (CCD)
อย่างไรก็ตาม คาดว่าหน่วยประมวลผลของอุปกรณ์ VR อาจมีข้อจำกัดด้านสมรรถนะในการคำนวณแบบเรียลไทม์ จากผลการทดลองพบว่า ระบบสามารถทำงานได้ตามปกติเมื่อรันผ่าน Unity และเชื่อมต่อกับอุปกรณ์ VR แต่เมื่อทำการ build และนำไปรันบนอุปกรณ์ VR โดยตรง ระบบเกิดอาการสั่นและขาดความเสถียร ส่งผลให้ไม่สามารถใช้งานได้อย่างมีประสิทธิภาพ
วิดีโอเมื่อรันผ่าน Unity และเชื่อมต่อกับอุปกรณ์ VR
วิดีโอเมื่อทำการ build และนำไปรันบนอุปกรณ์ VR โดยตรง