81 lines
2.5 KiB
C#
81 lines
2.5 KiB
C#
using UnityEngine;
|
|
using System.Collections;
|
|
using System;
|
|
using NUnit.Framework;
|
|
using System.Collections.Generic;
|
|
|
|
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;
|
|
|
|
private bool isMotorOn;
|
|
|
|
void Start()
|
|
{
|
|
if (motorStatusIndicator1 != null)
|
|
{
|
|
motorStatusIndicator1.GetComponent<MeshRenderer>().material = indicatorMaterial1;
|
|
}
|
|
if (motorStatusIndicator2 != null)
|
|
{
|
|
motorStatusIndicator2.GetComponent<MeshRenderer>().material = indicatorMaterial1;
|
|
}
|
|
}
|
|
|
|
private void Update()
|
|
{
|
|
OnPoseUpdateRequest?.Invoke();// TODO. 로봇을 잡고 움직일 때와 아닐 때 구분하기
|
|
}
|
|
|
|
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 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);
|
|
|
|
List<float> list_jAngle = new List<float>();
|
|
list_jAngle.Add(robotData.j6);
|
|
list_jAngle.Add(robotData.j5);
|
|
list_jAngle.Add(robotData.j4);
|
|
list_jAngle.Add(robotData.j3);
|
|
list_jAngle.Add(robotData.j2);
|
|
list_jAngle.Add(robotData.j1);
|
|
|
|
kinematicsNode.SetCurrentJointAxisRotations(list_jAngle, 'x');
|
|
}
|
|
}
|