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 void Eval_NamedVariable_ThrowsException()
{
SillyParser p = new SillyParser(null);
Leaf n = (Leaf)p.m("a");
n.Eval();
}
public void Unparse_NegatedNamedVariable_ReturnsNegatedName()
{
SillyParser p = new SillyParser(null);
Leaf n = (Leaf)p.m("a").Negate();
Assert.AreEqual("-a", n.Unparse());
}
public void Simplify_NegateLiteral_ReturnsNegativeValue()
{
SillyParser p = new SillyParser(null);
Leaf n = (Leaf)p.m(-2).Simplify();
Assert.AreEqual(-2, n.Eval());
}
public void Simplify_NamedVariable_ReturnsName()
{
SillyParser p = new SillyParser(null);
Leaf n = (Leaf)p.m("a").Simplify();
Assert.AreEqual("a", n.Unparse());
}
public void Eval_PositiveLiteral_success()
{
SillyParser p = new SillyParser(null);
Node n = p.m(2);
Assert.AreEqual(2, n.Eval());
}
public void interpretNodeTest_multiplyTwoDoubles()
{
SillyParser p = new SillyParser(null);
Node actual = p.InterpretNode("2*-1");
Assert.AreEqual(p.m("*", 2d, -1d), actual);
}
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 void interpretNodeTest_addNegOne()
{
SillyParser p = new SillyParser(null);
Node actual = p.InterpretNode("2+-1");
Assert.AreEqual(p.m("+", 2d, -1d), actual);
}
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 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 Simplify_NamedVariableWithVal_ReturnsVal()
{
SillyParser p = new SillyParser(null);
Leaf n = (Leaf)p.m("a");
p.SetVar("a", 2);
Assert.AreEqual(2, n.Eval());
}
public void Eval_NegateLiteral_ReturnsNegativeValue()
{
SillyParser p = new SillyParser(null);
Leaf n = (Leaf)p.m(-2);
Assert.AreEqual(-2, n.Eval());
Assert.AreEqual(2, n.Value);
Assert.IsTrue(n.IsNegative);
}
public void interpretNodeTest_simple()
{
SillyParser p = new SillyParser(null);
Assert.AreEqual(p.m(1d), p.InterpretNode("1"));
Assert.AreEqual(p.m("a"), p.InterpretNode("a"));
Assert.AreEqual(p.m("a"), p.InterpretNode("(a)"));
Assert.AreEqual(p.m("sin", 1d), p.InterpretNode("sin(1)"));
}
public void SetValueTwice_NamedVariable_ReturnsSecondVal()
{
SillyParser p = new SillyParser(null);
Leaf n = (Leaf)p.m("a");
p.SetVar("a", 2);
Assert.AreEqual(2, n.Eval());
p.SetVar("a", 3);
Assert.AreEqual(3, n.Eval());
}
public void interpretListOfNodesTest()
{
SillyParser p = new SillyParser(null);
Assert.AreEqual(p.m("atan2", 1d, 2d), p.InterpretNode("atan2(1, 2)"));
Node inner1 = p.m("atan2", 1d, 2d);
Node inner2 = p.m("atan2", 3d, 4d);
Assert.AreEqual(inner1, p.InterpretNode("atan2(1, 2)"));
Assert.AreEqual(inner2, p.InterpretNode("atan2(3, 4)"));
Assert.AreEqual(p.m("atan2", inner1, inner2), p.InterpretNode("atan2(atan2(1, 2), atan2(3, 4))"));
}
public void interpretNodeTest_multiplyTwoExpressions()
{
SillyParser p = new SillyParser(null);
Node mul = p.m("*", p.m(2d), p.m(1d));
Node atan2 = p.m("atan2", p.m(3d), p.m(4d));
Node add = p.m("+", mul, p.m(5d));
Assert.AreEqual(mul, p.InterpretNode("*(2,1)"));
Assert.AreEqual(atan2, p.InterpretNode("atan2(3,4)"));
Assert.AreEqual(add, p.InterpretNode("5+atan2(3,4)"));
Assert.AreEqual(p.m("+", add, atan2), p.InterpretNode("*(2,1)+5+atan2(3,4)"));
}
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 Simplify_NegatedSubtraction_NegativeMovedInside()
{
SillyParser p = new SillyParser(null);
Node n = p.m("-", p.m("a"), p.m("b"));
Assert.AreEqual("(a - b)", n.Unparse());
n = n.Negate();
Assert.AreEqual("-(a - b)", n.Unparse());
Node nSimp = n.Simplify();
Assert.AreEqual("(b - a)", nSimp.Unparse());
}
public void pemdas()
{
SillyParser p = new SillyParser(null);
Node atan2 = p.m("atan2", 5d, 6d);
Node minParen = p.m("-", -1d, 2d);
Node sin = p.m("sin", 3d);
Node mid = p.m("/", p.m("*", sin, minParen), atan2);
Node firstHalf = p.m("+", 2d, mid);
Node end1 = p.m("+", firstHalf, 2d);
Node end2 = p.m("-", end1, p.m("*", 5d, 7d));
Node whole = p.m("+", end2, 3d);
Assert.AreEqual(whole, p.InterpretNode("2+sin(3)*(-1-2)/atan2(5,6)+2-5*7+3"));
}
public void Simplify_NegatedExpression_NegativeMovedInside()
{
SillyParser p = new SillyParser(null);
Node n = p.m("*", p.m("a"), p.m("b"));
Assert.AreEqual("(a * b)", n.Unparse());
n = n.Negate();
Assert.AreEqual("-(a * b)", n.Unparse());
Node nSimp = n.Simplify();
Assert.AreEqual("(-a * -b)", nSimp.Unparse());
}
public void Simplify_NegativeNegations_success()
{
SillyParser p = new SillyParser(null);
Unary node = (Unary)p.m("-", p.m("a"));
Unary neg = (Unary)node.Negate();
Leaf negSimp = (Leaf)neg.Simplify();
Assert.AreEqual("--a", neg.Unparse());
Assert.AreEqual("a", negSimp.Unparse());
p.SetVar("a", 2);
Assert.AreEqual(-2, node.Eval());
Assert.AreEqual(2, neg.Eval());
Assert.AreEqual(2, negSimp.Eval());
}
/// <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()}");
}
