EAIK 1.0.0__tar.gz → 1.1.0__tar.gz

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

@@ -51,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,
@@ -105,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);
@@ -143,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;
@@ -232,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;
 	}
@@ -402,6 +377,7 @@ namespace IKS
 			//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;
 		}
 
@@ -415,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;
@@ -431,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;
 		}
 
@@ -489,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;
 		}
 
@@ -582,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)
@@ -593,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);
 
@@ -611,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);
@@ -648,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;
 	}
 
@@ -888,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-1.0.0 → 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-1.0.0 → eaik-1.1.0}/CPP/src/CMakeLists.txt

@@ -9,7 +9,7 @@ endif()
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_FLAGS_RELEASE "-O3")
 
-set(sources EAIK.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/kinematic_remodelling.cpp ${CMAKE_CURRENT_SOURCE_DIR}/IK/utils/kinematic_utils.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 ${CMAKE_CURRENT_SOURCE_DIR}/IK/utils)

{eaik-1.0.0 → eaik-1.1.0}/CPP/src/EAIK.cpp

@@ -3,7 +3,7 @@
 #include <thread>
 #include <mutex>
 
-#include "kinematic_remodelling.h"
+#include "kinematic_remodeling.h"
 #include "kinematic_utils.h"
 #include "EAIK.h"
 
@@ -34,8 +34,8 @@ namespace EAIK
             throw std::runtime_error("Wrong input dimensions for H and P. Note that #P = #H+1.");
         }
 
-        Eigen::MatrixXd P_remodelled;
-        Eigen::MatrixXd H_remodelled;
+        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)
@@ -44,43 +44,25 @@ namespace EAIK
             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_remodelled = remodel_kinematics(H_part, P_part, ZERO_THRESHOLD, AXIS_INTERSECT_THRESHOLD);
-            H_remodelled = H_part;
+            P_remodeled = remodel_kinematics(H_part, P_part, ZERO_THRESHOLD, AXIS_INTERSECT_THRESHOLD);
+            H_remodeled = H_part;
             this->R6T_partial = R6T_part;
         }
         else
         {
-            P_remodelled = remodel_kinematics(H, P, ZERO_THRESHOLD, AXIS_INTERSECT_THRESHOLD);
-            H_remodelled = H;
+            P_remodeled = remodel_kinematics(H, P, ZERO_THRESHOLD, AXIS_INTERSECT_THRESHOLD);
+            H_remodeled = H;
             this->R6T_partial = R6T;
         }
-        switch (H_remodelled.cols())
+        switch (H_remodeled.cols())
         {
         case 6:
-            // Check if last three axes intersect
-            if (P_remodelled.col(P_remodelled.cols()-3).norm() < ZERO_THRESHOLD && P_remodelled.col(P_remodelled.cols()-2).norm() < ZERO_THRESHOLD)
-            {   
-                spherical_wrist = true;
-                bot_kinematics = std::make_unique<IKS::Spherical_Wrist_Robot>(H_remodelled, P_remodelled);
-                original_kinematics = std::make_unique<IKS::General_Robot>(H, P);
-            }
-            else if(P_remodelled.col(1).norm() < ZERO_THRESHOLD && P_remodelled.col(2).norm() < ZERO_THRESHOLD)
-            {
-                // Spherical wrist is located at the base of the robot
-                spherical_wrist = true;
-                const auto&[H_reversed, P_reversed] = IKS::reverse_kinematic_chain(H_remodelled, P_remodelled);
-                bot_kinematics = std::make_unique<IKS::Spherical_Wrist_Robot>(H_reversed, P_reversed, true);
-                original_kinematics = std::make_unique<IKS::General_Robot>(H, P);
-            }
-            else 
-            {
-                bot_kinematics = std::make_unique<IKS::General_6R>(H_remodelled, P_remodelled);
-                original_kinematics = std::make_unique<IKS::General_Robot>(H, P);
-            }
+            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_remodelled, P_remodelled);
+                bot_kinematics = std::make_unique<IKS::General_3R>(H_remodeled, P_remodeled);
                 original_kinematics = std::make_unique<IKS::General_Robot>(H, P);  
             break;
         default:
@@ -90,8 +72,38 @@ namespace EAIK
     }
 
 
+    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);
@@ -110,6 +122,11 @@ namespace EAIK
     
     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);
@@ -231,7 +248,7 @@ namespace EAIK
     
     bool Robot::is_spherical() const
     {
-        return spherical_wrist;
+        return this->bot_kinematics->is_spherical();
     }
 
     bool Robot::has_known_decomposition() const

{eaik-1.0.0 → eaik-1.1.0}/CPP/src/EAIK.h

@@ -26,6 +26,15 @@ namespace EAIK
 
         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);
@@ -41,6 +50,9 @@ namespace EAIK
         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

{eaik-1.0.0 → eaik-1.1.0}/CPP/src/IK/3R_IK.cpp

@@ -97,20 +97,18 @@ namespace IKS
         }
         else
         {
-            double d = this->H.col(0).transpose()*(p_13-this->P.col(1));
-            SP4 sp4(this->H.col(0), this->P.col(2), this->H.col(1), d);
-            sp4.solve();
+            SP3 sp3(this->P.col(2), -this->P.col(1), this->H.col(1), p_13.norm());
+            sp3.solve();
 
-            const std::vector<double> &theta_2 = sp4.get_theta();
+            const std::vector<double> &theta_2 = sp3.get_theta();
 
             for (const auto &q2 : theta_2)
             {
                 const Eigen::Matrix3d r12 = Eigen::AngleAxisd(q2, this->H.col(1).normalized()).toRotationMatrix();
-                SP1 sp1(p_13, this->P.col(1)+r12*this->P.col(2), -this->H.col(0));
+                SP1 sp1(p_13, this->P.col(1) + r12*this->P.col(2), -this->H.col(0));
                 sp1.solve();
-
-                solution_t_12.Q.push_back({sp1.get_theta(), q2});
-                solution_t_12.is_LS_vec.push_back(sp4.solution_is_ls()||sp1.solution_is_ls());
+                solution_t_12.Q.push_back({sp1.get_theta(),q2});
+                solution_t_12.is_LS_vec.push_back(sp3.solution_is_ls()||sp1.solution_is_ls());
             }
         }