117 lines
3.8 KiB
C#
117 lines
3.8 KiB
C#
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; // 초록
|
|
|
|
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 Update()
|
|
{
|
|
//OnPoseUpdateRequest?.Invoke();// TODO. 로봇을 잡고 움직일 때와 아닐 때 구분하기
|
|
OnPoseUpdateRequest?.Invoke();
|
|
}
|
|
|
|
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개 축의 회전값
|
|
// kinematicsNode.targetTransform.localPosition = new Vector3(robotData.x, robotData.y, robotData.z);
|
|
// kinematicsNode.targetTransform.localRotation = new Quaternion(robotData.rx, robotData.ry, robotData.rz, 0);
|
|
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);
|
|
}
|
|
|
|
public void Movement()
|
|
{
|
|
movementPosition.x = Convert.ToSingle(Math.Round(-1 * kinematicsNode.targetTransform.localPosition.x * 1000, 2));
|
|
movementPosition.y = Convert.ToSingle(Math.Round(-1 * kinematicsNode.targetTransform.localPosition.z * 1000, 2));
|
|
movementPosition.z = Convert.ToSingle(Math.Round(kinematicsNode.targetTransform.localPosition.y * 1000, 2));
|
|
}
|
|
|
|
void OnDestroy()
|
|
{
|
|
IsMovementRunning = false;
|
|
}
|
|
}
|