Merge branch 'develop'

makefile

@@ -1,8 +1,13 @@
-all: kinematics dynamics joint   
+all: kinematics dynamics joint   test
 	g++ -o bin/jointControl lib/jointControl.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
+	g++ -o bin/testKinematics lib/testKinematics.o lib/Kinematics.o `pkg-config --libs opencv4`
+		
 		
 kinematics: src/Kinematics.cpp
 	g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4
+	
+test: src/testKinematics.cpp
+	g++ -c src/testKinematics.cpp -o lib/testKinematics.o -I./include -I/usr/include/opencv4
 
 joint: src/jointControl.cpp
 	g++ -c src/jointControl.cpp -o lib/jointControl.o -I./include -I/usr/include/opencv4

src/testKinematics.cpp

@@ -0,0 +1,21 @@
+#include "Kinematics.h"
+#include <iostream>
+
+int main()
+{       float threshold = 0.00001;
+        float q1 = deg2rad(90.0);
+        float q2 = deg2rad(0.0);
+        float l1 = 5.0;
+        float l2 = 6.0;
+        std::vector<float> expectedOutput {0.0, 11.0};
+
+        std::vector<float> computedOutput = computeForwardKinematics(q1, q2, l1, l2);
+                float error = 0.0;
+                for (int l=0; l<expectedOutput.size(); l++)
+                {
+                        error += abs(computedOutput[l]-expectedOutput[l]);
+                }
+                if (error < threshold)
+                        std::cout<<"test passed"<<std::endl;
+        return 0;
+}