EAIK 0.0.1__tar.gz → 1.1.0__tar.gz

{eaik-0.0.1/external/EAIK → eaik-1.1.0/CPP}/external/ik-geo/cpp/subproblems/sp.cpp

@@ -1,8 +1,3 @@
-//---------------------------------------------------------------//
-// Name: sp_1.cpp
-// Author: Runbin Chen and Amar Maksumic
-// Purpose: Port of the subproblem files functionality
-//---------------------------------------------------------------//
 #include "sp.h"
 #include <math.h>
 #include <vector>
@@ -56,50 +51,16 @@ namespace IKS
 
 	std::vector<std::complex<double>> quartic_roots(const Eigen::Matrix<double, 1, 5> &poly)
 	{
+		Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
+		solver.compute(poly);
+		const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType &roots = solver.roots();
 
-		const std::complex<double> i = std::complex<double>(0, 0);
-
-		std::vector<std::complex<double>> roots;
-
-		double A = poly(0, 0), B = poly(0, 1), C = poly(0, 2), D = poly(0, 3), E = poly(0, 4);
-
-		std::complex<double> alpha = -0.375 * B * B / (A * A) + C / A;
-		std::complex<double> beta = 0.125 * B * B * B / (A * A * A) - 0.5 * B * C / (A * A) + D / A;
-		std::complex<double> gamma = -B * B * B * B * 3. / (A * A * A * A * 256.) + C * B * B / (A * A * A * 16.) - B * D / (A * A * 4.) + E / A;
-
-		if (fabs(beta.real()) < ZERO_THRESH && fabs(beta.imag()) < ZERO_THRESH)
-		{
-			std::complex<double> tmp = sqrt(alpha * alpha - gamma * 4. + i);
-			roots.push_back(-B / (A * 4.) + sqrt((-alpha + tmp) / 2. + i));
-			roots.push_back(-B / (A * 4.) - sqrt((-alpha + tmp) / 2. + i));
-			roots.push_back(-B / (A * 4.) + sqrt((-alpha - tmp) / 2. + i));
-			roots.push_back(-B / (A * 4.) - sqrt((-alpha - tmp) / 2. + i));
-			return roots;
+		std::vector<std::complex<double>> result(roots.size());
+		for (int i = 0; i < roots.size(); ++i) {
+			result[i] = roots[i];
 		}
 
-		std::complex<double> P = -alpha * alpha / 12. - gamma;
-		std::complex<double> Q = -alpha * alpha * alpha / 108. + alpha * gamma / 3. - beta * beta * 0.125;
-		std::complex<double> R = -Q * 0.5 + sqrt(Q * Q * 0.25 + P * P * P / 27. + i);
-		std::complex<double> U = pow(R, 1. / 3);
-
-		std::complex<double> y;
-		if (fabs(U.real()) < ZERO_THRESH && fabs(U.imag()) < ZERO_THRESH)
-		{
-			y = -alpha * 5. / 6. - pow(Q, 1. / 3);
-		}
-		else
-		{
-			y = -alpha * 5. / 6. + U - P / (3. * U);
-		}
-
-		std::complex<double> W = sqrt(alpha + 2. * y + i);
-
-		roots.push_back(-B / (A * 4.) + (W + sqrt(-(alpha * 3. + 2. * y + beta * 2. / W))) / 2.);
-		roots.push_back(-B / (A * 4.) + (W - sqrt(-(alpha * 3. + 2. * y + beta * 2. / W))) / 2.);
-		roots.push_back(-B / (A * 4.) - (W + sqrt(-(alpha * 3. + 2. * y - beta * 2. / W))) / 2.);
-		roots.push_back(-B / (A * 4.) - (W - sqrt(-(alpha * 3. + 2. * y - beta * 2. / W))) / 2.);
-
-		return roots;
+		return result;
 	}
 
 	std::vector<std::pair<double, double>> solve_2_ellipse_numeric(const Eigen::Vector2d &xm1, const Eigen::Matrix<double, 2, 2> &xn1,
@@ -110,7 +71,7 @@ namespace IKS
 		xm1'*xm1 + xi'*xn1'*xn1*xi  + xm1'*xn1*xi == 1
 		xm2'*xm2 + xi'*xn2'*xn2*xi  + xm2'*xn2*xi == 1
 		Where xi = [xi_1; xi_2] */
-		const double EPSILON = ZERO_THRESH;
+		const double EPSILON = 1e-5;
 		Eigen::Matrix<double, 2, 2> A_1 = xn1.transpose() * xn1;
 		double a = A_1.coeffRef(0, 0);
 		double b = 2 * A_1.coeffRef(1, 0);
@@ -148,7 +109,7 @@ namespace IKS
     double z4 = a*a*c1*c1 - 2.0*a*c1*a1*c + a1*a1*c*c - b*a*b1*c1 - b*b1*a1*c + b*b*a1*c1 +
         c*a*b1*b1;
 		Eigen::Matrix<double, 1, 5> z;
-		z << z4, z3, z2, z1, z0;
+		z << z0,z1, z2, z3, z4;
 		std::vector<std::complex<double>> roots = quartic_roots(z);
 
 		std::vector<std::pair<double, double>> xi;
@@ -237,10 +198,19 @@ namespace IKS
 		a.row(1) = -k.cross(kxp);
 
 		const Eigen::Vector2d x = a * p2;
-		theta = std::atan2(x.x(), x.y());
 
-		_solution_is_ls = std::fabs(p1.norm() - p2.norm()) > ZERO_THRESH ||
-						  std::fabs(k.dot(p1) - k.dot(p2)) > ZERO_THRESH;
+		if(x.norm() > ZERO_THRESH)
+		{
+			theta = std::atan2(x.x(), x.y());
+			_solution_is_ls = std::fabs(p1.norm() - p2.norm()) > ZERO_THRESH ||
+			std::fabs(k.dot(p1) - k.dot(p2)) > ZERO_THRESH;
+		}
+		else
+		{
+			theta = 0;
+			_solution_is_ls = true;
+		}
+
 
 		is_calculated = true;
 	}
@@ -401,12 +371,13 @@ namespace IKS
 	{
 		const Eigen::Vector3d kxp = k.cross(p1);
 		Eigen::Matrix<double, 3, 2> a_1;
-		if(kxp.isZero(ZERO_THRESH))
+		if(kxp.isZero(ZERO_THRESH) || k.cross(p2).isZero(ZERO_THRESH))
 		{
 			// Minimization is independent of theta -> probably redundant!
 			//std::cout<<"Warning! - minimization of subproblem 3 is indipendent of theta. This may indicate redundancy"<<std::endl;
 			is_calculated = true;
 			theta.push_back(0); // Default angle
+			_solution_is_ls = true;
 			return;
 		}
 
@@ -420,7 +391,7 @@ namespace IKS
 
 		const Eigen::Vector2d x_ls = a_1.transpose() * (-2.0 * p2 * b / norm_a_sq);
 
-		if (x_ls.squaredNorm() > 1.0)
+		if (x_ls.squaredNorm() > 1.0 - ZERO_THRESH || 1.0 - b * b / norm_a_sq < 0)
 		{
 			theta.push_back(std::atan2(x_ls.x(), x_ls.y()));
 			_solution_is_ls = true;
@@ -436,6 +407,7 @@ namespace IKS
 
 			theta.push_back(std::atan2(sc_1.x(), sc_1.y()));
 			theta.push_back(std::atan2(sc_2.x(), sc_2.y()));
+
 			_solution_is_ls = false;
 		}
 
@@ -494,25 +466,32 @@ namespace IKS
 		const Eigen::Vector2d a = h.transpose() * a_1;
 
 		const double b = d - (h.transpose() * k * k.transpose() * p).x();
-
 		const double norm_a_sq = a.squaredNorm();
 		const Eigen::Vector2d x_ls = a_1.transpose() * h * b;
 
-		if (norm_a_sq > b * b)
+		if (norm_a_sq - b * b > 0)
 		{
-			const double xi = std::sqrt((norm_a_sq - b * b));
+			const double xi = std::sqrt(norm_a_sq - b * b );
 			const Eigen::Vector2d a_perp_tilde(a.y(), -a.x());
 
 			const Eigen::Vector2d sc_1 = x_ls + xi * a_perp_tilde;
 			const Eigen::Vector2d sc_2 = x_ls - xi * a_perp_tilde;
 
 			theta.push_back(std::atan2(sc_1.x(), sc_1.y()));
-			theta.push_back(std::atan2(sc_2.x(), sc_2.y()));
+			theta.push_back(std::atan2(sc_2.x(), sc_2.y()));			
 			_solution_is_ls = false;
 		}
 		else
 		{
-			theta.push_back(std::atan2(x_ls.x(), x_ls.y()));
+			if (x_ls.norm() < ZERO_THRESH)
+			{
+				theta.push_back(0);
+			}
+			else
+			{
+				theta.push_back(std::atan2(x_ls.x(), x_ls.y()));
+			}
+
 			_solution_is_ls = true;
 		}
 
@@ -587,7 +566,7 @@ namespace IKS
 
 		const Eigen::Matrix<double, 1, 5> eqn_real = convolution_3(rhs, rhs) - 4.0 * convolution_3(p_13_sq, r_1);
 
-		const std::vector<std::complex<double>> all_roots = quartic_roots(eqn_real);
+		std::vector<std::complex<double>> all_roots = quartic_roots(eqn_real.row(0).reverse());
 
 		std::vector<double> h_vec;
 		for (const auto &root : all_roots)
@@ -598,6 +577,16 @@ namespace IKS
 			}
 		}
 
