public void DotProductTest()
{
SillyParser p = new SillyParser(null);
Matrix A_0_1 = p.InterpretMatrix(
"cos(t1), -sin(t1), 0, 0;" +
"sin(t1), cos(t1), 0, 0;" +
"0, 0, 1, d1;" +
"0, 0, 0, 1");
Matrix A_1_2 = p.InterpretMatrix(
"0, 0, 1, 0;" +
"-1, 0, 0, 0;" +
"0, -1, 0, d2;" +
"0, 0, 0, 1");
Matrix A_2_3 = p.InterpretMatrix(
"0, -1, 0, 0;" +
"1, 0, 0, 0;" +
"0, 0, 1, d3;" +
"0, 0, 0, 1");
Matrix A_1_3 = p.InterpretMatrix(
"0, 0, 1, d3;" +
"0, 1, 0, 0;" +
"-1, 0, 0, d2;" +
"0, 0, 0, 1");
Matrix A_0_3 = p.InterpretMatrix(
"0, -sin(t1), cos(t1), d3 * cos(t1);" +
"0, cos(t1), sin(t1), d3 * sin(t1);" +
"-1, 0, 0, d1 + d2;" +
"0, 0, 0, 1");
Assert.AreEqual(A_1_3.PrettyPrint(), A_1_2.DotProduct(A_2_3).Simplify().PrettyPrint());
Matrix dot2 = A_0_1.DotProduct(A_1_3).Simplify();
Assert.AreEqual(A_0_3, dot2, $"Expected\n{A_0_3.PrettyPrint()}\nbut was\n{dot2.PrettyPrint()}");
}
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 void interpretMatrix()
{
double pi = Math.PI;
double thpi = 1.5 * pi; // three halves pi
double ohpi = 0.5 * pi; // one halves pi
SillyParser p = new SillyParser(null);
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(ohpi), 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);
}
public void interpretMatrixTest()
{
SillyParser p = new SillyParser(null);
Node[,] values = new Node[2, 2];
values[0, 0] = p.m(1);
values[0, 1] = p.m(2);
values[1, 0] = p.m(3);
values[1, 1] = p.m(4);
Matrix expected = new Matrix(values);
Assert.AreEqual(expected, p.InterpretMatrix("1, 2; 3, 4"));
}
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()}");
}