EAIK 0.0.1__tar.gz → 1.0.0__tar.gz

{eaik-0.0.1/external/EAIK → eaik-1.0.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>
@@ -401,7 +396,7 @@ 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;

{eaik-0.0.1/external/EAIK → eaik-1.0.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_remodelling.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.0.0/CPP/src/EAIK.cpp

@@ -0,0 +1,241 @@
+#include <iostream>
+#include <eigen3/Eigen/Dense>
+#include <thread>
+#include <mutex>
+
+#include "kinematic_remodelling.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_remodelled;
+        Eigen::MatrixXd H_remodelled;
+
+        // 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_remodelled = remodel_kinematics(H_part, P_part, ZERO_THRESHOLD, AXIS_INTERSECT_THRESHOLD);
+            H_remodelled = H_part;
+            this->R6T_partial = R6T_part;
+        }
+        else
+        {
+            P_remodelled = remodel_kinematics(H, P, ZERO_THRESHOLD, AXIS_INTERSECT_THRESHOLD);
+            H_remodelled = H;
+            this->R6T_partial = R6T;
+        }
+        switch (H_remodelled.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);
+            }
+            break;
+        
+        case 3:
+                bot_kinematics = std::make_unique<IKS::General_3R>(H_remodelled, P_remodelled);
+                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;
+        }
+    }
+
+
+    IKS::IK_Solution Robot::calculate_IK(const IKS::Homogeneous_T &ee_position_orientation) const
+    {
+        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
+    {
+        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 spherical_wrist;
+    }
+
+    bool Robot::has_known_decomposition() const
+    {
+        return this->bot_kinematics->has_known_decomposition();
+    }
+} // namespace EAIK

eaik-1.0.0/CPP/src/EAIK.h

@@ -0,0 +1,46 @@
+#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;
+    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 (!)
+    };
+} // namespace EAIK
+#endif

eaik-1.0.0/CPP/src/IK/3R_IK.cpp