+		// No analytical solution found - use all roots and decide later for valid solutions
+		if (h_vec.size() == 0)
+		{
+			for (const auto &root : all_roots)
+			{
+				h_vec.push_back(root.real());
+			}
+			_solution_is_ls = true;
+		}
+
 		const Eigen::Vector3d kxp1 = k1.cross(p1);
 		const Eigen::Vector3d kxp3 = k3.cross(p3);
 
@@ -616,6 +605,10 @@ namespace IKS
 		j << 0, 1,
 			-1, 0;
 
+		// We save angle combinations w.r.t. their error in std::fabs((v1 - h * k2).norm() - (v3 - h * k2).norm())
+		// If no combination reaches our threshold we can then still output the best approximate solution
+		std::vector<std::tuple<double, double, double, double>> norm_errors_angles;
+
 		for (const auto &h : h_vec)
 		{
 			Eigen::Vector2d const_1 = a_1.transpose() * k2 * (h - delta_1);
@@ -653,19 +646,76 @@ namespace IKS
 				Eigen::Vector3d v1 = a_1 * sc_1 + p1_s;
 				Eigen::Vector3d v3 = a_3 * sc_3 + p3_s;
 
-				if (std::fabs((v1 - h * k2).norm() - (v3 - h * k2).norm()) < EPSILON)
+				const double norm_equation_error = std::fabs((v1 - h * k2).norm() - (v3 - h * k2).norm());
+				if (norm_equation_error < EPSILON)
 				{
 					SP1 sp(v3, v1, k2);
 					sp.solve();
+					_solution_is_ls |= sp.solution_is_ls();
+
+					if (sc_1.norm() > ZERO_THRESH)
+					{
+						theta_1.push_back(std::atan2(sc_1.x(), sc_1.y()));
+					}
+					else
+					{
+						theta_1.push_back(0);
+						_solution_is_ls = true;
+					}
+
+					if (sc_3.norm() > ZERO_THRESH)
+					{
+						theta_3.push_back(std::atan2(sc_3.x(), sc_3.y()));
+					}
+					else
+					{
+						theta_3.push_back(0);
+						_solution_is_ls = true;
+					}
 
-					theta_1.push_back(std::atan2(sc_1.x(), sc_1.y()));
 					theta_2.push_back(sp.get_theta());
-					theta_3.push_back(std::atan2(sc_3.x(), sc_3.y()));
+				}
+				// Only save approximate solutions if no analytical one is given yet
+				else if (theta_1.size()==0)
+				{
+					SP1 sp(v3, v1, k2);
+					sp.solve();
+
+					double approx_t1 = 0;
+					double approx_t2 = 0;
+					double approx_t3 = 0;
+
+					if (sc_1.norm() > ZERO_THRESH)
+					{
+						approx_t1 = std::atan2(sc_1.x(), sc_1.y());
+					}
+
+					if (sc_3.norm() > ZERO_THRESH)
+					{
+						approx_t3 = std::atan2(sc_3.x(), sc_3.y());
+					}
+
+					approx_t2 = sp.get_theta();
+
+					norm_errors_angles.push_back({norm_equation_error, approx_t1, approx_t2, approx_t3});
 				}
 			}
 		}
 
