public void TestForwardKinematicWith2Joints()
{
//Arrange
var dhParams = new List <DHParameter>
{
new DHParameter(Math.PI, 0, 0, 500),
new DHParameter(Math.PI / 2, 500, Math.PI / 2, 0)
};
//Act
var kinematic = new RobotKinematics(dhParams);
var forwardMatrix = kinematic.GetForwardTransformationMatrix();
//Assert
var xAxis = forwardMatrix.Column(0);
var yAxis = forwardMatrix.Column(1);
var zAxis = forwardMatrix.Column(2);
var offset = forwardMatrix.Column(3);
Assert.IsTrue(xAxis[0].DoubleEquals(0));
Assert.IsTrue(xAxis[1].DoubleEquals(-1));
Assert.IsTrue(xAxis[2].DoubleEquals(0));
Assert.IsTrue(yAxis[0].DoubleEquals(0));
Assert.IsTrue(yAxis[1].DoubleEquals(0));
Assert.IsTrue(yAxis[2].DoubleEquals(-1));
Assert.IsTrue(zAxis[0].DoubleEquals(1));
Assert.IsTrue(zAxis[1].DoubleEquals(0));
Assert.IsTrue(zAxis[2].DoubleEquals(0));
Assert.IsTrue(offset[0].DoubleEquals(0));
Assert.IsTrue(offset[1].DoubleEquals(-500));
Assert.IsTrue(offset[2].DoubleEquals(500));
}
public void TestForwardKinematicWith1Joint()
{
//Arrange
var dhParams = new List <DHParameter>
{
new DHParameter(Math.PI, 0, 0, 500)
};
//Act
var kinematic = new RobotKinematics(dhParams);
var forwardMatrix = kinematic.GetForwardTransformationMatrix();
//Assert
//By rotating 180° around the x-Axis (Alpha = Pi), the z and y axis should be inverted.
var xAxis = forwardMatrix.Column(0);
var yAxis = forwardMatrix.Column(1);
var zAxis = forwardMatrix.Column(2);
var offset = forwardMatrix.Column(3);
Assert.IsTrue(xAxis[0].DoubleEquals(1));
Assert.IsTrue(xAxis[1].DoubleEquals(0));
Assert.IsTrue(xAxis[2].DoubleEquals(0));
Assert.IsTrue(yAxis[0].DoubleEquals(0));
Assert.IsTrue(yAxis[1].DoubleEquals(-1));
Assert.IsTrue(yAxis[2].DoubleEquals(0));
Assert.IsTrue(zAxis[0].DoubleEquals(0));
Assert.IsTrue(zAxis[1].DoubleEquals(0));
Assert.IsTrue(zAxis[2].DoubleEquals(-1));
Assert.IsTrue(offset[0].DoubleEquals(0));
Assert.IsTrue(offset[1].DoubleEquals(0));
Assert.IsTrue(offset[2].DoubleEquals(500));
}