endorob.h

Go to the documentation of this file.

// v1.4
/*MIT License
 
Copyright (c) 2018-2019 Pierre Berthet-Rayne
 
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.*/
 
#ifndef EndoRob_H
#define EndoRob_H
 
#include <Eigen/StdVector>
#include <iostream>
#include <fstream>
#include <thread>
#include <Eigen/Geometry>
#include <limits>
#include <Eigen/Dense>
#include <mutex>
#include <chrono>
#include <stdlib.h>
#include <time.h>
#include <ctime>
#include <string>
#include <atomic>
 
 
 
#ifdef _WIN32
#include <windows.h>
#endif
 
//#ifdef __linux__
//#include <pthread.h>
//#endif
 
 
 
// For plotting
//#include <qwt_plot.h>
//#include <qwt_plot_curve.h>
//#include <qwt_plot_grid.h>
//#include <qwt_symbol.h>
//#include <qwt_legend.h>
#define VERBOSE_MODE 1
 
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
 
 
class Virtual_Snake;
 
enum class Joint_type {
    Revolute,       
    Prismatic,      
    Rolling,        
    Rolling_flex,   
    Flex,           
};
 
 
enum class Solver_type {
    Jacobian_pseudo_inverse,            
    Full_body_jacobian,                 
    Jacobian_DLS,                       
    Jacobian_pseudo_inverse_null_space, 
    Jacobian_transpose,                 
};
 
typedef unsigned int uint;
typedef long long llong;
 
// Matrix and vector typedefs
typedef Eigen::MatrixXd Mat;
typedef Eigen::Matrix4d Mat4d;
typedef Eigen::Matrix3d Mat3d;
typedef Eigen::Vector3d Vec3d;
typedef Eigen::Vector2d Vec2d;
typedef Eigen::VectorXd Vec;
typedef Eigen::Quaterniond Quat;
 
// Infinity macro
#define INF_LIM std::numeric_limits<double>::infinity()
 
// Chrono
typedef std::chrono::high_resolution_clock::time_point Time_point;
typedef std::chrono::high_resolution_clock chrono_time;
typedef std::chrono::milliseconds Ms;
typedef std::chrono::duration<double> Delta_t;
 
// General Functions:
 
 
Mat4d invert_transform(const Mat4d &mat);
 
 
Vec3d get_rpy(const Mat &mat);
 
 
Vec3d get_Euler(const Mat &mat);
 
 
Mat4d set_rpy(const Mat &mat, Vec3d rpy);
 
 
Mat3d rot_x(const double &angle);
 
 
Mat3d rot_y(const double &angle);
 
 
Mat3d rot_z(const double &angle);
 
 
Mat3d rot_xyz(const double &angle, const Vec3d &axis);
 
bool is_almost_equal(const double &a, const double &b, const double &tolerance = 1E-4);
bool is_geq(const double &a, const double &b, const double &tolerance = 1E-4);
bool is_leq(const double &a, const double &b, const double &tolerance = 1E-4);
 
 
Mat4d rotate_transform_locally(const Mat4d &mat, const Mat3d &rot);
 
 
Mat4d rotate_transform_around_P(const Mat4d &T, const Mat3d &rot, const Vec &P);
 
 
Mat4d rotate_transform_around_axis_and_P(const Mat4d &T, const double &angle, const Vec3d &axis, const Vec &P);
 
 
Mat3d get_rotation_increment(const Mat3d &start, const Mat3d &goal);
 
 
Vec get_vector_increment(const Vec &start, const Vec &goal);
 
 
Mat4d get_transform_increment(const Mat4d &start, const Mat4d &goal);
 
 
Vec3d project_point_onto_line(const Vec3d &P, const Vec3d &A, const Vec3d &B);
 
 
Vec3d sphere_line_intersection(const double &radius, const Vec3d &center, const Vec3d &A, const Vec3d &B);
 
double rad_to_degree(const double &val);
double degree_to_rad(const double &val);
 
enum DH_type {
    DH_standard, 
    DH_modified, 
};
 
 
 
 
class DH_table_entry
{
 
public:
    DH_table_entry(const double &a, const double &alpha, const double &d, const double &theta, const Joint_type &type);
    DH_table_entry(const double &a, const double &a_bis, const double &alpha, const double &d, const double &theta, const Joint_type &type);
 
