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