-		reduce_solutionset();
+		// Check if any valid angles were found - if not, use best approximate solution
+		if (theta_1.size() == 0)
+		{
+			// Sort by error
+			std::sort(norm_errors_angles.begin(), norm_errors_angles.end(), [](const std::tuple<double, double, double, double>& a, const std::tuple<double, double, double, double>& b) {
+				return std::get<0>(a) < std::get<0>(b);
+			});
+			theta_1.push_back(std::get<1>(norm_errors_angles.at(0)));
+			theta_2.push_back(std::get<2>(norm_errors_angles.at(0)));
+			theta_3.push_back(std::get<3>(norm_errors_angles.at(0)));
+			_solution_is_ls = true;
+		}
+
+		//reduce_solutionset();
 		is_calculated = true;
 	}
 
@@ -893,8 +943,17 @@ namespace IKS
 		for (const auto[xi_1, xi_2] : xi)
 		{
 			Eigen::Matrix<double, 4, 1> x = x_min + x_null_1 * xi_1 + x_null_2 * xi_2;
-			theta_1.push_back(atan2(x(0, 0), x(1, 0)));
-			theta_2.push_back(atan2(x(2, 0), x(3, 0)));
+
+			if(x.norm() < ZERO_THRESH)
+			{
+				theta_1.push_back(0);
+				theta_2.push_back(0);
+			}
+			else
+			{
+				theta_1.push_back(atan2(x(0, 0), x(1, 0)));
+				theta_2.push_back(atan2(x(2, 0), x(3, 0)));
+			}
 		}
 
 		is_calculated = true;

