bool compare(int num)
{
var res = FanucModel.GetCoordsFromMat(model.ForwardTask(coords[num]));
var resInv = model.InverseTask(ref res);
bool[] f = new bool[resInv.Length / 6];
for (int i = 0; i < resInv.Length / 6; ++i)
{
f[i] = true;
for (int j = 0; j < 6; ++j)
{
if (Mathf.Abs(resInv[i, j] - coords[num][j]) > 0.5)
{
f[i] = false;
}
}
}
bool finalF = false;
for (int i = 0; i < resInv.Length / 6; ++i)
{
finalF = f[i] || finalF;
}
return(finalF);
}
void FixedUpdate()
{
A = PointA.transform.position;
B = PointB.transform.position;
if (Camera.GetComponent <raycast>().aims.Count > 0)
{
C = Camera.GetComponent <raycast>().aims[0];
C.y = teleg.transform.position.y;
}
else
{
C = A;
C.y = teleg.transform.position.y;
}
a = B - A;
b = C - A;
if (Math.Abs(A.x - C.x) < step && (Math.Abs(A.z - C.z) < step))
{
if (Camera.GetComponent <raycast>().aims.Count > 0)
{
isMoved = true;
Camera.GetComponent <raycast>().aims.RemoveAt(0);
Destroy(Camera.GetComponent <raycast>().balls[0]);
Camera.GetComponent <raycast>().balls.RemoveAt(0);
if (Camera.GetComponent <raycast>().aims.Count > 0)
{
C = Camera.GetComponent <raycast>().aims[0];
C.y = teleg.transform.position.y;
}
}
else
{
isMoved = false;
}
}
switch (Type)
{
case MoveType.RotateTeleguAndMove:
Directional();
break;
case MoveType.RotateBarsAndMove:
Parallel();
break;
}
float[] coord = FanucModel.GetCoordsFromMat(CoordTransformation.UnityToRobot(FanucModel.coordMatrixDegrees(teleg.transform.position, teleg.transform.eulerAngles)));
curPosText.text = coord[0].ToString("0.00") + " " + coord[1].ToString("0.00") + " " + coord[5].ToString("0.00");
}
// coords: x y z w p r
public override float[] CalculateWrist(ref float[] coords)
{
var param = KinematicChain;
Matrix4x4 rot = FanucModel.rotMatrixDegrees(coords[3], coords[4], coords[5]);
float[] coord = new float[]
{
coords[0] - rot[0, 2] * param[5]._dParam,
coords[1] - rot[1, 2] * param[5]._dParam,
coords[2] - rot[2, 2] * param[5]._dParam,
coords[3],
coords[4],
coords[5]
};
return(coord);
}
void FixedUpdate()
{
if (Input.anyKey)
{
first.transform.localRotation = Quaternion.Euler(0, 0, -jointAngles[0]);
second.transform.localRotation = Quaternion.Euler(0, -jointAngles[1], 0);
third.transform.localRotation = Quaternion.Euler(0, jointAngles[2] + jointAngles[1], 0);
fourth.transform.localRotation = Quaternion.Euler(-jointAngles[3], 0, 0);
fifth.transform.localRotation = Quaternion.Euler(0, jointAngles[4], 0);
sixth.transform.localRotation = Quaternion.Euler(jointAngles[5], 0, 0);
for (int i = 0; i < 6; ++i)
{
jointAnglesInc[i] = Input.GetAxis(Axis[i]) * speed * Time.deltaTime;
jointAngles[i] += jointAnglesInc[i];
}
//jointAngles[0] += Input.GetAxis("First") * speed * Time.deltaTime;
//jointAngles[1] += Input.GetAxis("Second") * speed * Time.deltaTime;
//jointAngles[2] += Input.GetAxis("Third") * speed * Time.deltaTime;
//jointAngles[3] += Input.GetAxis("Fourth") * speed * Time.deltaTime;
//jointAngles[4] += Input.GetAxis("Fifth") * speed * Time.deltaTime;
//jointAngles[5] += Input.GetAxis("Sixth") * speed * Time.deltaTime;
}
string outputAngles = "";
string outputForwardTask = "";
var res = FanucModel.GetCoordsFromMat(model.fanucForwardTask(ref jointAngles));
foreach (float j in jointAngles)
{
outputAngles += (j.ToString("0.00") + ", ");
}
jointCoord.text = outputAngles;
foreach (float r in res)
{
outputForwardTask += (r.ToString("0.00") + ", ");
}
worldCoord.text = outputForwardTask;
// Debug.Log(outputAngles + "| " + outputForwardTask);
}
//move to FanucScriptUI
void CoordDisplayAndSave()
{
string outputAngles = "";
string outputForwardTask = "";
worldPos = FanucModel.GetCoordsFromMat(model.ForwardTask(jointAngles));
foreach (float j in jointAngles)
{
outputAngles += (j.ToString("0.00") + ", ");
}
jointCoord.text = outputAngles;
foreach (float r in worldPos)
{
outputForwardTask += (r.ToString("0.00") + ", ");
}
worldCoord.text = outputForwardTask;
// Debug.Log(outputAngles + "| " + outputForwardTask);
}
// Use this for initialization
void Start()
{
isMoved = false;
rotstep = WheelAngleSpeed * WheelRadius / telegaRadius;
Debug.Log(rotstep);
//rotstep = 1e-3f;
step = WheelAngleSpeed * WheelRadius * Mathf.Deg2Rad;
C = PointA.transform.position;
//Net = SceneManager.Net;
//Net.gameObject.SetActive(true);
Camera = GameObject.Find("Camera");
modeText = GameObject.Find("Telega Mode Text").GetComponent <Text>();
curPosText = GameObject.Find("Telega currnet position").GetComponent <Text>();
Debug.Log(teleg.transform.position + " " + teleg.transform.eulerAngles);
var res = FanucModel.GetCoordsFromMat(CoordTransformation.UnityToRobot(FanucModel.coordMatrixDegrees(teleg.transform.position, teleg.transform.eulerAngles)));
Debug.Log(res[0] + " " + res[1] + " " + res[2] + " " + res[3] + " " + res[4] + " " + res[5]);
modeText.text = "Directional";
}
void DropdownValueChanged(Dropdown change)
{
var obj = SceneManager.Pull.Find(change.value);
//Debug.Log("ROT: " + obj.transform.rotation.eulerAngles);
//Vector4 coordsPos = new Vector4 ( obj.transform.position[0], obj.transform.position[1], obj.transform.position[2], 1 );
Debug.Log(obj.transform.eulerAngles);
Matrix4x4 coords = FanucModel.rotMatrixDegrees(obj.transform.eulerAngles.x, obj.transform.eulerAngles.y, obj.transform.eulerAngles.z);
coords[0, 3] = obj.transform.position[0];
coords[1, 3] = obj.transform.position[1];
coords[2, 3] = obj.transform.position[2];
coords = CoordTransformation.UnityToRobot(coords);
var outCoords = FanucModel.GetCoordsFromMat(coords);
outCoordsStr = outCoords[0] + " " + outCoords[1] + " " + (outCoords[2] + 190f) + " " + outCoords[3] + " "
+ outCoords[4] + " " + outCoords[5];
SceneManager.FanucSettingsPanel.coordField.text = outCoordsStr;
//coordText.text = coordTrans.UnityToRobotPosOnly(coordsPos).ToString();
}
void FixedUpdate()
{
Debug.Log(semafor);
if (Input.anyKey && !Input.GetMouseButton(0) && !Input.GetMouseButton(1) && !SceneManager.ObserverMode && !SceneManager.UserControlLock && !inputField.isFocused && !InputSpeedField.isFocused && !addPoint.input.isFocused)
{
Debug.Log("im in update");
for (int i = 0; i < 6; ++i)
{
UpdateCoord(i);
if (mode == 0)
{
if (jointAngles[i] + jointAnglesInc[i] > model.limMin[i] && jointAngles[i] + jointAnglesInc[i] < model.limMax[i])
{
jointAngles[i] += jointAnglesInc[i];
}
}
if (mode == 2)
{
worldPos[i] += worldPosInc[i];
}
}
RotateFanuc(FanucColliders, jointAngles);
if (NoCollisions)
{
RotateFanuc(Fanuc, jointAngles);
}
else
{
StopCoroutine("Move");
}
if (ReadytoSend)
{
StartCoroutine(CoordtoServer());
ReadytoSend = false;
}
NoCollisions = true;
if (mode == 2)
{
try
{
var tmp = model.InverseTask(ref worldPos);
if (tmp.Length == 0)
{
Debug.Log("ZEROOOOOOOO");
float[] worldPosTmp = new float[] {
worldPos[0] - (worldPosInc[0] / 2.0f),
worldPos[1] - (worldPosInc[1] / 2.0f),
worldPos[2] - (worldPosInc[2] / 2.0f),
worldPos[3] - (worldPosInc[3] / 2.0f),
worldPos[4] - (worldPosInc[4] / 2.0f),
worldPos[5] - (worldPosInc[5] / 2.0f)
};
tmp = model.InverseTask(ref worldPosTmp);
}
jointAngles = FanucModel.chooseNearestPose(tmp, ref jointAngles);
}
catch (System.Exception)
{
Debug.Log("GOT IT");
}
//RotateFanuc(Fanuc, jointAngles);
}
RotateFanuc(FanucColliders, jointAngles);
if (NoCollisions)
{
RotateFanuc(Fanuc, jointAngles);
}
else
{
StopCoroutine("Move");
}
if (ReadytoSend)
{
StartCoroutine(CoordtoServer());
ReadytoSend = false;
}
NoCollisions = true;
//realize what the hell is going on here
//move to FanucScriptUI
CoordDisplayAndSave();
}
}
public IEnumerator Move(float[] newCoord)
{
semafor += 1;
SceneManager.UserControlLock = true;
Debug.Log(newCoord[0] + " " + newCoord[1] + " " + newCoord[2] + " " +
newCoord[3] + " " + newCoord[4] + " " + newCoord[5] + " " + mode + "--------------------------------------------------------");
//Debug.Log(jointAngles[0] - newCoord[0]);
float[] diff = new float[6];
float normCoef;
if (mode == 0)
{
for (int i = 0; i < 6; ++i)
{
diff[i] = newCoord[i] - jointAngles[i];
}
normCoef = Mathf.Sqrt(diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2] +
diff[3] * diff[3] + diff[4] * diff[4] + diff[5] * diff[5]);
}
else
{
float[] newCoordTmp = FanucModel.chooseNearestPose(model.InverseTask(ref newCoord), ref jointAngles);
newCoord = newCoordTmp;
for (int i = 0; i < 6; ++i)
{
diff[i] = newCoord[i] - jointAngles[i];
}
normCoef = Mathf.Sqrt(diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2] +
diff[3] * diff[3] + diff[4] * diff[4] + diff[5] * diff[5]);
}
float[] diff2 = new float[6];
float error = normCoef;
while (error > 1 + speed * 0.02 && NoCollisions)
{
for (int i = 0; i < 6; ++i)
{
diff2[i] = newCoord[i] - jointAngles[i];
}
error = Mathf.Sqrt(diff2[0] * diff2[0] + diff2[1] * diff2[1] + diff2[2] * diff2[2] +
diff2[3] * diff2[3] + diff2[4] * diff2[4] + diff2[5] * diff2[5]);
//Debug.Log(error);
for (int i = 0; i < 6; ++i)
{
jointAngles[i] += diff[i] * speed * Time.deltaTime / normCoef;
}
RotateFanuc(FanucColliders, jointAngles);
//Debug.Log(NoCollisions);
yield return(new WaitForFixedUpdate());
RotateFanuc(Fanuc, jointAngles);
CoordDisplayAndSave();
if (ReadytoSend)
{
StartCoroutine(CoordtoServer());
ReadytoSend = false;
}
}
if (NoCollisions)
{
for (int i = 0; i < 6; ++i)
{
jointAngles[i] = newCoord[i];
}
CoordDisplayAndSave();
}
if (!SceneManager.ObserverMode)
{
SceneManager.UserControlLock = false;
}
semafor -= 1;
}
// coords: x y z w p r
public float[,] InverseTask(ref float[] coordIn)
{
var param = KinematicChain;
float[] coord = CalculateWrist(ref coordIn);
float a = 2.0f * param[0]._aParam * coord[0];
float b = 2.0f * param[0]._aParam * coord[1];
float c = 2.0f * param[1]._aParam * param[2]._aParam - 2.0f * param[1]._dParam * param[3]._dParam *
Mathf.Sin(param[1]._alphaParam) * Mathf.Sin(param[2]._alphaParam);
float d = 2.0f * param[2]._aParam * param[1]._dParam * Mathf.Sin(param[1]._alphaParam) + 2.0f * param[1]._aParam * param[3]._dParam
* Mathf.Sin(param[2]._alphaParam);
float e = param[1]._aParam * param[1]._aParam + param[2]._aParam * param[2]._aParam + param[1]._dParam *
param[1]._dParam + param[2]._dParam * param[2]._dParam + param[3]._dParam * param[3]._dParam -
param[0]._aParam * param[0]._aParam - coord[0] * coord[0] - coord[1] * coord[1] -
(coord[2] - param[0]._dParam) * (coord[2] - param[0]._dParam) + 2.0f *
param[1]._dParam * param[2]._dParam * Mathf.Cos(param[1]._alphaParam) + 2.0f * param[1]._dParam * param[3]._dParam *
Mathf.Cos(param[1]._alphaParam) * Mathf.Cos(param[2]._alphaParam) + 2.0f * param[2]._dParam * param[3]._dParam * Mathf.Cos(param[2]._alphaParam);
float f = coord[1] * Mathf.Sin(param[0]._alphaParam);
float g = -coord[0] * Mathf.Sin(param[0]._alphaParam);
float h = -param[3]._dParam * Mathf.Sin(param[1]._alphaParam) * Mathf.Sin(param[2]._alphaParam);
float i = param[2]._aParam * Mathf.Sin(param[1]._alphaParam);
float j = param[1]._dParam + param[2]._dParam * Mathf.Cos(param[1]._alphaParam) + param[3]._dParam *
Mathf.Cos(param[1]._alphaParam) * Mathf.Cos(param[2]._alphaParam) - (coord[2] - param[0]._dParam) *
Mathf.Cos(param[0]._alphaParam);
float r = 4.0f * param[0]._aParam * param[0]._aParam * (j - h) * (j - h) + Mathf.Sin(param[0]._alphaParam) *
Mathf.Sin(param[0]._alphaParam) * (e - c) * (e - c)
- 4.0f * param[0]._aParam * param[0]._aParam * Mathf.Sin(param[0]._alphaParam) * Mathf.Sin(param[0]._alphaParam)
* (coord[0] * coord[0] + coord[1] * coord[1]);
float s = 4.0f * (4.0f * param[0]._aParam * param[0]._aParam * i * (j - h) + Mathf.Sin(param[0]._alphaParam) *
Mathf.Sin(param[0]._alphaParam) * d *
(e - c));
float t = 2.0f * (4.0f * param[0]._aParam * param[0]._aParam * (j * j - h * h + 2.0f * i * i) +
Mathf.Sin(param[0]._alphaParam) * Mathf.Sin(param[0]._alphaParam)
* (e * e - c * c + 2.0f * d * d) - 4.0f * param[0]._aParam * param[0]._aParam *
Mathf.Sin(param[0]._alphaParam) * Mathf.Sin(param[0]._alphaParam) *
(coord[0] * coord[0] + coord[1] * coord[1]));
float u = 4.0f * (4.0f * param[0]._aParam * param[0]._aParam * i * (j + h) +
Mathf.Sin(param[0]._alphaParam) * Mathf.Sin(param[0]._alphaParam) * d * (e + c));
float v = 4.0f * param[0]._aParam * param[0]._aParam * (h + j) * (h + j) + Mathf.Sin(param[0]._alphaParam) *
Mathf.Sin(param[0]._alphaParam) *
(e + c) * (e + c) - 4.0f * param[0]._aParam * param[0]._aParam * Mathf.Sin(param[0]._alphaParam) *
Mathf.Sin(param[0]._alphaParam) *
(coord[0] * coord[0] + coord[1] * coord[1]);
float[] x = new float[4];
int numberOfRoots = Poly34.SolveP4(ref x, s / r, t / r, u / r, v / r);
if (numberOfRoots != 2 && numberOfRoots != 4)
{
Debug.Log("something is wrong with roots of equation");
return(new float[0, 0]);
}
float[,] theta = new float[numberOfRoots, 3];
for (int it = 0; it < numberOfRoots; ++it)
{
theta[it, 2] = 2.0f * Mathf.Atan(x[it]);
}
float costheta, sintheta;
for (int it = 0; it < numberOfRoots; ++it)
{
costheta = (-g * (c * Mathf.Cos(theta[it, 2]) + d * Mathf.Sin(theta[it, 2]) + e) +
b * (h * Mathf.Cos(theta[it, 2]) + i * Mathf.Sin(theta[it, 2] + j))) / (a * g - f * b);
sintheta = (f * (c * Mathf.Cos(theta[it, 2]) + d * Mathf.Sin(theta[it, 2]) + e) -
a * (h * Mathf.Cos(theta[it, 2]) + i * Mathf.Sin(theta[it, 2] + j))) / (a * g - f * b);
theta[it, 0] = Mathf.Atan2(sintheta, costheta);
}
for (int it = 0; it < numberOfRoots; ++it)
{
float a11 = param[1]._aParam + param[2]._aParam * Mathf.Cos(theta[it, 2]) +
param[3]._dParam * Mathf.Sin(param[2]._alphaParam) * Mathf.Sin(theta[it, 2]);
float a12 = -param[2]._aParam * Mathf.Cos(param[1]._alphaParam) *
Mathf.Sin(theta[it, 2]) + param[2]._dParam * Mathf.Sin(param[1]._alphaParam) + param[3]._dParam *
Mathf.Sin(param[2]._alphaParam) * Mathf.Cos(param[1]._alphaParam) * Mathf.Cos(theta[it, 2]) +
param[3]._dParam * Mathf.Sin(param[1]._alphaParam) * Mathf.Cos(param[2]._alphaParam);
costheta = (a11 * (coord[0] * Mathf.Cos(theta[it, 0]) + coord[1] * Mathf.Sin(theta[it, 0]) - param[0]._aParam)
- a12 * (-coord[0] * Mathf.Cos(param[0]._alphaParam) * Mathf.Sin(theta[it, 0]) + coord[1] *
Mathf.Cos(param[0]._alphaParam) * Mathf.Cos(theta[it, 0]) + (coord[2] - param[0]._dParam) * Mathf.Sin(param[0]._alphaParam)))
/ (a11 * a11 + a12 * a12);
sintheta = (a12 * (coord[0] * Mathf.Cos(theta[it, 0]) + coord[1] * Mathf.Sin(theta[it, 0]) - param[0]._aParam) + a11 *
(-coord[0] * Mathf.Cos(param[0]._alphaParam) * Mathf.Sin(theta[it, 0]) + coord[1] *
Mathf.Cos(param[0]._alphaParam) *
Mathf.Cos(theta[it, 0]) + (coord[2] - param[0]._dParam) * Mathf.Sin(param[0]._alphaParam))) / (a11 * a11 + a12 * a12);
theta[it, 1] = Mathf.Atan2(sintheta, costheta);
}
List <int> ind = new List <int>();
for (int it = 0; it < numberOfRoots; ++it)
{
bool isOk = true;
float[] q = { theta[it, 0], theta[it, 1], theta[it, 2], 0f, 0f, 0f };
var joints = JointsToQReverse(ref q);
for (int jt = 0; jt < 3; ++jt)
{
if (joints[jt] > limMax[jt] * Mathf.Deg2Rad || joints[jt] < limMin[jt] * Mathf.Deg2Rad)
{
isOk = false;
}
}
if (!float.IsNaN(theta[it, 1]) && isOk)
{
ind.Add(it);
}
}
if (ind.Count == 0)
{
Debug.Log("No solution for positioning task");
return(new float[0, 0]);
}
//--------------------------------------------------------------
int k = 2;
float[,] thetaPrefinal = new float[ind.Count * k, 6];
for (int it = 0; it < ind.Count; ++it)
{
for (int zt = 0; zt < k; ++zt)
{
thetaPrefinal[it * k + zt, 0] = theta[ind[it], 0];
thetaPrefinal[it * k + zt, 1] = theta[ind[it], 1];
thetaPrefinal[it * k + zt, 2] = theta[ind[it], 2];
}
}
for (int it = 0; it < ind.Count; ++it)
{
Matrix4x4 r03 = FanucModel.qi(param[0]._alphaParam, thetaPrefinal[it * k, 0]) * FanucModel.qi(param[1]._alphaParam, thetaPrefinal[it * k, 1]) * FanucModel.qi(param[2]._alphaParam, thetaPrefinal[it * k, 2]);
Matrix4x4 r36 = r03.inverse * FanucModel.rotMatrix(coord[3] * Mathf.Deg2Rad, coord[4] * Mathf.Deg2Rad, coord[5] * Mathf.Deg2Rad);
float xi = r36[0, 2];
float y = r36[1, 2];
float z = r36[2, 2];
float tau1 = (xi * Mathf.Sin(param[3]._alphaParam) + Mathf.Sqrt((xi * xi + y * y) * Mathf.Sin(param[3]._alphaParam) * Mathf.Sin(param[3]._alphaParam)
- (Mathf.Cos(param[4]._alphaParam) - z * Mathf.Cos(param[3]._alphaParam)) * (Mathf.Cos(param[4]._alphaParam) - z * Mathf.Cos(param[3]._alphaParam))))
/ (Mathf.Cos(param[4]._alphaParam) - z * Mathf.Cos(param[3]._alphaParam) - y * Mathf.Sin(param[3]._alphaParam));
float tau2 = (xi * Mathf.Sin(param[3]._alphaParam) - Mathf.Sqrt((xi * xi + y * y) * Mathf.Sin(param[3]._alphaParam) * Mathf.Sin(param[3]._alphaParam)
- (Mathf.Cos(param[4]._alphaParam) - z * Mathf.Cos(param[3]._alphaParam)) * (Mathf.Cos(param[4]._alphaParam) - z * Mathf.Cos(param[3]._alphaParam))))
/ (Mathf.Cos(param[4]._alphaParam) - z * Mathf.Cos(param[3]._alphaParam) - y * Mathf.Sin(param[3]._alphaParam));
thetaPrefinal[it * k, 3] = 2.0f * Mathf.Atan(tau1);
thetaPrefinal[it * k + 1, 3] = 2.0f * Mathf.Atan(tau2);
float s51 = ((Mathf.Sin(param[5]._alphaParam) * r36[0, 1] + Mathf.Cos(param[5]._alphaParam) * r36[0, 2]) * Mathf.Cos(thetaPrefinal[it * k, 3])
+ (Mathf.Sin(param[5]._alphaParam) * r36[1, 1] + Mathf.Cos(param[5]._alphaParam) * r36[1, 2]) * Mathf.Sin(thetaPrefinal[it * k, 3])) / Mathf.Sin(param[4]._alphaParam);
float s52 = ((Mathf.Sin(param[5]._alphaParam) * r36[0, 1] + Mathf.Cos(param[5]._alphaParam) * r36[0, 2]) * Mathf.Cos(thetaPrefinal[it * k + 1, 3])
+ (Mathf.Sin(param[5]._alphaParam) * r36[1, 1] + Mathf.Cos(param[5]._alphaParam) * r36[1, 2]) * Mathf.Sin(thetaPrefinal[it * k + 1, 3])) / Mathf.Sin(param[4]._alphaParam);
float c51 = (-Mathf.Cos(param[3]._alphaParam) * (Mathf.Sin(param[5]._alphaParam) * r36[0, 1] + Mathf.Cos(param[5]._alphaParam) * r36[0, 2]) * Mathf.Sin(thetaPrefinal[it * k, 3] = 2.0f * Mathf.Atan(tau1))
+ Mathf.Cos(param[3]._alphaParam) * (Mathf.Sin(param[5]._alphaParam) * r36[1, 1] + Mathf.Cos(param[5]._alphaParam) * r36[1, 2]) * Mathf.Cos(thetaPrefinal[it * k, 3] = 2.0f * Mathf.Atan(tau1))
+ Mathf.Sin(param[3]._alphaParam) * (Mathf.Sin(param[5]._alphaParam) * r36[2, 1] + Mathf.Cos(param[5]._alphaParam) * r36[2, 2])) / (-Mathf.Sin(param[4]._alphaParam));
float c52 = (-Mathf.Cos(param[3]._alphaParam) * (Mathf.Sin(param[5]._alphaParam) * r36[0, 1] + Mathf.Cos(param[5]._alphaParam) * r36[0, 2]) * Mathf.Sin(thetaPrefinal[it * k + 1, 3])
+ Mathf.Cos(param[3]._alphaParam) * (Mathf.Sin(param[5]._alphaParam) * r36[1, 1] + Mathf.Cos(param[5]._alphaParam) * r36[1, 2]) * Mathf.Cos(thetaPrefinal[it * k + 1, 3])
+ Mathf.Sin(param[3]._alphaParam) * (Mathf.Sin(param[5]._alphaParam) * r36[2, 1] + Mathf.Cos(param[5]._alphaParam) * r36[2, 2])) / (-Mathf.Sin(param[4]._alphaParam));
thetaPrefinal[it * k, 4] = (s51 >= 0 ? Mathf.Acos(c51) : -Mathf.Acos(c51));
thetaPrefinal[it * k + 1, 4] = (s52 >= 0 ? Mathf.Acos(c52) : -Mathf.Acos(c52));
float c61 = (r36[0, 0] * Mathf.Cos(thetaPrefinal[it * k, 3]) + r36[1, 0] * Mathf.Sin(thetaPrefinal[it * k, 3])) * Mathf.Cos(thetaPrefinal[it * k, 4])
+ (-Mathf.Cos(param[3]._alphaParam) * (r36[0, 0] * Mathf.Sin(thetaPrefinal[it * k, 3]) - r36[1, 0] * Mathf.Cos(thetaPrefinal[it * k, 3])) + Mathf.Sin(param[3]._alphaParam) * r36[2, 0])
* Mathf.Sin(thetaPrefinal[it * k, 4]);
float c62 = (r36[0, 0] * Mathf.Cos(thetaPrefinal[it * k + 1, 3]) + r36[1, 0] * Mathf.Sin(thetaPrefinal[it * k + 1, 3])) * Mathf.Cos(thetaPrefinal[it * k + 1, 4])
+ (-Mathf.Cos(param[3]._alphaParam) * (r36[0, 0] * Mathf.Sin(thetaPrefinal[it * k + 1, 3]) - r36[1, 0] * Mathf.Cos(thetaPrefinal[it * k + 1, 3])) + Mathf.Sin(param[3]._alphaParam) * r36[2, 0])
* Mathf.Sin(thetaPrefinal[it * k + 1, 4]);
float s61 = -Mathf.Cos(param[4]._alphaParam) * (r36[0, 0] * Mathf.Cos(thetaPrefinal[it * k, 3]) + r36[1, 0] * Mathf.Sin(thetaPrefinal[it * k, 3])) * Mathf.Sin(thetaPrefinal[it * k, 4])
+ Mathf.Cos(param[4]._alphaParam) * (-Mathf.Cos(param[3]._alphaParam) * (r36[0, 0] * Mathf.Sin(thetaPrefinal[it * k, 3]) - r36[1, 0] * Mathf.Cos(thetaPrefinal[it * k, 3])) + Mathf.Sin(param[3]._alphaParam) * r36[2, 0])
* Mathf.Cos(thetaPrefinal[it * k, 4]) + (Mathf.Sin(param[3]._alphaParam) * (r36[0, 0] * Mathf.Sin(thetaPrefinal[it * k, 3]) - r36[1, 0] * Mathf.Cos(thetaPrefinal[it * k, 3])) + Mathf.Cos(param[3]._alphaParam) * r36[2, 0]) * Mathf.Sin(param[4]._alphaParam);
float s62 = -Mathf.Cos(param[4]._alphaParam) * (r36[0, 0] * Mathf.Cos(thetaPrefinal[it * k + 1, 3]) + r36[1, 0] * Mathf.Sin(thetaPrefinal[it * k + 1, 3])) * Mathf.Sin(thetaPrefinal[it * k + 1, 4])
+ Mathf.Cos(param[4]._alphaParam) * (-Mathf.Cos(param[3]._alphaParam) * (r36[0, 0] * Mathf.Sin(thetaPrefinal[it * k + 1, 3]) - r36[1, 0] * Mathf.Cos(thetaPrefinal[it * k + 1, 3])) + Mathf.Sin(param[3]._alphaParam) * r36[2, 0])
* Mathf.Cos(thetaPrefinal[it * k + 1, 4]) + (Mathf.Sin(param[3]._alphaParam) * (r36[0, 0] * Mathf.Sin(thetaPrefinal[it * k + 1, 3]) - r36[1, 0] * Mathf.Cos(thetaPrefinal[it * k + 1, 3])) + Mathf.Cos(param[3]._alphaParam) * r36[2, 0]) * Mathf.Sin(param[4]._alphaParam);
thetaPrefinal[it * k, 5] = Mathf.Atan2(s61, c61);
thetaPrefinal[it * k + 1, 5] = Mathf.Atan2(s62, c62);
}
List <int> indFinal = new List <int>();
for (int it = 0; it < ind.Count * k; ++it)
{
bool isOk = true;
float[] q = { thetaPrefinal[it, 0], thetaPrefinal[it, 1], thetaPrefinal[it, 2], thetaPrefinal[it, 3], thetaPrefinal[it, 4], thetaPrefinal[it, 5] };
var joints = JointsToQReverse(ref q);
for (int jt = 0; jt < 6; ++jt)
{
thetaPrefinal[it, jt] = joints[jt] * Mathf.Rad2Deg;
}
for (int jt = 3; jt < 6; ++jt)
{
if (thetaPrefinal[it, jt] > limMax[jt] || thetaPrefinal[it, jt] < limMin[jt])
{
isOk = false;
}
}
if (isOk && !float.IsNaN(thetaPrefinal[it, 5]))
{
indFinal.Add(it);
}
}
if (indFinal.Count == 0)
{
Debug.Log("No solution");
return(new float[0, 0]);
}
float[,] thetaFinal = new float[indFinal.Count, 6];
for (int it = 0; it < indFinal.Count; ++it)
{
for (int jt = 0; jt < 6; ++jt)
{
thetaFinal[it, jt] = thetaPrefinal[indFinal[it], jt];
}
}
return(thetaFinal);
}
