Adding FK tests

makefile

@@ -1,5 +1,6 @@
-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/Kinematics.o lib/testKinematics.o `pkg-config --libs opencv4`
 		
 kinematics: src/Kinematics.cpp
 	g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4
@@ -13,6 +14,8 @@ image: src/ImageProcessing.cpp
 dynamics: src/DynamixelHandler.cpp
 	g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include
 
+test: src/testKinematics.cpp
+	g++ -c src/testKinematics.cpp -o lib/testKinematics.o -I./include -I/usr/include/opencv4
 clean:
 	rm lib/*.o
 	rm bin/*

src/testKinematics.cpp

@@ -0,0 +1,38 @@
+#include "Kinematics.h"
+#include <iostream>
+int main (){
+	// 1st test
+	float threshold=0.00001;
+	float q1=deg2rad(0.0); //deg
+	float q2=deg2rad(0.0); //deg
+	float L1=5.0; //cm
+	float L2=6.0; //cm
+	std::vector<float> expectedOutput{11.0,0.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;
+		}
+	
+	// 2nd test
+	q1=deg2rad(90.0); //deg
+	q2=deg2rad(0.0); //deg
+	expectedOutput[0] = 0.0;
+	expectedOutput[1] = 	11.0;
+	
+	computedOutput=computeForwardKinematics(q1,q2,L1,L2);
+	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;
+	
+}