    void set_offset(const double &offset);
    void set_joint_limits(const double &min, const double &max);
    void set_flex_resolution(const uint &res);
    void set_flex_gradient(const Vec &vec);
    double a();
    double alpha();
    double d();
    double theta();
    double a_bis();
    uint get_flex_resolution();
    Vec get_flex_gradient();
    Joint_type get_joint_type();
    double joint_limit_max();
    double joint_limit_min();
 
protected:
    double m_a, m_a_bis, m_alpha, m_d, m_theta;
    Joint_type m_joint_type;
    double m_offset;
    double m_joint_limit_min, m_joint_limit_max;
    uint m_flex_resolution;
    Vec m_flex_gradient;
 
};
 
 
 
class Snake_Path
{
public:
    Snake_Path();
    ~Snake_Path();
    void push_back(const Vec3d &point);
    void push_back(const Vec3d &point, const double &weight);
    void pop_back();
    uint size();
    void set_weight(const double &weight, const int &index = -1);
    Vec get_weights_vector(const uint &size);
    double get_weight(const uint &index);
    void clear();
 
 
    Vec3d& operator[] (uint x){
        return m_path[x];
    }
 
protected:
    std::vector<Vec3d, Eigen::aligned_allocator<Vec3d>> m_path;
    std::vector<double> m_weights;
 
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
};
 
 
 
 
 
 
class Endorob /*: protected DH_table_entry*/
{
 
public:
 
 
    Endorob();
 
 
    Endorob(const Solver_type &solver_type);
 
 
    ~Endorob();
 
 
    void set_DH_convention(const DH_type &convention);
 
 
    void save_DH(std::string filename);
 
 
    void load_DH(std::string filename);
 
 
    void add_DH_entry(const double &a, const double &alpha, const double &d, const double &theta, const Joint_type &type, const double &a_bis = 0, const double &theta_bis = 0, const uint &flex = 0);
 
 
    void set_joint_limits(const double &min, const double &max);
 
 
    void set_flex_resolution(const uint &res);
 
 
    void set_flex_gradient(const Vec &vec);
 
 
    void set_solver_type(Solver_type solver, const double &lambda = 1);
 
 
    void set_tool_frame(const Mat4d &T);
 
 
    void set_base_frame(const Mat4d &T);
 
 
    void set_mask(const Vec &mask);
 
 
    void set_joint_coupling(const Vec &coupling);
 
 
    void initialize();
 
 
    unsigned dof();
 
 
    unsigned joints();
 
 
    Mat4d get_forward_kinematics();
 
 
    Mat4d get_tip_pose(); // same as above, just different naming
 
 
    Vec3d get_tip_position();
 
 
    Mat4d get_forward_kinematics(const Vec &q);
 
 
    Mat get_Jacobian();
 
 
 
    Mat get_coupling_matrix(void);
 
 
 
 
    void set_solver_target(const Mat4d &T);
 
 
    void set_solver_body_target(const Vec &vec);
 
 
    void set_solver_body_target(const Vec &vec, const Mat4d &T);
 
 
    Vec get_instant_IK_solution();
 
 
    Vec get_IK_solution();
 
 
 
    void set_q_vec(const Vec &q);
 
 
    void reset_q_vec();
 
 
 
    Vec get_random_q_vec();
 
 
    Vec get_body_vec();
 
 
    Vec get_body_vec(const Vec &q);
 
 
    unsigned get_iteration();
 
 
    double get_error_norm();
 
 
    double get_RMS_error();
 
 
    Vec get_error_vector();
 
 
 
    Vec get_q_vec();
 
 
    Vec get_full_q_vec();
 
 
    Vec get_desired_curve();
 
 
    void set_desired_curve(const Vec &curve);
 
 
    void set_desired_curve(const Vec &curve, const Mat4d& mat);
 
 
    //Vec get_inversekinematics(const Mat4d &Td, const Vec &q_vec);
    //Vec get_inversekinematics(const Mat4d &Td);
 
    Vec solve_with_tentacle(Mat T_);
 
