3using System.Collections;
4using System.Collections.Generic;
6using NaughtyAttributes;
10using Valve.VR.InteractionSystem;
11using ViveHandTracking;
30 public GameObject[]
arms =
new GameObject[3];
37 public float ULNA = 0.124f;
38 float GRIPPER = 0.058f;
44 private float initialRotation = 0.0f;
50 private bool isGripping =
false;
51 private float pumpTimeout =
float.MaxValue;
55 private float saveTimeout = 0.0f;
56 private AudioSource _as;
62 _as = GetComponent<AudioSource>();
81 StartCoroutine(Follow());
86 initialRotation =
Parent.eulerAngles.y;
106 if (saveTimeout > 0) saveTimeout -= Time.deltaTime;
108 if (GestureProvider.Current && GestureProvider.Current.engine &&
109 GestureProvider.Current.engine.State.LeftHand !=
null && isGripping)
110 if (GestureProvider.Current.engine.State.LeftHand.gesture == GestureType.Five)
112 serial.SendGripperRelease();
118 if (GestureProvider.Current && GestureProvider.Current.engine &&
119 GestureProvider.Current.engine.State.LeftHand !=
null && !isGripping)
120 if (GestureProvider.Current.engine.State.LeftHand.gesture == GestureType.OK)
128 if (GestureProvider.Current && GestureProvider.Current.engine &&
129 GestureProvider.Current.engine.State.LeftHand !=
null)
130 if (GestureProvider.Current.engine.State.LeftHand.gesture == GestureType.Victory)
132 if (saveTimeout > 0)
return;
138 if (GestureProvider.Current && GestureProvider.Current.engine &&
139 GestureProvider.Current.engine.State.LeftHand !=
null)
140 if (GestureProvider.Current.engine.State.LeftHand.gesture == GestureType.Like)
142 if (saveTimeout > 0)
return;
150 pumpTimeout -= Time.deltaTime;
151 if (pumpTimeout <= 0)
153 pumpTimeout =
float.MaxValue;
174 serial.SendGripperRelease();
182 pumpTimeout -= Time.deltaTime;
183 if (pumpTimeout <= 0)
185 pumpTimeout =
float.MaxValue;
193 GetComponentInParent<ThrowableCanDisable>().ForceDrop();
198 void SetArm(
float x,
float y,
float z)
201 float bas_angle_r = Mathf.Atan2(x, z);
202 float bas_angle_d = bas_angle_r * Mathf.Rad2Deg + 90f;
205 float s_w = x * x + z * z + wrt_y * wrt_y;
206 float s_w_sqrt = Mathf.Sqrt(s_w);
209 float elb_angle_r = Mathf.Acos((hum_sq + uln_sq - s_w) / (2f *
HUMERUS *
ULNA));
210 float elb_angle_d = 270f - elb_angle_r * Mathf.Rad2Deg;
213 float a1 = Mathf.Atan2(wrt_y, Mathf.Sqrt(x * x + z * z));
214 float a2 = Mathf.Acos((hum_sq + s_w - uln_sq) / (2f *
HUMERUS * s_w_sqrt));
215 float shl_angle_r = a1 + a2;
216 float shl_angle_d = 180f - shl_angle_r * Mathf.Rad2Deg;
231 if (bas_angle_d >= -10f + initialRotation && bas_angle_d <= 190f + initialRotation)
233 if (shl_angle_d >= 15f && shl_angle_d <= 60f)
235 if (elb_angle_d >= 40f && elb_angle_d <= 105f)
239 public float A = 0.0f;
240 public float B = 0.0f;
251 savedPos.Add(Vector4.positiveInfinity);
256 serial.SendGripperRelease();
260 savedPos.Add(Vector4.negativeInfinity);
267 private void releaseGrip()
270 serial.SendGripperRelease();
289 internal readonly List<SerializableVector4> savedPos =
new List<SerializableVector4>();
306 StartCoroutine(ActualRepaly());
309 private IEnumerator ActualRepaly()
311 foreach (var po
in savedPos)
313 if (
float.IsInfinity(po.x))
315 if (
float.IsNegativeInfinity(po.x))
323 yield
return new WaitForSeconds(1.0f);
327 var currpos = transform.parent.localPosition;
328 for (
int i = 0; i < 100; i++)
330 transform.parent.localPosition =
Vector3.Lerp(currpos, po.AsVector3(), i / 100f);
331 yield
return new WaitForSeconds(0.015f);
333 yield
return new WaitForSeconds(1.0f);
344 pos = Quaternion.Euler(
new Vector3(0, -initialRotation, 0)) * pos;
353 if (savedPos.Count > 0)
361 private void SetCurrentPose()
366 serial.SendMoveCommand(UnityToDobot(pos * 635 /
Parent.localScale.x));
371 private void FollowPose()
373 StartCoroutine(Follow());
377 private void StopFollow()
392 yield
return new WaitForSeconds(saveTimeout);
395 pos = Quaternion.Euler(
new Vector3(0, -initialRotation, 0)) * pos;
399 yield
return new WaitForSeconds(0.25f);
424 public GameObject prefabVisualize;
426 private void visualize()
428 var allline = File.ReadAllLines(
@"E:\teszt.log");
429 foreach (var s
in allline)
431 var x = Convert.ToSingle(s.Split(
',')[0].Trim().Replace(
'.',
','));
432 var y = Convert.ToSingle(s.Split(
',')[1].Trim().Replace(
'.',
','));
433 var z = Convert.ToSingle(s.Split(
',')[2].Trim().Replace(
'.',
','));
435 var spwn = Instantiate(prefabVisualize,
ZeroCoord);
436 spwn.transform.localPosition =
new Vector3(x,y,z) / 635;
441 Vector3 UnityToDobot(Vector3 value)
443 return new Vector3(value.z, -value.x, value.y);
446 Vector4 UnityToDobot(Vector3 value,
float r)
448 return new Vector4(value.z, -value.x, value.y, r);
494 return String.Format(
"[{0}, {1}, {2}]",
x,
y,
z);
504 return new Vector3(rValue.
x, rValue.
y, rValue.
z);
574 return String.Format(
"[{0}, {1}, {2}, {3}]",
x,
y,
z,
w);
584 return new Vector4(rValue.
x, rValue.
y, rValue.
z, rValue.
w);
599 return new Vector3(
x,
y,
z);
UnityEngine.UI.Button Button
void Trigger(string trigger)
static DobotGrabber instance
static void SaveToFile(string path, SerializableVector4[] content)
SerialControllerBytes serial
void ReplaySavedPositions()
void SavePositionsToFile()
float loseTrackingDistance
void ResetSavedPositions()
AnimatorTrigger animTrigger
Interactable interactable
SteamVR_Action_Boolean gripButton
Since unity doesn't flag the Vector3 as serializable, we need to create our own version....
override string ToString()
Returns a string representation of the object
SerializableVector3(float rX, float rY, float rZ)
Constructor
Since unity doesn't flag the Vector4 as serializable, we need to create our own version....
SerializableVector4(float rX, float rY, float rZ, float rW)
Constructor
SerializableVector4(Vector3 v, float rW)
override string ToString()
Returns a string representation of the object