{eaik-0.0.1/external/EAIK → eaik-1.1.0/CPP}/external/ik-geo/cpp/subproblems/sp.h

@@ -8,7 +8,7 @@ namespace IKS
 {
 
 #ifndef ZERO_THRESH
-#define ZERO_THRESH 1e-8
+#define ZERO_THRESH 1e-12
 #endif
 
   std::pair<Eigen::Vector2d, Eigen::Vector3d> cone_polynomials(const Eigen::Vector3d &p0_i, const Eigen::Vector3d &k_i, const Eigen::Vector3d &p_i, const Eigen::Vector3d &p_i_s, const Eigen::Vector3d &k2);

{eaik-0.0.1/external/EAIK → eaik-1.1.0/CPP}/src/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.9)
 project(EAIK_Cpp)
 
-add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../src/IK build_IK)
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/IK build_IK)
 
 if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
@@ -9,8 +9,8 @@ endif()
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_FLAGS_RELEASE "-O3")
 
-set(sources EAIK.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/kinematic_remodelling.cpp)
+set(sources EAIK.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/kinematic_remodeling.cpp ${CMAKE_CURRENT_SOURCE_DIR}/IK/utils/kinematic_utils.cpp)
 
 add_library(EAIK ${sources})
-target_include_directories(EAIK PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/utils ${CMAKE_CURRENT_SOURCE_DIR}/IK)
+target_include_directories(EAIK PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/utils ${CMAKE_CURRENT_SOURCE_DIR}/IK ${CMAKE_CURRENT_SOURCE_DIR}/IK/utils)
 target_link_libraries(EAIK EAIK_Kinematics)

eaik-1.1.0/CPP/src/EAIK.cpp

