public void DHTableMatrixTest()
{
double pi = Math.PI;
double thpi = 1.5 * pi; // three halves pi
double zhpi = 0.5 * pi; // zero halves pi
SillyParser p = (SillyParser)SillyParser.GetInstance();
Matrix matrix = p.InterpretMatrix(
$"0, 0, d1, th1;" +
$"0, 1.5*{pi}, d2, 1.5*{pi};" +
$"0, 0, d3, 0.5*{pi}");
matrix = matrix.Simplify();
Assert.AreEqual(p.m(0), matrix[0, 0]);
Assert.AreEqual(p.m(0), matrix[0, 1]);
Assert.AreEqual(p.m("d1"), matrix[0, 2]);
Assert.AreEqual(p.m("th1"), matrix[0, 3]);
Assert.AreEqual(p.m(0), matrix[1, 0]);
Assert.AreEqual(p.m(thpi), matrix[1, 1]);
Assert.AreEqual(p.m("d2"), matrix[1, 2]);
Assert.AreEqual(p.m(thpi), matrix[1, 3]);
Assert.AreEqual(p.m(0), matrix[2, 0]);
Assert.AreEqual(p.m(0), matrix[2, 1]);
Assert.AreEqual(p.m("d3"), matrix[2, 2]);
Assert.AreEqual(p.m(zhpi), matrix[2, 3]);
}
public static void Main(string[] args)
{
string pi = Math.PI.ToString("F9");
SillyParser p = (SillyParser)SillyParser.GetInstance();
// build dh table
Matrix m = p.InterpretMatrix(
$"3, -{pi}/2, 0, t1;" +
$"0, {pi}/2, d2, 0;" +
$"2, 0, 0, t3");
DenavitHartenbergTable dhTable = new DenavitHartenbergTable(m);
Debug.WriteLine("DH Table = \n" + m.PrettyPrint().Replace("\\n", "\\n\\t"));
// get homogenous transforms
HomogeneousTransformation[] As = dhTable.IntermediateHomogeneousTransformations();
As[0] = As[0].Simplify();
As[1] = As[1].Simplify();
As[2] = As[2].Simplify();
Debug.WriteLine("A4.5 = \n" + ((Matrix)As[0]).PrettyPrint().Replace("\\n", "\\n\\t"));
Debug.WriteLine("A5.6 = \n" + ((Matrix)As[1]).PrettyPrint().Replace("\\n", "\\n\\t"));
Debug.WriteLine("A6.7 = \n" + ((Matrix)As[2]).PrettyPrint().Replace("\\n", "\\n\\t"));
// find composite homogenous transform
Matrix A4_6 = (As[0] * As[1]).Simplify();
Matrix A4_7 = (A4_6 * As[2]).Simplify();
Debug.WriteLine("A4.6 = \n" + A4_6.PrettyPrint().Replace("\\n", "\\n\\t"));
Debug.WriteLine("A4.7 = \n" + A4_7.PrettyPrint().Replace("\\n", "\\n\\t"));
Debug.WriteLine("done");
}
public HomogeneousTransformation[] IntermediateHomogeneousTransformations()
{
HomogeneousTransformation[] retval = new HomogeneousTransformation[Rows];
SillyParser p = (SillyParser)SillyParser.GetInstance();
Matrix[] matrices = new Matrix[Rows];
for (int i = 0; i < Rows; i++)
{
string matrixStr =
$"cos(_th{i}), -sin(_th{i}) * cos(_al{i}), sin(_th{i}) * sin(_al{i}), _a{i} * cos(_th{i});" +
$"sin(_th{i}), cos(_th{i}) * cos(_al{i}), -cos(_th{i}) * sin(_al{i}), _a{i} * sin(_th{i});" +
$"0, sin(_al{i}), cos(_al{i}), _d{i};" +
$"0, 0, 0, 1";
matrixStr = matrixStr.Replace($"_a{i}", this[i, 0].ToString());
matrixStr = matrixStr.Replace($"_al{i}", this[i, 1].ToString());
matrixStr = matrixStr.Replace($"_d{i}", this[i, 2].ToString());
matrixStr = matrixStr.Replace($"_th{i}", this[i, 3].ToString());
matrices[i] = p.InterpretMatrix(matrixStr);
}
for (int i = 0; i < Rows; i++)
{
Frame baseFrame = FrameRegistry.GetFrame($"{i}");
Frame toFrame = FrameRegistry.GetFrame($"{i + 1}");
retval[i] = new HomogeneousTransformation(matrices[i], baseFrame, toFrame);
}
return(retval);
}
/// <summary>
/// Copies the given values into a new 2D expression array.
/// Useful for constructing new
/// </summary>
/// <param name="vals">The </param>
/// <returns></returns>
public static Node[,] genNodes(double[,] vals)
{
Parser p = SillyParser.GetInstance();
Node[,] retval = new Node[vals.GetLength(0), vals.GetLength(1)];
for (int row = 0; row < vals.GetLength(0); row++)
{
for (int col = 0; col < vals.GetLength(1); col++)
{
retval[row, col] = p.m(vals[row, col]);
}
}
return(retval);
}
private static Matrix GenerateHomogenousMatrix(RotationMatrix rotation, TranslationVector translation)
{
Node[,] values = new Node[4, 4];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
values[i, j] = rotation.Values[i, j];
}
}
values[0, 3] = translation.X;
values[1, 3] = translation.Y;
values[2, 3] = translation.Z;
values[3, 0] = SillyParser.GetInstance().m(0);
values[3, 1] = SillyParser.GetInstance().m(0);
values[3, 2] = SillyParser.GetInstance().m(0);
values[3, 3] = SillyParser.GetInstance().m(1);
return(new Matrix(values));
}
public void IntermediateHomogeneousTransformationsTest()
{
SillyParser p = (SillyParser)SillyParser.GetInstance();
DenavitHartenbergTable table = new DenavitHartenbergTable(p.InterpretMatrix(
$"0, 0, d1, th1;" +
$"0, 1.5*PI, d2, 1.5*PI;" +
$"0, 0, d3, 0.5*PI"));
Matrix A_0_1 = p.InterpretMatrix(
"cos(th1), -sin(th1), 0, 0;" +
"sin(th1), cos(th1), 0, 0;" +
"0, 0, 1, d1;" +
"0, 0, 0, 1").Simplify();
Matrix A_1_2 = p.InterpretMatrix(
"0, 0, 1, 0;" +
"-1, 0, 0, 0;" +
"0, -1, 0, d2;" +
"0, 0, 0, 1").Simplify();
Matrix A_2_3 = p.InterpretMatrix(
"0, -1, 0, 0;" +
"1, 0, 0, 0;" +
"0, 0, 1, d3;" +
"0, 0, 0, 1").Simplify();
p.SetVar("PI", Math.PI);
HomogeneousTransformation[] HTs = table.IntermediateHomogeneousTransformations();
Matrix[] HTMs = new Matrix[HTs.Length];
for (int i = 0; i < HTs.Length; i++)
{
HTMs[i] = new Matrix(HTs[i].SubMatrix(0, 4, 0, 4));
HTMs[i] = HTMs[i].Simplify();
}
Assert.AreEqual(A_0_1, HTMs[0], $"Expected \n{A_0_1.PrettyPrint()}\nbut was\n{HTMs[0].PrettyPrint()}");
Assert.AreEqual(A_1_2, HTMs[1], $"Expected \n{A_1_2.PrettyPrint()}\nbut was\n{HTMs[1].PrettyPrint()}");
Assert.AreEqual(A_2_3, HTMs[2], $"Expected \n{A_2_3.PrettyPrint()}\nbut was\n{HTMs[2].PrettyPrint()}");
}
