Tanoda
SolveIK.cs
Go to the documentation of this file.
1using System;
2using UnityEngine;
3using System.Collections;
4using System.Collections.Generic;
5using System.IO;
6using NaughtyAttributes;
7using Newtonsoft.Json;
8#if !UNITY_WEBGL
9using Valve.VR;
10using Valve.VR.InteractionSystem;
11using ViveHandTracking;
12#endif
13
14public class SolveIK : MonoBehaviour
15{
17 public bool UseController = false;
18 public bool useIK = true; // IK mode or manual adjustment
19
20 public Vector3 targetPosition;
21 public Vector3 currentPosition;
22
23 //[Range(0.0f, 180.0f)]
24 public float thetaBase = 90f;
25 [Range(5.0f, 90.0f)]
26 public float thetaShoulder = 45f;
27 [Range(0.0f, 100.0f)]
28 public float thetaElbow = 100f;
29
30 public GameObject[] arms = new GameObject[3];
31
33
34 /* Arm dimensions( m ) */
35 public float BASE_HGT = 0.267f;
36 public float HUMERUS = 0.124f;
37 public float ULNA = 0.124f;
38 float GRIPPER = 0.058f;
39
40 /* pre-calculations */
41 float hum_sq;
42 float uln_sq;
43
44 private float initialRotation = 0.0f;
45
46#if !UNITY_WEBGL
47 public Interactable interactable;
48 public SteamVR_Action_Boolean gripButton;
49#endif
50 private bool isGripping = false;
51 private float pumpTimeout = float.MaxValue;
53
54 public float loseTrackingDistance = 0.5f;
55 private float saveTimeout = 0.0f;
56 private AudioSource _as;
57
58 public Vector3 pickupOffset = Vector3.zero;
59
60 IEnumerator Start ()
61 {
62 _as = GetComponent<AudioSource>();
63 BASE_HGT = Vector3.Distance(Floor.position, LiftLink.position);
64 HUMERUS = Vector3.Distance(RearArm.position, FrontArm.position);
65 ULNA = Vector3.Distance(FrontArm.position, GripperLink.position);
66
67 /* pre-calculations */
68 hum_sq = HUMERUS*HUMERUS;
69 uln_sq = ULNA*ULNA;
70
71 //serial.SendGripperRelease();
72 //yield return new WaitForSeconds(2.0f);
73 //serial.SendGripperOff();
74 yield return null;
75
76 //RotationHelper.localEulerAngles = new Vector3(5, 0, Rotation);
77 //var pos = GripperLink.position - ZeroCoord.position;
78 //pos = Quaternion.Euler(new Vector3(0, -initialRotation, 0)) * pos;
79 //UnityToDobot(pos * 635 / Parent.localScale.x, Rotation);
80
81 StartCoroutine(Follow());
82 }
83
84 void Update()
85 {
86 initialRotation = Parent.eulerAngles.y;
87 currentPosition = Floor.position;
88 // Set target position from itself
89 targetPosition = transform.position - pickupOffset;
90
91 if (useIK)
92 {
94 }
95
96 // Update robot arm model
97 arms[0].transform.localRotation = Quaternion.Euler(new Vector3(0f, 0f, thetaBase + 90 - initialRotation));
98 arms[1].transform.localRotation = Quaternion.Euler(new Vector3(thetaShoulder + 90, 0f, 0f));
99 arms[2].transform.localRotation = Quaternion.Euler(new Vector3(thetaElbow, 0f, 0f));
100
101 GripperLink.eulerAngles = new Vector3(90, 0, 180 - thetaBase - 90);
102#if !UNITY_WEBGL
103
104 if (!UseController)
105 {
106 if (saveTimeout > 0) saveTimeout -= Time.deltaTime;
107
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)
111 {
112 serial.SendGripperRelease();
113 isGripping = false;
114 pumpTimeout = 5f;
115 animTrigger.Trigger("Release");
116 }
117
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)
121 {
122 serial.SendGripperGrip();
123 isGripping = true;
124 pumpTimeout = 5f;
125 animTrigger.Trigger("Grip");
126 }
127
128 if (GestureProvider.Current && GestureProvider.Current.engine &&
129 GestureProvider.Current.engine.State.LeftHand != null)
130 if (GestureProvider.Current.engine.State.LeftHand.gesture == GestureType.Victory)
131 {
132 if (saveTimeout > 0) return;
133 saveTimeout = 1.0f;
134 _as.Play();
136 }
137
138 if (GestureProvider.Current && GestureProvider.Current.engine &&
139 GestureProvider.Current.engine.State.LeftHand != null)
140 if (GestureProvider.Current.engine.State.LeftHand.gesture == GestureType.Like)
141 {
142 if (saveTimeout > 0) return;
143 saveTimeout = 1.0f;
144 _as.Play();
145 //DobotXMLSerializer.SaveToFile("save.xml", serial.savedPos);
146 serial.savedPos.Clear();
147 }
148
149
150 pumpTimeout -= Time.deltaTime;
151 if (pumpTimeout <= 0)
152 {
153 pumpTimeout = float.MaxValue;
155 }
156 }
157 else
158 {
159 if (!interactable)
160 {
161 return;
162 }
163 if (interactable.attachedToHand && gripButton.GetState(interactable.attachedToHand.handType) && !isGripping)
164 {
165 serial.SendGripperGrip();
166 isGripping = true;
167 pumpTimeout = 5f;
168 animTrigger.Trigger("Grip");
170 }
171 else if (interactable.attachedToHand && !gripButton.GetState(interactable.attachedToHand.handType) &&
172 isGripping)
173 {
174 serial.SendGripperRelease();
175 isGripping = false;
176 pumpTimeout = 5f;
177 animTrigger.Trigger("Release");
179 }
180 else
181 {
182 pumpTimeout -= Time.deltaTime;
183 if (pumpTimeout <= 0)
184 {
185 pumpTimeout = float.MaxValue;
187 }
188 }
189 }
190
191 if (Vector3.Distance(Parent.position, transform.parent.position) > loseTrackingDistance)
192 {
193 GetComponentInParent<ThrowableCanDisable>().ForceDrop();
194 }
195#endif
196 }
197
198 void SetArm(float x, float y, float z)
199 {
200 // Base angle
201 float bas_angle_r = Mathf.Atan2(x, z);
202 float bas_angle_d = bas_angle_r * Mathf.Rad2Deg + 90f;
203
204 float wrt_y = y - BASE_HGT; // Wrist relative height to shoulder
205 float s_w = x * x + z * z + wrt_y * wrt_y; // Shoulder to wrist distance square
206 float s_w_sqrt = Mathf.Sqrt(s_w);
207
208 // Elbow angle: knowing 3 edges of the triangle, get the angle
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;
211
212 // Shoulder angle = a1 + a2
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;
217
218 elb_angle_d -= 100;
219 shl_angle_d -= 90f;
220 A = shl_angle_d;
221 B = elb_angle_d;
222
223 // Update angles
224 //if (bas_angle_d >= -20f + initialRotation && bas_angle_d <= 200f + initialRotation)
225 // thetaBase = bas_angle_d;
226 //if (shl_angle_d >= 5f && shl_angle_d <= 90f)
227 // thetaShoulder = shl_angle_d;
228 //if (elb_angle_d >= 0f && elb_angle_d <= 100f)
229 // thetaElbow = elb_angle_d;
230
231 if (bas_angle_d >= -10f + initialRotation && bas_angle_d <= 190f + initialRotation)
232 thetaBase = bas_angle_d;
233 if (shl_angle_d >= 15f && shl_angle_d <= 60f)
234 thetaShoulder = shl_angle_d;
235 if (elb_angle_d >= 40f && elb_angle_d <= 105f)
236 thetaElbow = elb_angle_d;
237 }
238
239 public float A = 0.0f;
240 public float B = 0.0f;
241
242 public void ToggleGripping()
243 {
244#if !UNITY_WEBGL
245 if (!isGripping)
246 {
247 serial.SendGripperGrip();
248 isGripping = true;
249 pumpTimeout = 5f;
250 animTrigger.Trigger("Grip");
251 savedPos.Add(Vector4.positiveInfinity);
253 }
254 else
255 {
256 serial.SendGripperRelease();
257 isGripping = false;
258 pumpTimeout = 5f;
259 animTrigger.Trigger("Release");
260 savedPos.Add(Vector4.negativeInfinity);
262 }
263#endif
264 }
265
266
267 private void releaseGrip()
268 {
269#if !UNITY_WEBGL
270 serial.SendGripperRelease();
271 isGripping = false;
272 pumpTimeout = 5f;
273 animTrigger.Trigger("Release");
275#endif
276 }
277
278 private void grip()
279 {
280#if !UNITY_WEBGL
281 serial.SendGripperGrip();
282 isGripping = true;
283 pumpTimeout = 5f;
284 animTrigger.Trigger("Grip");
286#endif
287 }
288
289 internal readonly List<SerializableVector4> savedPos = new List<SerializableVector4>();
290
292 {
293 savedPos.Clear();
294 }
295
296 public void ResetStage()
297 {
298 savedPos.Clear();
299 ReplayLogic.instance.FastReplay();
300 }
301
302 [Button]
304 {
305 StopAllCoroutines();
306 StartCoroutine(ActualRepaly());
307 }
308
309 private IEnumerator ActualRepaly()
310 {
311 foreach (var po in savedPos)
312 {
313 if (float.IsInfinity(po.x)) //Grip
314 {
315 if (float.IsNegativeInfinity(po.x))
316 {
317 releaseGrip();
318 }
319 else
320 {
321 grip();
322 }
323 yield return new WaitForSeconds(1.0f);
324 }
325 else //Move
326 {
327 var currpos = transform.parent.localPosition;
328 for (int i = 0; i < 100; i++)
329 {
330 transform.parent.localPosition = Vector3.Lerp(currpos, po.AsVector3(), i / 100f);
331 yield return new WaitForSeconds(0.015f);
332 }
333 yield return new WaitForSeconds(1.0f);
334 }
335 }
336 yield return null;
337 FollowPose();
338 }
339
340 [Button]
341 public void SavePosition()
342 {
343 var pos = GripperLink.position - ZeroCoord.position;
344 pos = Quaternion.Euler(new Vector3(0, -initialRotation, 0)) * pos;
345 savedPos.Add(new SerializableVector4(UnityToDobot(pos * 635 / Parent.localScale.x)/*transform.parent.localPosition*/, Rotation));
346 }
347
348 [Button]
350 {
351#if !UNITY_WEBGL
352 //File.WriteAllText("DOBOT_POSITIONS.json", JsonConvert.SerializeObject(savedPos));
353 if (savedPos.Count > 0)
354 {
355 DobotXMLSerializer.SaveToFile(NetworkManager.instance.selectedLevel.name + ".xml", savedPos);
356 }
357#endif
358 }
359
360 [Button]
361 private void SetCurrentPose()
362 {
363 var pos = GripperLink.position - ZeroCoord.position;
364
365#if !UNITY_WEBGL
366 serial.SendMoveCommand(UnityToDobot(pos * 635 / Parent.localScale.x));
367#endif
368 }
369
370 [Button]
371 private void FollowPose()
372 {
373 StartCoroutine(Follow());
374 }
375
376 [Button]
377 private void StopFollow()
378 {
379 StopAllCoroutines();
380 }
381
382 public Transform RotationHelper;
383 [Range(-149, 150)]
384 public float Rotation = 0.0f; // -149 - 150
385 IEnumerator Follow()
386 {
387 while (true)
388 {
389 RotationHelper.localEulerAngles = new Vector3(5,0,Rotation);
390 if (saveTimeout > 0)
391 {
392 yield return new WaitForSeconds(saveTimeout);
393 }
394 var pos = GripperLink.position - ZeroCoord.position;
395 pos = Quaternion.Euler(new Vector3(0, -initialRotation, 0)) * pos;
396#if !UNITY_WEBGL
397 serial.SendMoveCommand(UnityToDobot(pos * 635 / Parent.localScale.x, Rotation));
398#endif
399 yield return new WaitForSeconds(0.25f);
400 }
401 }
402
403 public void RotateLeft()
404 {
405 Rotation -= 1;
406
407 if (Rotation < -149)
408 Rotation = -149;
409
410 RotationHelper.localEulerAngles = new Vector3(5, 0, Rotation);
411 }
412
413 public void RotateRight()
414 {
415 Rotation += 1;
416
417 if (Rotation > 150)
418 Rotation = 150;
419
420 RotationHelper.localEulerAngles = new Vector3(5, 0, Rotation);
421 }
422
423#if UNITY_EDITOR
424 public GameObject prefabVisualize;
425 [Button]
426 private void visualize()
427 {
428 var allline = File.ReadAllLines(@"E:\teszt.log");
429 foreach (var s in allline)
430 {
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('.', ','));
434
435 var spwn = Instantiate(prefabVisualize, ZeroCoord);
436 spwn.transform.localPosition = new Vector3(x,y,z) / 635;
437 }
438 }
439#endif
440
441 Vector3 UnityToDobot(Vector3 value)
442 {
443 return new Vector3(value.z, -value.x, value.y);
444 }
445
446 Vector4 UnityToDobot(Vector3 value, float r)
447 {
448 return new Vector4(value.z, -value.x, value.y, r);
449 }
450}
451
457[System.Serializable]
459{
463 public float x;
464
468 public float y;
469
473 public float z;
474
481 public SerializableVector3(float rX, float rY, float rZ)
482 {
483 x = rX;
484 y = rY;
485 z = rZ;
486 }
487
492 public override string ToString()
493 {
494 return String.Format("[{0}, {1}, {2}]", x, y, z);
495 }
496
502 public static implicit operator Vector3(SerializableVector3 rValue)
503 {
504 return new Vector3(rValue.x, rValue.y, rValue.z);
505 }
506
512 public static implicit operator SerializableVector3(Vector3 rValue)
513 {
514 return new SerializableVector3(rValue.x, rValue.y, rValue.z);
515 }
516}
522[Serializable]
524{
528 public float x;
529
533 public float y;
534
538 public float z;
539
543 public float w;
544
552 public SerializableVector4(float rX, float rY, float rZ, float rW)
553 {
554 x = rX;
555 y = rY;
556 z = rZ;
557 w = rW;
558 }
559
560 public SerializableVector4(Vector3 v, float rW)
561 {
562 x = v.x;
563 y = v.y;
564 z = v.z;
565 w = rW;
566 }
567
572 public override string ToString()
573 {
574 return String.Format("[{0}, {1}, {2}, {3}]", x, y, z, w);
575 }
576
582 public static implicit operator Vector4(SerializableVector4 rValue)
583 {
584 return new Vector4(rValue.x, rValue.y, rValue.z, rValue.w);
585 }
586
592 public static implicit operator SerializableVector4(Vector4 rValue)
593 {
594 return new SerializableVector4(rValue.x, rValue.y, rValue.z, rValue.w);
595 }
596
597 public Vector3 AsVector3()
598 {
599 return new Vector3(x,y,z);
600 }
601}
UnityEngine.UI.Button Button
Definition: Pointer.cs:7
void Trigger(string trigger)
void ReleaseNow()
Definition: DobotGrabber.cs:47
static DobotGrabber instance
Definition: DobotGrabber.cs:9
void GrabNow()
Definition: DobotGrabber.cs:26
static void SaveToFile(string path, SerializableVector4[] content)
float B
Definition: SolveIK.cs:240
bool useIK
Definition: SolveIK.cs:18
Transform FrontArm
Definition: SolveIK.cs:32
Vector3 currentPosition
Definition: SolveIK.cs:21
SerialControllerBytes serial
Definition: SolveIK.cs:16
Transform LiftLink
Definition: SolveIK.cs:32
void ReplaySavedPositions()
Definition: SolveIK.cs:303
float HUMERUS
Definition: SolveIK.cs:36
bool UseController
Definition: SolveIK.cs:17
float ULNA
Definition: SolveIK.cs:37
void SavePosition()
Definition: SolveIK.cs:341
Transform RearArm
Definition: SolveIK.cs:32
Transform Center
Definition: SolveIK.cs:32
float thetaBase
Definition: SolveIK.cs:24
void SavePositionsToFile()
Definition: SolveIK.cs:349
Transform Floor
Definition: SolveIK.cs:32
float loseTrackingDistance
Definition: SolveIK.cs:54
float thetaShoulder
Definition: SolveIK.cs:26
void ResetSavedPositions()
Definition: SolveIK.cs:291
void RotateRight()
Definition: SolveIK.cs:413
Transform RotationHelper
Definition: SolveIK.cs:382
float thetaElbow
Definition: SolveIK.cs:28
Vector3 pickupOffset
Definition: SolveIK.cs:58
Transform GripperLink
Definition: SolveIK.cs:32
Transform ZeroCoord
Definition: SolveIK.cs:32
Transform Parent
Definition: SolveIK.cs:32
AnimatorTrigger animTrigger
Definition: SolveIK.cs:52
Vector3 targetPosition
Definition: SolveIK.cs:20
float BASE_HGT
Definition: SolveIK.cs:35
Interactable interactable
Definition: SolveIK.cs:47
float A
Definition: SolveIK.cs:239
void RotateLeft()
Definition: SolveIK.cs:403
float Rotation
Definition: SolveIK.cs:384
GameObject[] arms
Definition: SolveIK.cs:30
SteamVR_Action_Boolean gripButton
Definition: SolveIK.cs:48
void ResetStage()
Definition: SolveIK.cs:296
void ToggleGripping()
Definition: SolveIK.cs:242
Since unity doesn't flag the Vector3 as serializable, we need to create our own version....
Definition: SolveIK.cs:459
float x
x component
Definition: SolveIK.cs:463
override string ToString()
Returns a string representation of the object
Definition: SolveIK.cs:492
SerializableVector3(float rX, float rY, float rZ)
Constructor
Definition: SolveIK.cs:481
float z
z component
Definition: SolveIK.cs:473
float y
y component
Definition: SolveIK.cs:468
Since unity doesn't flag the Vector4 as serializable, we need to create our own version....
Definition: SolveIK.cs:524
float w
w component
Definition: SolveIK.cs:543
float y
y component
Definition: SolveIK.cs:533
SerializableVector4(float rX, float rY, float rZ, float rW)
Constructor
Definition: SolveIK.cs:552
float x
x component
Definition: SolveIK.cs:528
SerializableVector4(Vector3 v, float rW)
Definition: SolveIK.cs:560
override string ToString()
Returns a string representation of the object
Definition: SolveIK.cs:572
Vector3 AsVector3()
Definition: SolveIK.cs:597
float z
z component
Definition: SolveIK.cs:538