From 7d1255736f8010ce43ab5ae58b6818c32681fc2d Mon Sep 17 00:00:00 2001
From: Mattis <mattis.roellinger@ecam.fr>
Date: Wed, 6 Mar 2024 15:25:24 +0100
Subject: [PATCH] Adding FK tests

---
 makefile               |  7 +++++--
 src/testKinematics.cpp | 38 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 43 insertions(+), 2 deletions(-)
 create mode 100644 src/testKinematics.cpp

diff --git a/makefile b/makefile
index f7500c4..46b3cf3 100644
--- a/makefile
+++ b/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
@@ -12,7 +13,9 @@ 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/*
\ No newline at end of file
diff --git a/src/testKinematics.cpp b/src/testKinematics.cpp
new file mode 100644
index 0000000..3b2f81a
--- /dev/null
+++ b/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;
+	
+}