@@ -0,0 +1,258 @@
+#include <iostream>
+#include <eigen3/Eigen/Dense>
+#include <thread>
+#include <mutex>
+
+#include "kinematic_remodeling.h"
+#include "kinematic_utils.h"
+#include "EAIK.h"
+
+namespace EAIK
+{
+    Robot::Robot(const Eigen::VectorXd& dh_alpha, const Eigen::VectorXd& dh_a, const Eigen::VectorXd& dh_d, const Eigen::Matrix<double, 3, 3> &R6T, const std::vector<std::pair<int, double>>& fixed_axes, bool is_double_precision)
+    {
+        const auto&[H, P, R6T_dh] = IKS::dh_to_H_P(dh_alpha, dh_a, dh_d);
+        this->R6T = R6T*R6T_dh;
+        init(H, P, fixed_axes, is_double_precision);
+    }
+
+    Robot::Robot(const Eigen::MatrixXd &H, const Eigen::MatrixXd &P, const Eigen::Matrix<double, 3, 3> &R6T, const std::vector<std::pair<int, double>>& fixed_axes, bool is_double_precision) : R6T(R6T)
+    {
+        init(H, P, fixed_axes, is_double_precision);
+    }
+
+    void Robot::init(const Eigen::MatrixXd &H, const Eigen::MatrixXd &P, const std::vector<std::pair<int, double>>& fixed_axes, bool is_double_precision)
+    {
+        if(is_double_precision)
+        {
+            ZERO_THRESHOLD = 1e-13;
+            AXIS_INTERSECT_THRESHOLD = 1e-9;
+        }
+
+        if(P.cols() != H.cols() + 1)
+        {
+            throw std::runtime_error("Wrong input dimensions for H and P. Note that #P = #H+1.");
+        }
+
+        Eigen::MatrixXd P_remodeled;
+        Eigen::MatrixXd H_remodeled;
+
+        // Insert fixed axes (e.g., to cope with >6DOF manipulators) into the kinematic chain
+        if(fixed_axes.size()>0)
+        {
+            this->fixed_axes = fixed_axes;
+            std::sort(this->fixed_axes.begin(), this->fixed_axes.end(), [](const std::pair<int, double>& a, const std::pair<int, double>& b) {return a.first < b.first;});
+
+            const auto&[H_part, P_part, R6T_part] = partial_joint_parametrization(H, P, fixed_axes, R6T);
+            P_remodeled = remodel_kinematics(H_part, P_part, ZERO_THRESHOLD, AXIS_INTERSECT_THRESHOLD);
+            H_remodeled = H_part;
+            this->R6T_partial = R6T_part;
+        }
+        else
+        {
+            P_remodeled = remodel_kinematics(H, P, ZERO_THRESHOLD, AXIS_INTERSECT_THRESHOLD);
+            H_remodeled = H;
+            this->R6T_partial = R6T;
+        }
+        switch (H_remodeled.cols())
+        {
+        case 6:
+            bot_kinematics = std::make_unique<IKS::General_6R>(H_remodeled, P_remodeled);
+            original_kinematics = std::make_unique<IKS::General_Robot>(H, P);
+            break;
+        
+        case 3:
+                bot_kinematics = std::make_unique<IKS::General_3R>(H_remodeled, P_remodeled);
+                original_kinematics = std::make_unique<IKS::General_Robot>(H, P);  
+            break;
+        default:
+            throw std::runtime_error("Currently, only 6R and 3R Robots are solvable with EAIK.");
+            break;
+        }
+    }
+
+
+    Eigen::MatrixXd Robot::get_remodeled_H() const
+    {
+        return bot_kinematics->get_H();
+    }
+
+    Eigen::MatrixXd Robot::get_remodeled_P() const
+    {
+        return bot_kinematics->get_P();
+    }
+
+    Eigen::MatrixXd Robot::get_original_H() const
+    {
+        return original_kinematics->get_H();
+    }
+
+    Eigen::MatrixXd Robot::get_original_P() const
+    {
+        return original_kinematics->get_P();
+    }
+
+    std::string Robot::get_kinematic_family() const
+    {
+        return bot_kinematics->get_kinematic_family();
+    }
+
+    IKS::IK_Solution Robot::calculate_IK(const IKS::Homogeneous_T &ee_position_orientation) const
+    {
+        if (!bot_kinematics->has_known_decomposition())
+        {
+            throw std::runtime_error(decomposition_unknown_exception_info);
+        }
+
+        IKS::Homogeneous_T ee_06_rot = ee_position_orientation;
+        ee_06_rot.block<3,3>(0,0) *= R6T_partial.transpose();
+        IKS::IK_Solution solution = bot_kinematics->calculate_IK(ee_06_rot);
+
+        // TODO: Find alternative to this costly insert operation
+        for(const auto&[joint_index, value] : fixed_axes)
+        {
+            for(auto& Qs : solution.Q)
+            {
+                Qs.insert(Qs.begin()+joint_index, value);
+            }
+        }
+
+        return solution;
+    }    
+    
+    std::vector<IKS::IK_Solution> Robot::calculate_IK_batched(std::vector<IKS::Homogeneous_T> EE_pose_batch, const unsigned worker_threads) const
+    {
+        if (!bot_kinematics->has_known_decomposition())
+        {
+            throw std::runtime_error(decomposition_unknown_exception_info);
+        }
+
+        std::vector<IKS::IK_Solution> solutions(EE_pose_batch.size());
+        std::vector<std::thread> thread_pool;
+        thread_pool.reserve(worker_threads);
+
+        std::mutex pose_mutex;
+
+        auto kernel = 
+        [this, &pose_mutex, &solutions, &EE_pose_batch]{
+            pose_mutex.lock();
+            int solution_index = EE_pose_batch.size()-1;
+
+            while(solution_index >= 0)
+            {
+                const IKS::Homogeneous_T& pose = EE_pose_batch.back();
+                EE_pose_batch.pop_back();  
+                pose_mutex.unlock(); 
+
+                solutions.at(solution_index) = calculate_IK(pose);
+
+                pose_mutex.lock();
+                solution_index = EE_pose_batch.size()-1;
+            }
+            pose_mutex.unlock(); 
+        };
+
+        for(unsigned i = 0; i < worker_threads; i++)
+        {
+            thread_pool.push_back(std::thread(kernel));
+        }
+
+        for(unsigned i = 0; i < worker_threads; i++)
+        {
+            thread_pool.at(i).join();
+        }
+
+        return solutions;
+    }
+
+    // Eigen matrices as solutions for the pybindings
+    IKS::IK_Eigen_Solution Robot::calculate_Eigen_IK(const IKS::Homogeneous_T &ee_position_orientation) const
+    {
+        IKS::IK_Solution vector_solution = calculate_IK(ee_position_orientation);
+        IKS::IK_Eigen_Solution eigen_solution;
+
+        if(vector_solution.Q.size() > 0)
+        {
+            eigen_solution.is_LS_vec.resize(vector_solution.is_LS_vec.size());
+
+            // TODO: Find a way to avoid slow loop in future releases
+            for(unsigned i = 0; i < vector_solution.is_LS_vec.size(); ++i)
+            {
+                eigen_solution.is_LS_vec(i) = vector_solution.is_LS_vec.at(i);
+            }
+
+            eigen_solution.Q = Eigen::MatrixXd(vector_solution.Q.size(), vector_solution.Q.at(0).size());
+            for (unsigned i = 0; i < vector_solution.Q.size(); i++)
+            {
+                eigen_solution.Q.row(i) = Eigen::VectorXd::Map(vector_solution.Q.at(i).data(),vector_solution.Q.at(0).size());
+            }
+        }
+        return eigen_solution;                
+    }
+
+    std::vector<IKS::IK_Eigen_Solution> Robot::calculate_Eigen_IK_batched(std::vector<IKS::Homogeneous_T> EE_pose_batch, const unsigned worker_threads) const
+    {
+        std::vector<IKS::IK_Eigen_Solution> solutions(EE_pose_batch.size());
+        std::vector<std::thread> thread_pool;
+        thread_pool.reserve(worker_threads);
+
+        std::mutex pose_mutex;
+
+        auto kernel = 
+        [this, &pose_mutex, &solutions, &EE_pose_batch]{
+            pose_mutex.lock();
+            int solution_index = EE_pose_batch.size()-1;
+
+            while(solution_index >= 0)
+            {
+                const IKS::Homogeneous_T& pose = EE_pose_batch.back();
+                EE_pose_batch.pop_back();  
+                pose_mutex.unlock(); 
+
+                solutions.at(solution_index) = calculate_Eigen_IK(pose);
+
+                pose_mutex.lock();
+                solution_index = EE_pose_batch.size()-1;
+            }
+            pose_mutex.unlock(); 
+        };
+
+        for(unsigned i = 0; i < worker_threads; i++)
+        {
+            thread_pool.push_back(std::thread(kernel));
+        }
+
+        for(unsigned i = 0; i < worker_threads; i++)
+        {
+            thread_pool.at(i).join();
+        }
+
+        return solutions;
+    }    
+
+    IKS::Homogeneous_T Robot::fwdkin(const std::vector<double> &Q) const
+    {
+        IKS::Homogeneous_T sol_r06 = original_kinematics->fwdkin(Q);
+        sol_r06.block<3,3>(0,0) *= R6T;
+        return sol_r06;
+    }
+
+    // Function for use with pybindings and the corresponding numpy arrays
+    IKS::Homogeneous_T Robot::fwdkin_Eigen(const Eigen::VectorXd &Q) const
+    {
+        std::vector<double> Q_stdVector(Q.data(), Q.data() + Q.size());
+        IKS::Homogeneous_T sol_r06 = original_kinematics->fwdkin(Q_stdVector);
+        sol_r06.block<3,3>(0,0) *= R6T;
+        return sol_r06;
+    }
+    
+    bool Robot::is_spherical() const
+    {
+        return this->bot_kinematics->is_spherical();
+    }
+
+    bool Robot::has_known_decomposition() const
+    {
+        return this->bot_kinematics->has_known_decomposition();
+    }
+} // namespace EAIK

