Пример #1
0
        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));
        }
Пример #2
0
        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));
        }