XRLib/Assets/HybridIK/Scripts/Main/RobotController.cs

using NUnit.Framework;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Net.Sockets;
using System.Threading.Tasks;
using UnityEngine;
using UnityEngine.UIElements;

public class RobotController : MonoBehaviour
{
    [Header("IK")]
    [SerializeField] private HybridInverseKinematicsNode kinematicsNode;

    [Header("Motor State")]
    [SerializeField] private GameObject motorStatusIndicator1;
    [SerializeField] private GameObject motorStatusIndicator2;

    [SerializeField] private Material indicatorMaterial1; // 기본색(회색)
    [SerializeField] private Material indicatorMaterial2; // 초록

    [SerializeField] public GameObject toolEndpoint;
    [SerializeField] public Material toolEndpointHighlight;
    [SerializeField] public Material toolEndpointErrorHighlight;
    [SerializeField] public Material toolEndpointOriginal;

    [SerializeField] public GameObject testSphere; // 타겟 위치에 표시할 오브젝트

    public event Action OnPoseUpdateRequest;
    public event Action OnPoseUpdateReceive;

    public Vector3 movementPosition;
    private Quaternion movementRotation;

    private bool isMotorOn;
    public bool IsMovementRunning = false;

    void Start()
    {
        if (motorStatusIndicator1 != null)
        {
            motorStatusIndicator1.GetComponent<MeshRenderer>().material = indicatorMaterial1;
        }
        if (motorStatusIndicator2 != null)
        {
            motorStatusIndicator2.GetComponent<MeshRenderer>().material = indicatorMaterial1;
        }
    }

    private void FixedUpdate()
    {
        OnPoseUpdateRequest?.Invoke();
    }

    private void LateUpdate()
    {
        if (kinematicsNode != null && kinematicsNode.enabled == false)
        {
            UpdateHandleToEndPoint();
        }
    }

    public void SetMotorState(bool isOn)
    {
        isMotorOn = isOn;


        if (isMotorOn)
        {
            if (indicatorMaterial2 != null)
            {
                motorStatusIndicator1.GetComponent<MeshRenderer>().material = indicatorMaterial2;
                motorStatusIndicator2.GetComponent<MeshRenderer>().material = indicatorMaterial2;
            }
        }
        else
        {
            if (indicatorMaterial1 != null)
            {
                motorStatusIndicator1.GetComponent<MeshRenderer>().material = indicatorMaterial1;
                motorStatusIndicator2.GetComponent<MeshRenderer>().material = indicatorMaterial1;
            }
        }
    }

    public void EnableIK()
    {
        if (kinematicsNode != null) kinematicsNode.enabled = true;
    }

    public void DisableIK()
    {
        if (kinematicsNode != null) kinematicsNode.enabled = false;
    }

    /*
    public void SetRobotPosition(RobotData robotData) // 가상 로봇 위치 업데이트
    {
        // x, y, z, rx, ry, rz => endpoint값
        // j1, ..., j6 => 6개 축의 회전값
        if (robotData == null)
        {
            return; // 데이터가 없으면 아무것도 하지 않음
        }

        List<Quaternion> list_jAngle = new List<Quaternion>();

        list_jAngle.Add(Quaternion.AngleAxis(-1 * robotData.j1, Vector3.forward));
        list_jAngle.Add(Quaternion.AngleAxis((robotData.j2 - 90), Vector3.up));
        list_jAngle.Add(Quaternion.AngleAxis(robotData.j3, Vector3.up));
        list_jAngle.Add(Quaternion.AngleAxis(robotData.j4, Vector3.right));
        list_jAngle.Add(Quaternion.AngleAxis(robotData.j5, Vector3.up));
        list_jAngle.Add(Quaternion.AngleAxis(robotData.j6, Vector3.right));

        kinematicsNode.SetJointTargetRotations(list_jAngle);
    }
    */

    private void UpdateHandleToEndPoint()
    {
        if (kinematicsNode == null || kinematicsNode.targetTransform == null ||
            kinematicsNode.nodes == null || kinematicsNode.nodes.Count == 0)
        {
            return;
        }

        Transform endPoint = kinematicsNode.nodes[kinematicsNode.nodes.Count - 1].jointTransform;
        if (endPoint == null) return;

        kinematicsNode.targetTransform.position = endPoint.position;
        kinematicsNode.targetTransform.rotation = endPoint.rotation;
    }

    public Transform GetEndPoint()
    {
        Transform endPoint = kinematicsNode.nodes[kinematicsNode.nodes.Count - 1].jointTransform;
        return endPoint;
    }

    void OnDestroy()
    {
        IsMovementRunning = false;
    }
}