eaik-1.1.0/CPP/src/EAIK.h

@@ -0,0 +1,58 @@
+#ifndef EAIK_IMPL
+#define EAIK_IMPL
+
+#include <eigen3/Eigen/Dense>
+#include <memory>
+#include <tuple>
+
+#include "IKS.h"
+
+namespace EAIK
+{
+    class Robot
+    {
+    public:
+        Robot(const Eigen::MatrixXd &H, const Eigen::MatrixXd &P, const Eigen::Matrix<double, 3, 3> &R6T=Eigen::Matrix<double, 3, 3>::Identity(), const std::vector<std::pair<int, double>>& fixed_axes={}, bool is_double_precision=true);
+        Robot(const Eigen::VectorXd& dh_alpha, const Eigen::VectorXd& dh_a, const Eigen::VectorXd& dh_d, const Eigen::Matrix<double, 3, 3> &R6T=Eigen::Matrix<double, 3, 3>::Identity(), const std::vector<std::pair<int, double>>& fixed_axes={}, bool is_double_precision=true);
+
+        IKS::IK_Solution calculate_IK(const IKS::Homogeneous_T &ee_position_orientation) const;
+        std::vector<IKS::IK_Solution> calculate_IK_batched(std::vector<IKS::Homogeneous_T> EE_pose_batch, const unsigned worker_threads) const;
+
+        IKS::IK_Eigen_Solution calculate_Eigen_IK(const IKS::Homogeneous_T &ee_position_orientation) const;
+        std::vector<IKS::IK_Eigen_Solution> calculate_Eigen_IK_batched(std::vector<IKS::Homogeneous_T> EE_pose_batch, const unsigned worker_threads) const;
+
+        IKS::Homogeneous_T fwdkin(const std::vector<double> &Q) const;
+        IKS::Homogeneous_T fwdkin_Eigen(const Eigen::VectorXd &Q) const;
+
+        bool is_spherical() const;
+        bool has_known_decomposition() const;
+
+        Eigen::MatrixXd get_remodeled_H() const;
+        Eigen::MatrixXd get_remodeled_P() const;
+
+        Eigen::MatrixXd get_original_H() const;
+        Eigen::MatrixXd get_original_P() const;
+
+        std::string get_kinematic_family() const;
+
+    private:
+        // Init function to allow nice constructor overloading
+        void init(const Eigen::MatrixXd &H, const Eigen::MatrixXd &P, const std::vector<std::pair<int, double>>& fixed_axes={}, bool is_double_precision=true);
+        std::unique_ptr<IKS::General_Robot> bot_kinematics;
+        std::unique_ptr<IKS::General_Robot> original_kinematics; // Forward Kinematics calculated by original kinematics
+
+        bool spherical_wrist{false};
+
+        // single precision default thresholds
+        double ZERO_THRESHOLD = 1e-7; 
+        double AXIS_INTERSECT_THRESHOLD = 1e-6;
+        
+        Eigen::Matrix<double, 3, 3> R6T;
+        Eigen::Matrix<double, 3, 3> R6T_partial;
+        std::vector<std::pair<int, double>> fixed_axes; // Locked axes sorted by rising indices (!)
+
+        const std::string decomposition_unknown_exception_info{"No Subproblem-Decomposition for the given manipulator is currently known! Analytical IK computation is not possible."};
+
+    };
+} // namespace EAIK
+#endif