    // basic follow the leader navigation
    Vec follow_the_leader(const std::vector<Vec3d, Eigen::aligned_allocator<Vec3d>> path, const Vec &desired_shape, const double &translation);
    Vec follow_the_leader_2(const std::vector<Vec3d, Eigen::aligned_allocator<Vec3d>> path, const Vec &desired_shape, const double &translation);
    Vec follow_the_leader_3(Snake_Path path, const Vec &desired_shape, const double &translation);
    Virtual_Snake follow_the_leader_weighted(Snake_Path path, const Virtual_Snake &desired_shape, const double &translation);
 
 
 
    std::vector<DH_table_entry> get_DH_table();
 
 
    Mat get_full_kinematics();
 
    // Transform a full body vector relative to its base:
    Vec set_vec_relative_to_base(const Vec &vec);
 
    // Get Snake base matrix
    Mat get_snake_base();
 
 
    void set_solver_weight_mat(const uint& index, const double &val);
    void set_solver_weight_mat(const Vec& vec);
    void set_solver_weight_vec(const uint& index, const double &val);
 
    // To set the global reference frame
    void set_robot_reference_frame(const Mat4d &T);
 
 
    void enforce_joint_limits_mode(void);
 
 
    void set_faulty_joint(const uint &joint, const double &val);
 
 
    void disable_fault_tolerance();
 
 
protected:
    // Full forward kinematics (all intermediate joints) using class member variables
    void calculate_full_forward_kinematics();
 
    // Full forward kinematics (all intermediate joints) using external joint vector
    Mat calculate_full_forward_kinematics(const Vec &q);
 
    // Calculate jacobian matrix using class member variables
    void calculate_jacobian();
 
    // Calculate full body jacobian matrix
    void calculate_full_body_jacobian();
 
    // Calculate full body jacobian matrix
    void calculate_full_mobile_body_jacobian();
 
 
 
 
    // Solver function running in its own thread
    void threaded_solver(int a, int b);
    void threaded_solver_2(int a, int b);
 
    // Error (pos + ori) between desired and end effector positions
    void get_error(const Mat4d &Td, const Mat4d &Te);
 
    // Full body error between desired and current full body position.
    Vec get_full_body_error();
 
    // Full mobile body error between desired and current full body position.
    void get_full_mobile_body_error();
 
    // Position error between desired and end effector positions
    Vec3d get_position_error(const Mat4d &Td, const Mat4d &Te);
 
    // Orientation error between desired and end effector positions
    Vec3d get_orientation_error(const Mat4d &Td, const Mat4d &Te);
 
    // Check if solver has converged (might be blocking if no good solution is found)
    bool solver_has_converged();
 
    // Check if solver has ended (whether good solution or iteration overflow)
    bool solver_has_ended();
 
    //void set_DH_table(const std::vector<DH_table_entry> &DH_table_entry);
 
    // Forward kinematics matrix
    Mat4d base_transformation_matrix(DH_table_entry DH, double q);
 
    // Calculate pseudo inverse using SVD
    void calculate_pseudo_inverse();
 
    // Calculate masked pseudo inverse (reduced workspace dimension)
    void calculate_pseudo_inverse_masked();
 
    // Calculate pseudo inverse for DLS
    void calculate_pseudo_inverse_DLS();
 
    // Clean a vector for values close to zero
    Vec zero_thresholding(const Vec &q_vec, const double &val = M_PI/180/20);
 
    // Wrap angles of vector to be within [-pi,pi]
    Vec angle_modulus(const Vec &q_vec);
 
    // Remove rows specified by mask vector
    Mat mask_resize(const Mat &mat);
 
    // Apply joint coupling
    void apply_joint_coupling();
 
    // Calculate coupling matrix to increase dimension
    void calculate_coupling_matrix_increase();
 
    // Calculate coupling matrix to reduce dimension
    void calculate_coupling_matrix_reduce();
 
    // Calculate robot's DOF after coupling
    void calculte_dof_after_coupling();
 
    std::vector<std::string> split(const std::string & s, char delimiter, bool remove_empty);
 
    // Check vector for joint limits
    bool check_joint_in_limit(const Vec &q);
 
    // Calculate joint lookup table
    void calculate_joint_LUT();
 
