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));
}
public Robot(string name, RobotManufacturer robotBrand, RobotModels robotModel)
{
ModelName = name;
Joints = new SortedList <int, Joint>();
var dhParams = DHParameterFactory.GetDhParameterForRobot(robotModel);
for (int i = 0; i < dhParams.Count; i++)
{
Joints.Add(i, new RotationalJoint(0, 360, dhParams[i]));
}
Kinematic = new RobotKinematics(dhParams);
InitializeMember();
Joint6ToFlangeTrafo = RobotBaseDataProvider.GetJoint6ToFlangeTransformation(robotBrand);
}
