/// <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(); } }