    // Put forward kinematics into vector
    void convert_fwd_k_into_vec();
 
    // Put forward kinematics into vector from joint vector
    Vec convert_fwd_k_into_vec(Mat fwd_k);
 
    // Joint limit based jacobian
    Vec get_joint_limit_scalar();
 
    // Pseudo inverse method, simple approach
    void pseudo_inverse_simple();
 
    // Pseudo inverse method, null-space approach
    void pseudo_inverse_null_space();
 
    // Constraint vector calculation for the null space method
    void null_space_constraint_calculation();
 
    // Pseudo inverse method, normal equation approach
    void pseudo_inverse_normal();
 
    // Damped least square approach
    void damped_least_square_method();
 
    // Jacobian transpose approach
    void jacobian_transpose();
 
    // Template iterative solve equation
    void iterative_solve_equation();
 
    // Check machine CPU available
    void check_for_cpu();
 
    // error function
    void modify_error_with_f();
 
    // Solver with mask
    void solve_for_mask();
 
    // Check for convergence
    void check_for_convergence();
 
    // Solve function
    void solve();
 
    // Apply joint limits
    void apply_joint_limits();
 
public:
    // Calculate faulty joint vector without compensation
    Vec get_faulty_q();
 
 
 
 
protected:
 
    // Robot variables
    uint m_joints;
    uint m_dof;
    Mat4d m_base_frame_T, m_tool_frame_T;
    DH_type m_DH_convention;
    Mat4d m_tip_T, m_target_T;
    Vec m_q_vec, m_q_vec_rest;
    Vec m_prev_q_vec;
    Vec m_q_vec_for_user;
 
    std::vector<DH_table_entry>m_DH_table;
    bool m_check_joint_limit_jacobian;
    double m_lambda;
    void (Endorob::*p_solver_type)(void);
 
 
    // Matrices for joint coupling (mechanical coupling or rolling joint)
    Vec m_coupling_vector;
    Mat m_joint_coupling_matrix;
    Mat m_joint_coupling_reduce_dimension;
    bool m_apply_joint_coupling;
    bool m_rolling_joint_present;
    Vec m_xi_vec, m_xi_vec_rest;
    Vec m_prev_xi_vec;
    Vec m_joint_lookup_table;
 
 
    // Thread variables
    std::thread *psolver;
    volatile bool m_start_solver, m_run;
    volatile bool m_converged, m_diverged;
    bool m_iteration_overflow;
    unsigned long m_iterations;
    bool m_do_not_start_solver;
    volatile bool m_wait_for_sync;
    volatile bool m_solver_pending;
    volatile bool m_thread_running;
 
    // Mask matrix for workspace reduction
    unsigned m_mask_dof;
    Vec m_mask_vector;
    bool m_apply_mask;
 
    // Jacobian calculation variables:
    Mat m_J, m_J_inv, m_J_masked, m_J_inv_masked;
    Mat m_J_for_user;
    Mat m_J_vel, m_J_rot;
    Vec3d m_p_e;
    Vec3d m_z_tmp, m_p_tmp;
    Mat m_error_weights;
    Vec m_error_weights_vec;
    Vec m_phi, m_alpha;
    Mat m_identity;
    unsigned m_joint_index_offset;
    bool m_apply_weighted_solution;
 
    // Forward Kinematics variables:
    //std::vector<Mat4d, Eigen::aligned_allocator<Mat4d>> m_fwd_k_vec;
    Mat m_fwd_k_vec;
    Vec m_full_body_vec_current, m_full_body_vec_desired;
    Vec m_virtual_snake;
    bool m_full_body_needed;
    Mat4d m_robot_frame_T, m_global_frame_to_robot_increment, m_robot_frame_to_global_increment;
 
    // Error vector
    Vec m_error, m_prev_error, m_error_masked, m_prev_error_masked;
 
    // Path
    std::vector<Vec3d, Eigen::aligned_allocator<Vec3d>> m_robot_path;
    static Vec m_virtual_robot;
 
    // CPU and instantiation
    static uint m_CPU_amount;
    static uint m_CPU_cpt;
    uint m_CPU_affinity;
    static volatile bool m_assigning_CPU;
 
