/// <summary>
/// generate the endeffector position vector by using conventional dh parameters
/// </summary>
/// <returns>The 4x4 Matrix of position.</returns>
public Matrix4x4 DHForwardKinematics()
{
Matrix4x4 T = Matrix4x4.identity;
for (int i = 0; i < dhkinematics.ConventionalDHtable.Count; i++)
{
//TransformationMatrix( rz, rx, tx, tz)
T *= KinematicsMatrix.TransformationMatrix(dhkinematics.ConventionalDHtable[i][0], dhkinematics.ConventionalDHtable[i][2],
dhkinematics.ConventionalDHtable[i][3], dhkinematics.ConventionalDHtable[i][1]).transpose;
}
Matrix4x4 newT = dhkinematics.root.localToWorldMatrix * T;
return(newT);
}
/// <summary>
/// eliminate configurations that might break the physical robot
/// </summary>
public void SetAvoidList()
{
underGround = new int[counter];
for (int i = 0; i < counter; i++)
{
dhkinematics.SetFullDHtable(i);
Matrix4x4 CheckT = Matrix4x4.identity;
for (int j = 0; j < dhkinematics.ConventionalDHtable.Count; j++)
{
CheckT *= KinematicsMatrix.TransformationMatrix(dhkinematics.ConventionalDHtable[j][0], dhkinematics.ConventionalDHtable[j][2],
dhkinematics.ConventionalDHtable[j][3], dhkinematics.ConventionalDHtable[j][1]).transpose;
//Debug.Log("each frame position"+(dhkinematics.root.localToWorldMatrix ).ExtractPosition().y);
if ((dhkinematics.root.localToWorldMatrix * CheckT).ExtractPosition().y < -edgeLength / 2 - 0.01f)
{
//Debug.Log("take me off");
underGround[i] = 1;
break;
}
}
dhkinematics.ConventionalDHtable.Clear();
}
}