@@ -0,0 +1,150 @@
+#include <eigen3/Eigen/Dense>
+#include <vector>
+#include <iostream>
+#include <math.h>
+
+#include "sp.h"
+#include "IKS.h"
+#include "utils/kinematic_utils.h"
+
+namespace IKS
+{
+    General_3R::General_3R(const Eigen::Matrix<double, 3, 3> &H, const Eigen::Matrix<double, 3, 4> &P)
+        : General_Robot(H,P), H(H), P(P)
+    {
+    }
+
+    IK_Solution General_3R::calculate_IK(const Homogeneous_T &ee_position_orientation) const
+    {
+        IK_Solution solution;
+        IK_Solution solution_t_12;
+        const Eigen::Vector3d p_0t = ee_position_orientation.block<3, 1>(0, 3);
+        const Eigen::Matrix3d r_03 = ee_position_orientation.block<3, 3>(0, 0);
+        const Eigen::Vector3d p_13 = p_0t - this->P.col(0) - r_03*this->P.col(3);        
+
+        // Checking for intersecting/parallel Axes
+        if (std::fabs(this->H.col(1).cross(this->H.col(2)).transpose() * this->P.col(2)) < ZERO_THRESH && !this->H.col(1).cross(this->H.col(2)).isZero(ZERO_THRESH))
+        {
+            // (h2 x h3)(p23)==0) -> h2 intersects h3 - make sure they are not parallel!
+        
+            if(this->P.col(1).isZero(ZERO_THRESH) && this->P.col(2).isZero())
+            {
+                // Spherical-Wrist case with three intersecting axes
+                SP4 sp4(this->H.col(0), this->H.col(2), this->H.col(1), this->H.col(0).transpose()*r_03*this->H.col(2));
+                sp4.solve();
+
+                for(const auto& q2 : sp4.get_theta())
+                {
+                    const Eigen::Matrix3d r12 = Eigen::AngleAxisd(q2, this->H.col(1).normalized()).toRotationMatrix();
+                    SP1 sp1_q1(r_03*this->H.col(2), r12*this->H.col(2), -this->H.col(0));
+                    sp1_q1.solve();
+
+                    const Eigen::Matrix3d r01 = Eigen::AngleAxisd(sp1_q1.get_theta(), this->H.col(0).normalized()).toRotationMatrix();
+                    const Eigen::Vector3d hn = create_normal_vector(this->H.col(2));
+
+                    SP1 sp1_q3(hn, r12.transpose()*r01.transpose()*r_03*hn, this->H.col(2));
+                    sp1_q3.solve();
+                    solution.Q.push_back({sp1_q1.get_theta(), q2, sp1_q3.get_theta()});
+                    solution.is_LS_vec.push_back(sp1_q1.solution_is_ls()||sp4.solution_is_ls()||sp1_q3.solution_is_ls());
+                }
+
+                // All angles calculated in this case
+                return solution;
+            }
+            SP1 sp1_q1(this->P.col(1), p_13, this->H.col(0));
+            sp1_q1.solve();
+
+            const double q1 = sp1_q1.get_theta();
+            const Eigen::Matrix3d r01 = Eigen::AngleAxisd(q1, this->H.col(0).normalized()).toRotationMatrix();
+
+            // Solve Theta 2 via orientation kinematics
+            SP1 sp1_q2(this->H.col(2),r01.transpose()*r_03*this->H.col(2),this->H.col(1));
+            sp1_q2.solve();
+            solution_t_12.Q.push_back({q1,sp1_q2.get_theta()});
+            solution_t_12.is_LS_vec.push_back(sp1_q1.solution_is_ls()||sp1_q2.solution_is_ls());
+        }
+        else if (this->H.col(0).cross(this->H.col(1)).norm() < ZERO_THRESH)
+        {
+            // Axis 1 || Axis 2
+            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 = 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.solve();
+
+                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());
+            }
+        }
+        else if (std::fabs(this->H.col(0).cross(this->H.col(1)).transpose() * this->P.col(1)) < ZERO_THRESH)
+        {
+            // (h1 x h2)(p12)==0) -> h1 intersects h2
+            SP2 sp2(p_13, this->P.col(2), -this->H.col(0), this->H.col(1));
+            sp2.solve();
+
+            for(unsigned i = 0; i < sp2.get_theta_1().size(); i++)
+            {
+                const double q1 = sp2.get_theta_1().at(i);
+                const double q2 = sp2.get_theta_2().at(i);
+                solution_t_12.Q.push_back({q1, q2});
+                solution_t_12.is_LS_vec.push_back(sp2.solution_is_ls());
+            }
+        }
+        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();
+
+            const std::vector<double> &theta_2 = sp4.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.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());
+            }
+        }
+
+        for(unsigned i = 0; i < solution_t_12.Q.size(); i++)
+        {
+            const double q1 = solution_t_12.Q.at(i).at(0);
+            const double q2 = solution_t_12.Q.at(i).at(1);
+
+            const Eigen::Matrix3d r12 = Eigen::AngleAxisd(q2, this->H.col(1).normalized()).toRotationMatrix();
+            const Eigen::Matrix3d r01 = Eigen::AngleAxisd(q1, this->H.col(0).normalized()).toRotationMatrix();
+
+            // hn must not be collinear to H3!
+            const Eigen::Vector3d hn = create_normal_vector(this->H.col(2));
+
+            SP1 sp1(hn, r12.transpose()*r01.transpose()*r_03*hn, this->H.col(2));
+            sp1.solve();
+            solution.Q.push_back({q1, q2, sp1.get_theta()});
+            solution.is_LS_vec.push_back(solution_t_12.is_LS_vec.at(i)|sp1.solution_is_ls());
+        }
+
+        // Finding 6DOF Problem for 3R is overconstrained -> "Throw away" extraneous orientation solutions
+        for(unsigned i = 0; i < solution.Q.size(); i++)
+        {
+            if(!solution.is_LS_vec.at(i))
+            {
+				IKS::Homogeneous_T result = fwdkin(solution.Q.at(i));
+				double error = (result - ee_position_orientation).norm();
+
+                if(error > ZERO_THRESH)
+                {
+                    solution.is_LS_vec.at(i) = true;
+                }
+            }
+        }
+        return solution;
+    }
+};

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

@@ -9,7 +9,7 @@ endif()
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_FLAGS_RELEASE "-O3")
 
-set(sources Spherical_IK.cpp General_IK.cpp)
+set(sources Spherical_IK.cpp General_IK.cpp 3R_IK.cpp ${CMAKE_CURRENT_SOURCE_DIR}/utils/kinematic_utils.cpp)
 
 add_library(EAIK_Kinematics ${sources})
 target_include_directories(EAIK_Kinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/../../external/ik-geo/cpp/subproblems)