    static uint m_instantiations;
    static bool m_cpu_info_dispayed;
    uint m_instantiation_index;
 
    // Fault tolerance
    std::vector<bool> m_faulty_joint_vec;
    std::atomic<bool> m_apply_fault_tolerance;
 
 
 
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
};
 
 
class Virtual_Snake
{
public:
    Virtual_Snake();
    Virtual_Snake(Endorob *robot);
    Virtual_Snake(const Virtual_Snake &snake);
    ~Virtual_Snake();
    Vec3d pe();
    void set_pe(const Vec3d &pe);
    Vec3d p0();
    void set_p0(const Vec3d &p0);
    Vec3d pb();
    Vec base();
    Vec curve() const;
    void set_curve(const Vec &curve);
    void set_weights(const Vec &weights);
    void set_weight(const uint &index, const double &val);
    Vec weights() const;
    uint get_links();
    uint rows()const;
    Vec3d block(const uint &index);
    void set(const uint &index1, const uint &index2, const uint &block1, const uint &block2, const Vec &val);
 
 
    double& operator() (uint x){
        return m_curve(x);
    }
 
    const double& operator() (uint x)const
    {
        return m_curve(x);
    }
 
 
protected:
    Vec m_curve;
    Vec m_weights;
 
 
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
};
 
 
class Motion_RW
{
    enum Motion_mode {
        Write,      
        Read,       
        Read_Write, 
        Benchmark,  
        Idle,       
    };
 
public:
    Motion_RW();
    Motion_RW(Endorob *robot);
    ~Motion_RW();
    void set_endorob_pointer(Endorob *robot);
    void set_virt_snake_pointer(Virtual_Snake *virt_snake);
    void set_write_frequency(const double &freq);
    void set_read_frequency(const double &freq);
    void start_recording();
    void stop_recording();
    void set_file_name_write(const std::string &file);
    void set_file_name_read(const std::string &file);
    void load_file(const std::string &file);
    void load_virtual_snake_file(const std::string &file);
    void benchmark_virtual_snake_file(const std::string &file);
    void benchmark_virtual_snake_vec(const std::vector<Vec> &virtual_vec, const std::vector<Mat4d> &Td_vec);
    void write_once(const Virtual_Snake &curv, const Mat4d &Ttip);
    void write_once(const char& key);
    bool is_reading_finished();
    bool is_running();
 
 
 
 
protected:
    void motion_RW_loop(int a, int b);
    void output_header();
    void output_data();
 
protected:
    std::thread *pthread;
    volatile bool m_run;
    bool m_header_written;
    volatile bool m_start_loop;
    double m_period_write, m_period_read;
    Endorob *probot;
    Virtual_Snake *pvirt_snake;
    std::ofstream m_file_write;
    std::ifstream m_file_read;
    Time_point m_t0_write, m_t0_read, m_start_time;
    std::string m_file_name_write, m_file_name_read;
    Motion_mode m_mode;
    std::vector<Mat, Eigen::aligned_allocator<Mat>> m_q_data;
    std::vector<Mat, Eigen::aligned_allocator<Mat>> m_T_data;
    std::vector<double> m_error_data;
    //std::vector<double> m_input_data;
    std::vector<double> m_input_timing;
    std::vector<Vec> m_virtual_snake_data;
    std::vector<Mat4d> m_tip_data;
    bool m_overwrite_file_period;
    enum class File_type {
        Joint,          
        Virtual_robot,  
        Master          
    };
    File_type m_file_type;
    static uint m_file_counter;
    bool m_runonce;
    std::ofstream m_benchmark_file;
    std::ofstream m_benchmark_check_file;
 
    // TRO review
    Snake_Path m_robot_path_for_plot;
 
 
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
};
 
 
 
 
#endif // EndoRob_H
 
 
 
 
// Actuation space reduction problem:
// robot dof (i2snake) 8 DOF after coupling
// robot joints: 14 DOF
// robot model : 27 DOF
// ikine process: 8 -> 14 -> 27 -> Jacobian calculation -> 27 -> 14 -> 8
// question is will the joint values be identical where expected
// hypothesis is yes since jacobian column will be the same...
// q is 27x1 so mask matrix should be 8x27.27x1 => 8x1