Creating points for robot (End Effector) to go through them
Hi everyone.
I'm trying to create an environment, whereby the user would be able to save desired points (Button: Save Points) within the 3D space and once that is done, the end effector would go through said points (Button: Go). Somehow the issue I'm facing is that the robot's joint gets distorted after each iteration. Attached below is the image (Creating point 1-4) and code for the path teaching (To save points and go through them).
Would appreciate any help and thanks in advance.
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;
public class simPTP : MonoBehaviour
{
public GameObject Part1, Part2, Part3, Part4, Part5, Part6;
public float time = 1.0f;
Quaternion[] memy1Array = new Quaternion[5];
Quaternion[] memy2Array = new Quaternion[5];
Quaternion[] memy3Array = new Quaternion[5];
Quaternion[] memy4Array = new Quaternion[5];
Quaternion[] memy5Array = new Quaternion[5];
Quaternion[] memy6Array = new Quaternion[5];
int memPoints = 0;
public void SavePoint()
{
//saves position, up to a maximum of 5
if (memPoints < 5)
{
memy1Array[memPoints] = Part1.transform.localRotation;
memy2Array[memPoints] = Part2.transform.localRotation;
memy3Array[memPoints] = Part3.transform.localRotation;
memy4Array[memPoints] = Part4.transform.localRotation;
memy5Array[memPoints] = Part5.transform.localRotation;
memy6Array[memPoints] = Part6.transform.localRotation;
memPoints++;
}
}
public void GoThroughPoints()
{
/*if (time.text != null)
{
StartCoroutine(RotateMe(float.Parse(time.text)));
}*/
StartCoroutine(RotateMe(time));
}
IEnumerator RotateMe(float inTime)
{
for (int i = 0; i < memPoints; i++)
{
Quaternion Angle1 = Part1.transform.localRotation;
Quaternion Angle2 = Part2.transform.localRotation;
Quaternion Angle3 = Part3.transform.localRotation;
Quaternion Angle4 = Part4.transform.localRotation;
Quaternion Angle5 = Part5.transform.localRotation;
Quaternion Angle6 = Part6.transform.localRotation;
for (float t = 0f; t < 1f; t += Time.deltaTime / inTime)
{
Part1.transform.localRotation = Quaternion.Lerp(Angle1, memy1Array[i], t);
Part2.transform.localRotation = Quaternion.Lerp(Angle2, memy2Array[i], t);
Part3.transform.localRotation = Quaternion.Lerp(Angle3, memy3Array[i], t);
Part4.transform.localRotation = Quaternion.Lerp(Angle4, memy1Array[i], t);
Part5.transform.localRotation = Quaternion.Lerp(Angle5, memy2Array[i], t);
Part6.transform.localRotation = Quaternion.Lerp(Angle6, memy3Array[i], t);
yield return null;
}
}
}
// Start is called before the first frame update
void Start()
{
}
// Update is called once per frame
void Update()
{
}
}
path-teaching.png
(198.5 kB)
Comment