├── LICENSE ├── README.md ├── c++ ├── CMakeLists.txt ├── include │ ├── constantcurvaturemodel.h │ ├── cosseratrodmodel.h │ ├── piecewiseconstantcurvaturemodel.h │ ├── pseudorigidbodymodel.h │ ├── subsegmentcosseratrodmodel.h │ └── tendondrivenrobot.h ├── main.cpp └── src │ ├── constantcurvaturemodel.cpp │ ├── cosseratrodmodel.cpp │ ├── piecewiseconstantcurvaturemodel.cpp │ ├── pseudorigidbodymodel.cpp │ ├── subsegmentcosseratrodmodel.cpp │ └── tendondrivenrobot.cpp └── matlab ├── Models ├── CC │ ├── CC_solver.m │ ├── calctendonlength.m │ ├── configuration.m │ ├── construct_tdcr_cc.m │ ├── coupletransformations.m │ ├── plot_tdcr_cc.m │ └── trans_mat_cc.m ├── CCsub │ ├── CCsub_solver.m │ ├── construct_tdcr_ccsub.m │ ├── cross_product.m │ ├── plot_tdcr_ccsub.m │ ├── rotation_mat_ccsub.m │ └── trans_mat_ccsub.m ├── PRBM │ ├── PRBM_solver.m │ ├── construct_tdcr_prbm.m │ ├── plot_tdcr_prbm.m │ ├── rotation_mat_prbm.m │ └── trans_mat_prbm.m └── VC │ ├── VC_solver.m │ ├── VCref_solver.asv │ ├── VCref_solver.m │ ├── boundcond.m │ ├── get_stiffness.m │ ├── intermedquant.m │ ├── lie.m │ ├── plot_tdcr_vc.m │ └── setup_param_vc.m ├── Parameters └── tdcr_2segments.m ├── main.asv └── main.m /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Continuum Robotics Lab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Modeling of Tendon Driven Continuum Robots 2 | 3 | This code implements different approaches to model the kinematics/statics of a tendon driven continuum robot (TDCR) found in the current state of the art. 4 | The implementation considers a two segment TDCR with three tendons per segment subject to an external load the robot's tip. 5 | Both C++ and MATLAB code is provided. 6 | The following modeling approaches are implemented: 7 | 8 | - A geometry based modeling approach assuming constant curvature bending for each segment (Constant Curvature Model) 9 | - A static modeling approach modeling assuming constant curvature bending for each subsegment, i.e. between neighbouring disks (Piecewise Constant Curvature Model) 10 | - A static modeling approach modeling each subsegment as a pseudo rigid body with virtual discrete joints (Pseudo Rigid Body Model) 11 | - A static modeling approach modeling each segment as a Cosserat rods subject to tendon loads (Cosserat Rod Model) 12 | - A static modeling approach modeling each subsegment, i.e. between neighbouring disks, as a Cosserat rods subject to tendon loads (Subsegment Cosserat Rod Model) 13 | 14 | Every model is based upon a state of the art modeling approach. 15 | This repository is part of the following publication, which also provides theoretical derivations of each model as well as references to the original publications of each: 16 | 17 | How to model tendon-driven continuum robots and benchmark modelling performance 18 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 19 | frontiers in Robotics and AI 2021 20 | DOI: 10.3389/frobt.2020.630245 21 | 22 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 23 | 24 | ### Dependencies (C++) 25 | 26 | The C++ implementation makes heavy use of both the Eigen library and the GNU Scientific Library, which need to be installed in order to compile the provided C++ code. 27 | 28 | - [Eigen Library](http://eigen.tuxfamily.org/index.php?title=Main_Page) 29 | - [GNU Scientific Library](https://www.gnu.org/software/gsl/) 30 | 31 | ### Installation Instructions using CMake (C++) 32 | 33 | In the root directory of the C++ code (where the main.cpp file is located) run the following commands: 34 | 35 | mkdir build 36 | cd build 37 | cmake .. 38 | make 39 | 40 | Alternatively, the code can be compiled in Release mode for performance: 41 | 42 | mkdir build 43 | cd build 44 | cmake -DCMAKE_BUILD_TYPE=Release .. 45 | make 46 | 47 | Afterwards, you can execute the code by running the executable "tdcr-modeling". 48 | 49 | ### More Information 50 | 51 | It might be worthwhile to dry different numerical solvers for the non-linear least squares fitting to check which offers the best convergence performance for a given set of TDCR parameters, tendon tensions and external loads. Different solvers can be commented and uncommented in the .cpp files of each implemented model. From experience, both the Levenberg-Marquadt solver (selected by default) and the Dogleg solver perform generally well. 52 | 53 | If you found the provided implementation of the TDCR modeling approaches helpful or used parts of it yourself, please refer to it using the following BibTeX entry: 54 | 55 | @ARTICLE{RaoTDCRModelling, 56 | AUTHOR={Rao, Priyanka and Peyron, Quentin and Lilge, Sven and Burgner-Kahrs, Jessica}, 57 | TITLE={How to Model Tendon-Driven Continuum Robots and Benchmark Modelling Performance}, 58 | JOURNAL={Frontiers in Robotics and AI}, 59 | VOLUME={7}, 60 | YEAR={2021}, 61 | DOI={10.3389/frobt.2020.630245}, 62 | ISSN={2296-9144} 63 | } 64 | 65 | -------------------------------------------------------------------------------- /c++/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.2) 2 | 3 | project(tdcr-modeling) 4 | 5 | #Important CMAKE stuff 6 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 7 | 8 | #Eigen 9 | find_package (Eigen3 3.3 REQUIRED NO_MODULE) 10 | find_package(GSL REQUIRED) 11 | 12 | #include directories 13 | include_directories(${PROJECT_SOURCE_DIR}/include) 14 | include_directories(${PROJECT_SOURCE_DIR}) 15 | 16 | #include source code 17 | file(GLOB H_FILES ${PROJECT_SOURCE_DIR}/include/*.h) 18 | file(GLOB CPP_FILES ${PROJECT_SOURCE_DIR}/src/*.cpp) 19 | 20 | add_executable(tdcr-modeling main.cpp ${H_FILES} ${CPP_FILES}) 21 | target_link_libraries(tdcr-modeling Eigen3::Eigen GSL::gsl) 22 | -------------------------------------------------------------------------------- /c++/include/constantcurvaturemodel.h: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | #ifndef CONSTANTCURVATUREMODEL_H 15 | #define CONSTANTCURVATUREMODEL_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | 28 | // This class implements a simple Constant Curvature model to solve the forward kinematics of a TDCR. 29 | // This code is explicitly written for a two segment TDCR with either three or two tendons per segment (two segments if two_tendons is true). 30 | // The tendons are equally distributed around the backbone with the distance pradius_disks. 31 | // The first tendon is located at the y-axis of the local disk frame, while the other tendons are offset by 120 degrees (three tendons) or 120 degrees (two tendons), respectively. 32 | class ConstantCurvatureModel 33 | { 34 | public: 35 | 36 | // Constructor to set up the Constant Curvature model with default parameters 37 | ConstantCurvatureModel(); 38 | 39 | // Simple destructor 40 | virtual ~ConstantCurvatureModel(); 41 | 42 | // This function runs the forward kinematics for the TDCR. It returns true, if the forward kinematics were solved successfully. 43 | // 44 | // Inputs: 45 | // q 6x1 vector containing the actuation values for each tendon (displacements in meter). 46 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 47 | // 48 | // Outputs: 49 | // diskFrames 4x(4*n) matrix containing the current disk frames (4x4 transformation matrix for each discrete disk) of the robot as a stacked matrix. 50 | bool forwardKinematics(Eigen::MatrixXd &diskFrames, Eigen::MatrixXd q); 51 | 52 | // This function allows to set and update the TDCR parameters. 53 | // 54 | // Inputs: 55 | // length std::array that holds the length of each of the two segments of the TDCR. 56 | // number_disks std::array that holds the number of disks for each of the two segments of the TDCR. 57 | // pradius_disks std::array that holds the pitch radius of the disks (distance between tendon routing and backbone) for each of the two segments of the TDCR. 58 | // two_tendons Specifies, if only two tendons for each segment are employed and actuated. 59 | void setRobotParameters(std::array length, std::array number_disks, std::array pradius_disks, bool two_tendons); 60 | 61 | private: 62 | 63 | // Member variables 64 | std::array m_length; 65 | std::vector m_routing; 66 | std::array m_number_disks; 67 | std::array m_pradius_disks; 68 | bool m_two_tendons; 69 | }; 70 | 71 | #endif // CONSTANTCURVATUREMODEL_H 72 | -------------------------------------------------------------------------------- /c++/include/cosseratrodmodel.h: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | #ifndef COSSERATRODMODEL_H 15 | #define COSSERATRODMODEL_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | // This class implements a Cosserat Rod model to solve the forward kinematics of a TDCR. 34 | // This code is written for a two segment TDCR with to three tendons per segment subject to an external load at the tip. 35 | class CosseratRodModel 36 | { 37 | public: 38 | 39 | // Constructor to set up the Constant Curvature model with default parameters 40 | CosseratRodModel(); 41 | 42 | // Simple destructor 43 | virtual ~CosseratRodModel(); 44 | 45 | // This function runs the forward kinematics for the TDCR. It returns true, if the forward kinematics were solved successfully. 46 | // 47 | // Inputs: 48 | // q 6x1 vector containing the actuation values for each tendon (force in Newton). 49 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 50 | // f_ext 3x1 vector containing the external force acting at the tip/end-effector of the robot expressed in the base frame. 51 | // l_ext 3x1 vector containing the external moment acting at the tip/end-effector of the robot expressed in the base frame. 52 | // 53 | // Outputs: 54 | // diskFrames 4x(4*n) matrix containing the current disk frames (4x4 transformation matrix for each discrete disk) of the robot as a stacked matrix. 55 | bool forwardKinematics(Eigen::MatrixXd &diskFrames, Eigen::MatrixXd q, Eigen::Vector3d f_ext, Eigen::Vector3d l_ext); 56 | 57 | // This function implements the Cosserat Rod ODEs 58 | // 59 | // Inputs: 60 | // y Vector containing the current state. 61 | // 62 | // Outputs: 63 | // dy Vector containing the derivative of the current state. 64 | void cosserat_ode(Eigen::MatrixXd &dy, Eigen::MatrixXd y); 65 | 66 | // This function implements the boundary conditions that need to be solved for the Cosserat rod model (force and moment equilibirium at the tip of the robot). 67 | // 68 | // Inputs: 69 | // output 6x1 vector containing the force and moment equilibirium at the tip of the robot. 70 | // 71 | // Outputs: 72 | // input 6x1 vector containing the current initial values for u and v at s = 0. 73 | void get_res(Eigen::MatrixXd &output, Eigen::MatrixXd input); 74 | 75 | // This function enables a continuation mode. 76 | // That means, that every new run of the forward kinematics will use the final values for these variables obtained from the last forward kinematics solution as the new initial guess. 77 | // This makes sense in cases, where configurations of the robot only change marginally (thus the initial guesses would be similar), 78 | // and can increase computation time a lot, since the algorithm will converge in just a couple of iterations. 79 | // 80 | // Inputs: 81 | // keep Boolean value to indicate, if the continuation mode is used or not. 82 | // If continuation mode is disabled, the default initial values are used for the initial guess 83 | // (by default this is the straight, not bent state of the robot, but they can also be changed - see below). 84 | void setKeepInits(bool keep); 85 | 86 | // This function allows to set and update the TDCR parameters. 87 | // 88 | // Inputs: 89 | // length std::array that holds the length of each of the two segments of the TDCR. 90 | // youngs_modulus Youngs modulus of the backbone of the TDCR. 91 | // routing std::vector that holds the routing position of each tendon of the TDCR expressed as a 3x1 position vector in the local disk frame. 92 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 93 | // number_disks std::array that holds the number of disks for each of the two segments of the TDCR. 94 | // pradius_disks std::array that holds the pitch radius of the disks (distance between tendon routing and backbone) for each of the two segments of the TDCR. 95 | // ro Radius of the backbone of the TDCR. 96 | void setRobotParameters(std::array length, double youngs_modulus, std::vector routing, std::array number_disks, std::array pradius_disks, double ro); 97 | 98 | // This function allows to set the default initial values that are used as an initial guess for the implemented shooting method. 99 | // 100 | // Inputs: 101 | // inits 6x1 vector containing the default values that are used as an initial guess for u and v at s = 0, if continuation mode is disabled. 102 | void setDefaultInitValues(Eigen::MatrixXd inits); 103 | 104 | // This function returns the last values final values of the shooting method for the last solution of the forward kinematics. 105 | // It returns 6x1 vector containing the last values for u and v at s = 0. 106 | Eigen::MatrixXd getFinalInitValues(); 107 | 108 | // This function returns a double value containing the least squares resudial of the boundary conditions for the last solution of the forward kinematics. 109 | double getFinalResudial(); 110 | 111 | 112 | private: 113 | // Private member function that solves the initial value problem of the Cosserat Rod ODEs. 114 | void run_IVP(Eigen::MatrixXd &states, Eigen::MatrixXd init_state); 115 | 116 | // Member variables 117 | std::array m_length; 118 | double m_youngs_modulus; 119 | std::vector m_routing; 120 | std::array m_number_disks; 121 | std::array m_pradius_disks; 122 | double m_ro; 123 | 124 | bool m_keep_inits; 125 | 126 | Eigen::Matrix3d m_Kbt; 127 | Eigen::Matrix3d m_Kse; 128 | 129 | Eigen::Vector3d m_f_ext; 130 | Eigen::Vector3d m_l_ext; 131 | 132 | Eigen::Matrix m_tau; 133 | 134 | Eigen::MatrixXd m_last_inits; 135 | Eigen::MatrixXd m_default_inits; 136 | 137 | double m_residual_error; 138 | 139 | Eigen::MatrixXd m_states1; 140 | Eigen::MatrixXd m_states2; 141 | 142 | int m_current_segment; 143 | 144 | //Stuff for ODE solving 145 | gsl_odeiv2_system m_ode_sys; 146 | const gsl_odeiv2_step_type* m_ode_step_type; 147 | gsl_odeiv2_step* m_ode_step; 148 | gsl_odeiv2_control* m_ode_controller; 149 | gsl_odeiv2_evolve* m_ode_evolve; 150 | }; 151 | 152 | #endif // COSSERATRODMODEL_H 153 | -------------------------------------------------------------------------------- /c++/include/piecewiseconstantcurvaturemodel.h: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | #ifndef PIECEWISECONSTANTCURVATUREMODEL_H 15 | #define PIECEWISECONSTANTCURVATUREMODEL_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | // This class implements a Piecewise Constant Curvature model to solve the forward kinematics of a TDCR. 33 | // This code is written for a two segment TDCR with to three tendons per segment subject to an external load at the tip. 34 | class PiecewiseConstantCurvatureModel 35 | { 36 | public: 37 | 38 | // Constructor to set up the Piecewise Constant Curvature model with default parameters 39 | PiecewiseConstantCurvatureModel(); 40 | 41 | // Simple destructor 42 | virtual ~PiecewiseConstantCurvatureModel(); 43 | 44 | // This function runs the forward kinematics for the TDCR. It returns true, if the forward kinematics were solved successfully. 45 | // 46 | // Inputs: 47 | // q 6x1 vector containing the actuation values for each tendon (force in Newton). 48 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 49 | // f_ext 3x1 vector containing the external force acting at the tip/end-effector of the robot expressed in the base frame. 50 | // l_ext 3x1 vector containing the external moment acting at the tip/end-effector of the robot expressed in the base frame. 51 | // 52 | // Outputs: 53 | // diskFrames 4x(4*n) matrix containing the current disk frames (4x4 transformation matrix for each discrete disk) of the robot as a stacked matrix. 54 | bool forwardKinematics(Eigen::MatrixXd &diskFrames, Eigen::MatrixXd q, Eigen::Vector3d f_ext, Eigen::Vector3d l_ext); 55 | 56 | // This function implements the boundary conditions that need to be solved for the Cosserat rod model (force and moment equilibirium at the tip of the robot). 57 | // 58 | // Inputs: 59 | // output 3nx1 vector containing the moment equilibirium at the end of each subsegment. 60 | // 61 | // Outputs: 62 | // input 3nx1 vector containing the current values for each subsegment (beta, gamma, epsilon). 63 | void get_res(Eigen::MatrixXd &output, Eigen::MatrixXd input); 64 | 65 | // This function enables a continuation mode. 66 | // That means, that every new run of the forward kinematics will use the final values for these variables obtained from the last forward kinematics solution as the new initial guess. 67 | // This makes sense in cases, where configurations of the robot only change marginally (thus the initial guesses would be similar), 68 | // and can increase computation time a lot, since the algorithm will converge in just a couple of iterations. 69 | // 70 | // Inputs: 71 | // keep Boolean value to indicate, if the continuation mode is used or not. 72 | // If continuation mode is disabled, the default initial values are used for the initial guess 73 | // (by default this is the straight, not bent state of the robot, but they can also be changed - see below). 74 | void setKeepInits(bool keep); 75 | 76 | // This function allows to set and update the TDCR parameters. 77 | // 78 | // Inputs: 79 | // length std::array that holds the length of each of the two segments of the TDCR. 80 | // youngs_modulus Youngs modulus of the backbone of the TDCR. 81 | // routing std::vector that holds the routing position of each tendon of the TDCR expressed as a 3x1 position vector in the local disk frame. 82 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 83 | // number_disks std::array that holds the number of disks for each of the two segments of the TDCR. 84 | // pradius_disks std::array that holds the pitch radius of the disks (distance between tendon routing and backbone) for each of the two segments of the TDCR. 85 | // ro Radius of the backbone of the TDCR. 86 | void setRobotParameters(std::array length, double youngs_modulus, std::vector routing, std::array number_disks, std::array pradius_disks, double ro); 87 | 88 | // This function returns the number of total disks throughout the whole robot structure. 89 | double getNumberOfTotalDisks(); 90 | 91 | // This function allows to set the default initial values that are used as an initial guess for the implemented shooting method. 92 | // 93 | // Inputs: 94 | // inits 3nx1 vector containing the default values that are used as an initial guess for the values (beta, gamma, epsilon) for each subsegment, if continuation mode is disabled. 95 | void setDefaultInitValues(Eigen::MatrixXd inits); 96 | 97 | // This function returns the last values final values of the shooting method for the last solution of the forward kinematics. 98 | // It returns 3nx1 vector containing the last values (beta, gamma, epsilon) for each subsegment. 99 | Eigen::MatrixXd getFinalInitValues(); 100 | 101 | // This function returns a double value containing the least squares resudial of the boundary conditions for the last solution of the forward kinematics. 102 | double getFinalResudial(); 103 | 104 | private: 105 | 106 | // Member function returning the transformation matrix between two disks indexed by idx_from and idx_to. 107 | Eigen::Matrix4d getDiskTransform(Eigen::MatrixXd var, Eigen::VectorXd length_ss, int idx_from, int idx_to); 108 | 109 | // Member variables 110 | std::array m_length; 111 | double m_youngs_modulus; 112 | std::vector m_routing; 113 | std::array m_number_disks; 114 | std::array m_pradius_disks; 115 | double m_ro; 116 | 117 | bool m_keep_inits; 118 | 119 | Eigen::Vector3d m_f_ext; 120 | Eigen::Vector3d m_l_ext; 121 | 122 | Eigen::Matrix m_tau; 123 | 124 | Eigen::MatrixXd m_last_inits; 125 | Eigen::MatrixXd m_default_inits; 126 | 127 | double m_residual_error; 128 | 129 | }; 130 | 131 | #endif // PIECEWISECONSTANTCURVATUREMODEL_H 132 | -------------------------------------------------------------------------------- /c++/include/pseudorigidbodymodel.h: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | #ifndef PSEUDORIGIDBODYMODEL_H 15 | #define PSEUDORIGIDBODYMODEL_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | // This class implements a Pseudo Rigid Body model to solve the forward kinematics of a TDCR. 33 | // This code is written for a two segment TDCR with to three tendons per segment subject to an external load at the tip. 34 | class PseudoRigidBodyModel 35 | { 36 | public: 37 | 38 | // Constructor to set up the Pseudo Rigid Body model with default parameters 39 | PseudoRigidBodyModel(); 40 | 41 | // Simple destructor 42 | virtual ~PseudoRigidBodyModel(); 43 | 44 | // This function runs the forward kinematics for the TDCR. It returns true, if the forward kinematics were solved successfully. 45 | // 46 | // Inputs: 47 | // q 6x1 vector containing the actuation values for each tendon (force in Newton). 48 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 49 | // f_ext 3x1 vector containing the external force acting at the tip/end-effector of the robot expressed in the base frame. 50 | // l_ext 3x1 vector containing the external moment acting at the tip/end-effector of the robot expressed in the base frame. 51 | // 52 | // Outputs: 53 | // diskFrames 4x(4*n) matrix containing the current disk frames (4x4 transformation matrix for each discrete disk) of the robot as a stacked matrix. 54 | bool forwardKinematics(Eigen::MatrixXd &diskFrames, Eigen::MatrixXd q, Eigen::Vector3d f_ext, Eigen::Vector3d l_ext); 55 | 56 | // This function implements the boundary conditions that need to be solved for the Cosserat rod model (force and moment equilibirium at the tip of the robot). 57 | // 58 | // Inputs: 59 | // output 5nx1 vector containing the moment and force equilibirium at the end of each subsegment. 60 | // 61 | // Outputs: 62 | // input 5nx1 vector containing the current values for the pseudo discrete joints of each subsegment. 63 | void get_res(Eigen::MatrixXd &output, Eigen::MatrixXd input); 64 | 65 | 66 | // This function enables a continuation mode. 67 | // That means, that every new run of the forward kinematics will use the final values for these variables obtained from the last forward kinematics solution as the new initial guess. 68 | // This makes sense in cases, where configurations of the robot only change marginally (thus the initial guesses would be similar), 69 | // and can increase computation time a lot, since the algorithm will converge in just a couple of iterations. 70 | // 71 | // Inputs: 72 | // keep Boolean value to indicate, if the continuation mode is used or not. 73 | // If continuation mode is disabled, the default initial values are used for the initial guess 74 | // (by default this is the straight, not bent state of the robot, but they can also be changed - see below). 75 | void setKeepInits(bool keep); 76 | 77 | // This function allows to set and update the TDCR parameters. 78 | // 79 | // Inputs: 80 | // length std::array that holds the length of each of the two segments of the TDCR. 81 | // youngs_modulus Youngs modulus of the backbone of the TDCR. 82 | // routing std::vector that holds the routing position of each tendon of the TDCR expressed as a 3x1 position vector in the local disk frame. 83 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 84 | // number_disks std::array that holds the number of disks for each of the two segments of the TDCR. 85 | // pradius_disks std::array that holds the pitch radius of the disks (distance between tendon routing and backbone) for each of the two segments of the TDCR. 86 | // ro Radius of the backbone of the TDCR. 87 | void setRobotParameters(std::array length, double youngs_modulus, std::vector routing, std::array number_disks, std::array pradius_disks, double ro); 88 | 89 | // This function returns the number of total disks throughout the whole robot structure. 90 | double getNumberOfTotalDisks(); 91 | 92 | // This function returns the number of pseudo discrete joints per subsegment. 93 | double getNumberOfJoints(); 94 | 95 | // This function allows to set the default initial values that are used as an initial guess for the implemented shooting method. 96 | // 97 | // Inputs: 98 | // inits 5nx1 vector containing the default values that are used as an initial guess for the pseudo discrete joint values for each subsegment, if continuation mode is disabled. 99 | void setDefaultInitValues(Eigen::MatrixXd inits); 100 | 101 | // This function returns the last values final values of the shooting method for the last solution of the forward kinematics. 102 | // It returns 5nx1 vector containing the last pseudo discrete joint values for each subsegment. 103 | Eigen::MatrixXd getFinalInitValues(); 104 | 105 | // This function returns a double value containing the least squares resudial of the boundary conditions for the last solution of the forward kinematics. 106 | double getFinalResudial(); 107 | 108 | private: 109 | 110 | // Member function returning the transformation matrix between two disks indexed by idx_from and idx_to. 111 | Eigen::Matrix4d getDiskTransform(Eigen::MatrixXd &Trb, Eigen::MatrixXd var, Eigen::VectorXd length_ss, int idx_from, int idx_to); 112 | 113 | // Member variables 114 | std::array m_length; 115 | double m_youngs_modulus; 116 | std::vector m_routing; 117 | std::array m_number_disks; 118 | std::array m_pradius_disks; 119 | double m_ro; 120 | 121 | bool m_keep_inits; 122 | 123 | Eigen::Vector3d m_f_ext; 124 | Eigen::Vector3d m_l_ext; 125 | 126 | Eigen::Matrix m_tau; 127 | 128 | Eigen::MatrixXd m_last_inits; 129 | Eigen::MatrixXd m_default_inits; 130 | 131 | double m_residual_error; 132 | 133 | Eigen::VectorXd m_joint_pos; 134 | 135 | }; 136 | 137 | #endif // PSEUDORIGIDBODYMODEL_H 138 | -------------------------------------------------------------------------------- /c++/include/subsegmentcosseratrodmodel.h: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | #ifndef SUBSEGMENTCOSSERATRODMODEL_H 15 | #define SUBSEGMENTCOSSERATRODMODEL_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | // This class implements a Subsegment Cosserat Rod model to solve the forward kinematics of a TDCR. 34 | // This code is written for a two segment TDCR with to three tendons per segment subject to an external load at the tip. 35 | // Tendon paths are assumed to be only partially constrained at specific disk locations and are assumed straight between two disks. 36 | // Each subsegment between two disks is solved as an individual Cosserat Rod subject to tendon/external loads and boundary conditions at the transition to neighbouring segments. 37 | class SubsegmentCosseratRodModel 38 | { 39 | public: 40 | 41 | // Constructor to set up the Subsegment Constant Curvature model with default parameters 42 | SubsegmentCosseratRodModel(); 43 | 44 | // Simple destructor 45 | virtual ~SubsegmentCosseratRodModel(); 46 | 47 | // This function runs the forward kinematics for the TDCR. It returns true, if the forward kinematics were solved successfully. 48 | // 49 | // Inputs: 50 | // q 6x1 vector containing the actuation values for each tendon (force in Newton). 51 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 52 | // f_ext 3x1 vector containing the external force acting at the tip/end-effector of the robot expressed in the base frame. 53 | // l_ext 3x1 vector containing the external moment acting at the tip/end-effector of the robot expressed in the base frame. 54 | // 55 | // Outputs: 56 | // diskFrames 4x(4*n) matrix containing the current disk frames (4x4 transformation matrix for each discrete disk) of the robot as a stacked matrix. 57 | bool forwardKinematics(Eigen::MatrixXd &diskFrames, Eigen::MatrixXd q, Eigen::Vector3d f_ext, Eigen::Vector3d l_ext); 58 | 59 | // This function implements the Cosserat Rod ODEs 60 | // 61 | // Inputs: 62 | // y Vector containing the current state. 63 | // 64 | // Outputs: 65 | // dy Vector containing the derivative of the current state. 66 | void cosserat_ode(Eigen::MatrixXd &dy, Eigen::MatrixXd y); 67 | 68 | 69 | // This function implements the boundary conditions that need to be solved for the Cosserat rod model (force and moment equilibirium at the tip of the robot). 70 | // 71 | // Inputs: 72 | // output 6nx1 vector containing the force and moment equilibirium at the end of each subsegment. 73 | // 74 | // Outputs: 75 | // input 6nx1 vector containing the current initial values for u and v at the beginning of each subsegment. 76 | void get_res(Eigen::MatrixXd &output, Eigen::MatrixXd input); 77 | 78 | 79 | // This function enables a continuation mode. 80 | // That means, that every new run of the forward kinematics will use the final values for these variables obtained from the last forward kinematics solution as the new initial guess. 81 | // This makes sense in cases, where configurations of the robot only change marginally (thus the initial guesses would be similar), 82 | // and can increase computation time a lot, since the algorithm will converge in just a couple of iterations. 83 | // 84 | // Inputs: 85 | // keep Boolean value to indicate, if the continuation mode is used or not. 86 | // If continuation mode is disabled, the default initial values are used for the initial guess 87 | // (by default this is the straight, not bent state of the robot, but they can also be changed - see below). 88 | void setKeepInits(bool keep); 89 | 90 | // This function allows to set and update the TDCR parameters. 91 | // 92 | // Inputs: 93 | // length std::array that holds the length of each of the two segments of the TDCR. 94 | // youngs_modulus Youngs modulus of the backbone of the TDCR. 95 | // routing std::vector that holds the routing position of each tendon of the TDCR expressed as a 3x1 position vector in the local disk frame. 96 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 97 | // number_disks std::array that holds the number of disks for each of the two segments of the TDCR. 98 | // pradius_disks std::array that holds the pitch radius of the disks (distance between tendon routing and backbone) for each of the two segments of the TDCR. 99 | // ro Radius of the backbone of the TDCR. 100 | void setRobotParameters(std::array length, double youngs_modulus, std::vector routing, std::array number_disks, std::array pradius_disks, double ro); 101 | 102 | // This function returns the number of total disks throughout the whole robot structure. 103 | double getNumberOfTotalDisks(); 104 | 105 | // This function allows to set the default initial values that are used as an initial guess for the implemented shooting method. 106 | // 107 | // Inputs: 108 | // inits 6nx1 vector containing the default values that are used as an initial guess for u and v at the beginning of each subsegment, if continuation mode is disabled. 109 | void setDefaultInitValues(Eigen::MatrixXd inits); 110 | 111 | // This function returns the last values final values of the shooting method for the last solution of the forward kinematics. 112 | // It returns 6nx1 vector containing the last values for u and v at the beginning of each subsegment. 113 | Eigen::MatrixXd getFinalInitValues(); 114 | 115 | // This function returns a double value containing the least squares resudial of the boundary conditions for the last solution of the forward kinematics. 116 | double getFinalResudial(); 117 | 118 | 119 | private: 120 | // Private member function that solves the initial value problem of the Cosserat Rod ODEs. 121 | void run_IVP(Eigen::MatrixXd &states, Eigen::MatrixXd init_state); 122 | 123 | // Member variables 124 | std::array m_length; 125 | double m_youngs_modulus; 126 | std::vector m_routing; 127 | std::array m_number_disks; 128 | std::array m_pradius_disks; 129 | double m_ro; 130 | 131 | bool m_keep_inits; 132 | 133 | Eigen::Matrix3d m_Kbt; 134 | Eigen::Matrix3d m_Kse; 135 | 136 | Eigen::Vector3d m_f_ext; 137 | Eigen::Vector3d m_l_ext; 138 | 139 | Eigen::Matrix m_tau; 140 | 141 | Eigen::MatrixXd m_last_inits; 142 | Eigen::MatrixXd m_default_inits; 143 | 144 | double m_residual_error; 145 | 146 | Eigen::MatrixXd m_states; 147 | 148 | Eigen::VectorXd m_length_ss; 149 | 150 | int m_current_segment; 151 | 152 | //Stuff for ODE solving 153 | gsl_odeiv2_system m_ode_sys; 154 | const gsl_odeiv2_step_type* m_ode_step_type; 155 | gsl_odeiv2_step* m_ode_step; 156 | gsl_odeiv2_control* m_ode_controller; 157 | gsl_odeiv2_evolve* m_ode_evolve; 158 | }; 159 | 160 | #endif // SUBSEGMENTCOSSERATRODMODEL_H 161 | -------------------------------------------------------------------------------- /c++/include/tendondrivenrobot.h: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | #ifndef TENDONDRIVENROBOT_H 15 | #define TENDONDRIVENROBOT_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | #include "cosseratrodmodel.h" 27 | #include "constantcurvaturemodel.h" 28 | #include "piecewiseconstantcurvaturemodel.h" 29 | #include "pseudorigidbodymodel.h" 30 | #include "subsegmentcosseratrodmodel.h" 31 | 32 | 33 | // This class implements a simple wrapper for the different TDCR modeling approaches. 34 | // Currently, the implementation is tailored to a two segment TDCR with three tendons per segment. 35 | // Each modeling approach is implemented using its own class and can also be used 36 | // individually. 37 | class TendonDrivenRobot 38 | { 39 | public: 40 | // Constructor to set up a TDCR with default parameters 41 | TendonDrivenRobot(); 42 | 43 | // Simple destructor 44 | ~TendonDrivenRobot(); 45 | 46 | // Enumerated type to choose between different model implementations 47 | enum Model { CosseratRod, ConstantCurvature, PiecewiseConstantCurvature, PseudoRigidBody, SubsegmentCosseratRod }; 48 | 49 | 50 | // This function runs the forward kinematics for the TDCR. It returns true, if the forward kinematics were solved successfully. 51 | // 52 | // Inputs: 53 | // q 6x1 vector containing the actuation values for each tendon (either force in Newton or displacements in meter). 54 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 55 | // f_ext 3x1 vector containing the external force acting at the tip/end-effector of the robot expressed in the base frame. 56 | // l_ext 3x1 vector containing the external moment acting at the tip/end-effector of the robot expressed in the base frame. 57 | // model The chosen modeling approach to use for solving the forward kinematics. 58 | // 59 | // Outputs: 60 | // ee_frame 4x4 matrix containing the end-effector pose as a 4x4 frame (R p; 0 0 0 1). 61 | bool forwardKinematics(Eigen::Matrix4d &ee_frame, Eigen::MatrixXd q, Eigen::Vector3d f_ext, Eigen::Vector3d l_ext, Model model); 62 | 63 | 64 | // This function allows to set and update the TDCR parameters. 65 | // 66 | // Inputs: 67 | // length std::array that holds the length of each of the two segments of the TDCR. 68 | // youngs_modulus Youngs modulus of the backbone of the TDCR. 69 | // routing std::vector that holds the routing position of each tendon of the TDCR expressed as a 3x1 position vector in the local disk frame. 70 | // First three entries belong to the first segment, while the last three entries belong to the second segment. 71 | // number_disks std::array that holds the number of disks for each of the two segments of the TDCR. 72 | // pradius_disks std::array that holds the pitch radius of the disks (distance between tendon routing and backbone) for each of the two segments of the TDCR. 73 | // ro Radius of the backbone of the TDCR. 74 | // two_tendons Specifies, if only two tendons for each segment are employed and actuated. 75 | // Only affects the implementation of the Constant Curvature modeling approach (details can be found there). 76 | void setRobotParameters(std::array length, double youngs_modulus, std::vector routing, std::array number_disks, std::array pradius_disks, double ro, bool two_tendons); 77 | 78 | // This function enables a continuation mode for the modeling approaches that are utilizing an optimization scheme that is based on an initial guess. 79 | // That means, that every new run of the forward kinematics will use the final values for these variables obtained from the last forward kinematics solution as the new initial guess. 80 | // This makes sense in cases, where configurations of the robot only change marginally (thus the initial guesses would be similar), 81 | // and can increase computation time a lot, since the algorithm will converge in just a couple of iterations. 82 | // 83 | // Inputs: 84 | // keep Boolean value to indicate, if the continuation mode is used or not. 85 | // If continuation mode is disabled, the default initial state of the robot (straight, no bending) is assumed for the initial guess. 86 | void keepInits(bool keep); 87 | 88 | // This function returns the current end-effector/tip frame (4x4 transformation matrix) of the robot (based on the last forward kinematics computation). 89 | Eigen::Matrix4d getEEFrame(); 90 | 91 | // This function returns the current configuration/actuation values (6x1 vector) for each tendon of the TDCR (based on the last q that was handed to the forward kinematics computation). 92 | Eigen::MatrixXd getCurrentConfig(); 93 | 94 | // This function returns the current disk frames (4x4 transformation matrix for each discrete disk) of the robot (based on the last forward kinematics computation) as a stacked matrix. 95 | // The returned matrix of size 4x(4*n), where n is the total number of disks and every nth four columns belong to the nth disk frame. 96 | Eigen::MatrixXd getDiskFrames(); 97 | 98 | // This function returns the current tendon displacements (6x1 vector) for each tendon of the robot (based on the last forward kinematics computation). 99 | // This is done by evaluating the current shape of the robot and computing the length of each tendon based on the current disk frames. 100 | // This function can be used to obtain tendon displacements from the TDCR if it has currently been operated using a modeling approach that is based on tendon force acutation. 101 | Eigen::MatrixXd getTendonDisplacements(); 102 | 103 | private: 104 | 105 | // Pointers to the different modeling approaches 106 | CosseratRodModel* mp_cr_model; 107 | ConstantCurvatureModel* mp_cc_model; 108 | PiecewiseConstantCurvatureModel* mp_pcc_model; 109 | PseudoRigidBodyModel* mp_prb_model; 110 | SubsegmentCosseratRodModel* mp_sscr_model; 111 | 112 | // Member variables 113 | Eigen::MatrixXd m_actuation_values; 114 | Eigen::Vector3d m_ext_forces; 115 | Eigen::Vector3d m_ext_moments; 116 | Eigen::Matrix4d m_ee_frame; 117 | Eigen::MatrixXd m_disk_frames; 118 | bool m_keep_inits; 119 | 120 | //Robot Parameter 121 | std::array m_length; 122 | double m_youngs_modulus; 123 | std::vector m_routing; 124 | std::array m_number_disks; 125 | std::array m_pradius_disks; 126 | double m_ro; 127 | bool m_two_tendons; 128 | 129 | }; 130 | 131 | #endif // TENDONDRIVENROBOT_H 132 | -------------------------------------------------------------------------------- /c++/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | //includes 15 | #include "tendondrivenrobot.h" 16 | #include 17 | #include 18 | 19 | 20 | int main(int argc, char **argv) 21 | { 22 | //Create tendon driven robot with default parameters 23 | TendonDrivenRobot robot; 24 | 25 | //Example on defining custom parameters for the robot 26 | 27 | //Length per segment 28 | std::array length; 29 | length[0] = 0.2; 30 | length[1] = 0.2; 31 | 32 | //Youngs modulus 33 | double youngs_modulus = 54*1e9; 34 | 35 | //Number of disks per segment 36 | std::array number_disks; 37 | number_disks[0] = 10; 38 | number_disks[1] = 10; 39 | 40 | //Pitch radius per segment (distance from tendons to backbone) 41 | std::array pradius_disks; 42 | pradius_disks[0] = 10*1e-3; 43 | pradius_disks[1] = 10*1e-3; 44 | 45 | 46 | //Radius of the backbone 47 | double ro = 0.7*1e-3; 48 | 49 | 50 | 51 | //Tendon routing expressed as a three-dimensional position vector in the local (disk) frame of the robot for each tendon 52 | //First three tendons for first segment, last three tendons for second segment 53 | //Stored in a std::vector 54 | std::vector routing; 55 | 56 | Eigen::Vector3d tendon1; 57 | tendon1 << 0, 58 | pradius_disks[0], 59 | 0; 60 | 61 | Eigen::Vector3d tendon2; 62 | tendon2 << pradius_disks[0]*std::cos(-M_PI/6), 63 | pradius_disks[0]*std::sin(-M_PI/6), 64 | 0; 65 | Eigen::Vector3d tendon3; 66 | tendon3 << pradius_disks[0]*std::cos(7*M_PI/6), 67 | pradius_disks[0]*std::sin(7*M_PI/6), 68 | 0; 69 | 70 | routing.push_back(tendon1); 71 | routing.push_back(tendon2); 72 | routing.push_back(tendon3); 73 | 74 | tendon1 << 0, 75 | pradius_disks[1], 76 | 0; 77 | 78 | tendon2 << pradius_disks[1]*std::cos(-M_PI/6), 79 | pradius_disks[1]*std::sin(-M_PI/6), 80 | 0; 81 | 82 | tendon3 << pradius_disks[1]*std::cos(7*M_PI/6), 83 | pradius_disks[1]*std::sin(7*M_PI/6), 84 | 0; 85 | 86 | routing.push_back(tendon1); 87 | routing.push_back(tendon2); 88 | routing.push_back(tendon3); 89 | 90 | //Update the robot parameters 91 | robot.setRobotParameters(length,youngs_modulus,routing,number_disks,pradius_disks,ro,false); 92 | 93 | //Defining the actuation values 94 | //Every entry in q defined actuation of one tendon in the order used above 95 | //q is tendon force in Newton for static models tendon displacement in meter for Constant Curvature model 96 | Eigen::MatrixXd q; 97 | q.resize(6,1); 98 | q << 4, 99 | 0, 100 | 0, 101 | 0, 102 | 2, 103 | 0; 104 | 105 | //External force at tip set to zero 106 | Eigen::Vector3d f_ext; 107 | f_ext.setZero(); 108 | 109 | //External monment at tip set to zero 110 | Eigen::Vector3d l_ext; 111 | l_ext.setZero(); 112 | 113 | int success; 114 | 115 | //Variable to store the tip frame 116 | Eigen::Matrix4d ee_frame; 117 | 118 | //Running the forward kinematics using the different models 119 | success = robot.forwardKinematics(ee_frame,q,f_ext,l_ext,TendonDrivenRobot::Model::CosseratRod); 120 | if(success) 121 | std::cout << "Tip frame calculated using Cosserat Rod:" < length, std::array number_disks, std::array pradius_disks, bool two_tendons) 30 | { 31 | 32 | //Update parameters 33 | m_length[0] = length[0]; 34 | m_length[1] = length[1]; 35 | m_number_disks[0] = number_disks[0]; 36 | m_number_disks[1] = number_disks[1]; 37 | m_pradius_disks[0] = pradius_disks[0]; 38 | m_pradius_disks[1] = pradius_disks[1]; 39 | m_two_tendons = two_tendons; 40 | 41 | } 42 | 43 | ConstantCurvatureModel::~ConstantCurvatureModel() 44 | { 45 | } 46 | 47 | 48 | bool ConstantCurvatureModel::forwardKinematics(Eigen::MatrixXd &diskFrames, Eigen::MatrixXd q) 49 | { 50 | // Compute arc parameters for each segment 51 | double kappa1, kappa2; 52 | double phi1, phi2; 53 | double l1, l2; 54 | 55 | // Calculate tendon lengths 56 | Eigen::Vector3d l_t1; 57 | l_t1 << m_length[0] - q(0), 58 | m_length[0] - q(1), 59 | m_length[0] - q(2); 60 | 61 | Eigen::Vector3d l_t2; 62 | l_t2 << m_length[1] - q(3), 63 | m_length[1] - q(4), 64 | m_length[1] - q(5); 65 | 66 | 67 | // First segment 68 | 69 | // Check if tendon is straight 70 | double temp =std::sqrt(l_t1(0)*l_t1(0) + l_t1(1)*l_t1(1) + l_t1(2)*l_t1(2) - l_t1(0)*l_t1(1) - l_t1(0)*l_t1(2) - l_t1(1)*l_t1(2)); 71 | 72 | if(m_two_tendons == false) 73 | { 74 | 75 | if(temp == 0 || std::isnan(temp)) 76 | { 77 | kappa1 = 0; 78 | phi1 = 0; 79 | l1 = l_t1(0); 80 | } 81 | else 82 | { 83 | // Compute arc parameters 84 | kappa1 = 2*temp/(m_pradius_disks[0]*(l_t1(0) + l_t1(1) + l_t1(2))); 85 | phi1 = std::atan2(sqrt(3)*(l_t1(1)+l_t1(2)-2*l_t1(0)),3*(l_t1(2)-l_t1(1))); 86 | l1 = ((m_number_disks[0]*m_pradius_disks[0]*(l_t1(0)+l_t1(1)+l_t1(2)))/temp)*std::asin(temp/(3*m_number_disks[0]*m_pradius_disks[0])); 87 | } 88 | 89 | // Second segment 90 | 91 | // Check if tendon is straight 92 | temp =std::sqrt(l_t2(0)*l_t2(0) + l_t2(1)*l_t2(1) + l_t2(2)*l_t2(2) - l_t2(0)*l_t2(1) - l_t2(0)*l_t2(2) - l_t2(1)*l_t2(2)); 93 | 94 | if(temp == 0 || std::isnan(temp)) 95 | { 96 | kappa2 = 0; 97 | phi2 = 0; 98 | l2 = l_t2(0); 99 | } 100 | else 101 | { 102 | // Compute arc parameters 103 | kappa2 = 2*temp/(m_pradius_disks[1]*(l_t2(0) + l_t2(1) + l_t2(2))); 104 | phi2 = std::atan2(sqrt(3)*(l_t2(1)+l_t2(2)-2*l_t2(0)),3*(l_t2(2)-l_t2(1))); 105 | l2 = ((m_number_disks[1]*m_pradius_disks[1]*(l_t2(0)+l_t2(1)+l_t2(2)))/temp)*std::asin(temp/(3*m_number_disks[1]*m_pradius_disks[1])); 106 | } 107 | } 108 | else 109 | { 110 | if(q(0) > 0) 111 | { 112 | phi1 = M_PI/2; 113 | kappa1 = q(0)/(m_pradius_disks[0]*m_length[0]); 114 | 115 | } 116 | else if(q(1) > 0) 117 | { 118 | phi1 = 3*M_PI/2; 119 | kappa1 = q(1)/(m_pradius_disks[0]*m_length[0]); 120 | } 121 | else 122 | { 123 | phi1 = 0; 124 | kappa1 = 0; 125 | } 126 | 127 | 128 | if(q(3) > 0) 129 | { 130 | phi2 = M_PI/2; 131 | kappa2 = q(3)/(m_pradius_disks[1]*m_length[1]); 132 | 133 | } 134 | else if(q(4) > 0) 135 | { 136 | phi2 = 3*M_PI/2; 137 | kappa2 = q(4)/(m_pradius_disks[1]*m_length[1]); 138 | } 139 | else 140 | { 141 | phi2 = 0; 142 | kappa2 = 0; 143 | } 144 | } 145 | 146 | //Calculate disk transformation matrices using robot independent mapping 147 | 148 | //First segment 149 | Eigen::MatrixXd disk_frames1; 150 | disk_frames1.resize(4,4*(m_number_disks[0]+1)); 151 | 152 | //First disk is identity (base disk) 153 | disk_frames1.block(0,0,4,4) = Eigen::Matrix4d::Identity(); 154 | 155 | Eigen::Matrix4d T; 156 | double c_p = std::cos(phi1); 157 | double s_p = std::sin(phi1); 158 | for(int i = 1; i <= m_number_disks[0]; i ++) 159 | { 160 | double s = i*m_length[0]/m_number_disks[0]; 161 | double c_ks = std::cos(kappa1*s); 162 | double s_ks = std::sin(kappa1*s); 163 | if(kappa1 == 0) 164 | { 165 | T << c_p*c_ks, -s_p, c_p*s_ks, 0, 166 | s_p*c_ks, c_p, s_p*s_ks, 0, 167 | -s_ks, 0, c_ks, s, 168 | 0, 0, 0, 1; 169 | } 170 | else 171 | { 172 | T << c_p*c_ks, -s_p, c_p*s_ks, c_p*(1-c_ks)/kappa1, 173 | s_p*c_ks, c_p, s_p*s_ks, s_p*(1-c_ks)/kappa1, 174 | -s_ks, 0, c_ks, s_ks/kappa1, 175 | 0, 0, 0, 1; 176 | } 177 | //Apply Transformation to fix the z-axis (local z-orientation stays fixed for TDCR) 178 | Eigen::Matrix4d rot_phi; 179 | rot_phi << std::cos(phi1), std::sin(phi1), 0, 0, 180 | -std::sin(phi1), std::cos(phi1),0, 0, 181 | 0, 0, 1, 0, 182 | 0, 0, 0, 1; 183 | T = T*rot_phi; 184 | disk_frames1.block(0,4*i,4,4) = T; 185 | 186 | } 187 | 188 | //Second segment 189 | Eigen::MatrixXd disk_frames2; 190 | disk_frames2.resize(4,4*m_number_disks[1]); 191 | 192 | c_p = std::cos(phi2); 193 | s_p = std::sin(phi2); 194 | for(int i = 1; i <= m_number_disks[1]; i ++) 195 | { 196 | double s = i*m_length[1]/m_number_disks[1]; 197 | double c_ks = std::cos(kappa2*s); 198 | double s_ks = std::sin(kappa2*s); 199 | if(kappa2 == 0) 200 | { 201 | T << c_p*c_ks, -s_p, c_p*s_ks, 0, 202 | s_p*c_ks, c_p, s_p*s_ks, 0, 203 | -s_ks, 0, c_ks, s, 204 | 0, 0, 0, 1; 205 | } 206 | else 207 | { 208 | T << c_p*c_ks, -s_p, c_p*s_ks, c_p*(1-c_ks)/kappa2, 209 | s_p*c_ks, c_p, s_p*s_ks, s_p*(1-c_ks)/kappa2, 210 | -s_ks, 0, c_ks, s_ks/kappa2, 211 | 0, 0, 0, 1; 212 | } 213 | //Apply Transformation to fix the z-axis 214 | Eigen::Matrix4d rot_phi; 215 | rot_phi << std::cos(phi2), std::sin(phi2), 0, 0, 216 | -std::sin(phi2), std::cos(phi2),0, 0, 217 | 0, 0, 1, 0, 218 | 0, 0, 0, 1; 219 | T = T*rot_phi; 220 | //Apply transformation of first segment 221 | T = disk_frames1.block(0,4*m_number_disks[0],4,4)*T; 222 | 223 | disk_frames2.block(0,4*(i-1),4,4) = T; 224 | 225 | } 226 | 227 | diskFrames.resize(4,disk_frames1.cols()+disk_frames2.cols()); 228 | 229 | diskFrames << disk_frames1, disk_frames2; 230 | 231 | return true; 232 | 233 | } 234 | 235 | 236 | 237 | 238 | -------------------------------------------------------------------------------- /c++/src/cosseratrodmodel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | #include "cosseratrodmodel.h" 15 | 16 | // Wrapper function for ODEs for GNU::GSL 17 | int differential_equations_cr(double t, const double input[2], double deriv[2], void *param) 18 | { 19 | 20 | (void)(t); /* avoid unused parameter warning */ 21 | 22 | Eigen::Matrix y; 23 | Eigen::MatrixXd dy; 24 | 25 | //Map input to y 26 | for(int i = 0; i < 19; i++) 27 | { 28 | y(0,i) = input[i]; 29 | } 30 | 31 | 32 | CosseratRodModel *robot = (CosseratRodModel*)param; 33 | robot->cosserat_ode(dy,y); 34 | 35 | //Map dy to deriv 36 | for(int i = 0; i < 19; i++) 37 | { 38 | deriv[i] = dy(0,i); 39 | } 40 | 41 | return GSL_SUCCESS; 42 | } 43 | 44 | // Wrapper function for non-linear boundary conditions for GNU::GSL 45 | int evaluate_boundary_conditions_cr(const gsl_vector * x, void *param, gsl_vector * f) 46 | { 47 | 48 | Eigen::Matrix inputs; 49 | 50 | Eigen::MatrixXd outputs; 51 | 52 | for(int i = 0; i < 6; i++) 53 | { 54 | inputs(i,0) = gsl_vector_get (x, i); 55 | } 56 | 57 | CosseratRodModel *model = (CosseratRodModel*)param; 58 | model->get_res(outputs, inputs); 59 | 60 | for(int i = 0; i < 6; i++) 61 | { 62 | 63 | gsl_vector_set (f, i, outputs(i,0)); 64 | } 65 | 66 | 67 | return GSL_SUCCESS; 68 | } 69 | 70 | 71 | // Callback function that can be used during non-linear squares solving 72 | void 73 | callback_cr(const size_t iter, void *params, 74 | const gsl_multifit_nlinear_workspace *w) 75 | { 76 | 77 | //Pring current iteration 78 | std::cout << "Iteration " << iter << std::endl; 79 | 80 | gsl_vector * x = gsl_multifit_nlinear_position(w); 81 | gsl_vector * r = gsl_multifit_nlinear_residual(w); 82 | 83 | std::cout << "Current values:" << std::endl; 84 | for(int i = 0; i < 6; i++) 85 | { 86 | std::cout << " " << gsl_vector_get(x, i); 87 | } 88 | std::cout << std::endl << "Current residuals:" << std::endl; 89 | for(int i = 0; i < 6; i++) 90 | { 91 | std::cout << " " << gsl_vector_get(r, i); 92 | } 93 | 94 | double chisq; 95 | gsl_blas_ddot(r, r, &chisq); 96 | 97 | std::cout << std::endl << "Current cost: "<< chisq << std::endl << std::endl; 98 | 99 | } 100 | 101 | CosseratRodModel::CosseratRodModel() 102 | { 103 | //Initialize member variables and set up default parameters for TDCR 104 | m_length[0] = 0.1; 105 | m_length[1] = 0.1; 106 | m_youngs_modulus = 60e9; 107 | m_number_disks[0] = 10; 108 | m_number_disks[1] = 10; 109 | m_pradius_disks[0] = 0.008; 110 | m_pradius_disks[1] = 0.006; 111 | m_ro = 0.0005; 112 | 113 | std::vector routing; 114 | 115 | Eigen::Vector3d tendon1; 116 | tendon1 << 0, 117 | m_pradius_disks[0], 118 | 0; 119 | 120 | Eigen::Vector3d tendon2; 121 | tendon2 << m_pradius_disks[0]*std::cos(-M_PI/6), 122 | m_pradius_disks[0]*std::sin(-M_PI/6), 123 | 0; 124 | Eigen::Vector3d tendon3; 125 | tendon3 << m_pradius_disks[0]*std::cos(7*M_PI/6), 126 | m_pradius_disks[0]*std::sin(7*M_PI/6), 127 | 0; 128 | 129 | routing.push_back(tendon1); 130 | routing.push_back(tendon2); 131 | routing.push_back(tendon3); 132 | 133 | tendon1 << 0, 134 | m_pradius_disks[1], 135 | 0; 136 | 137 | tendon2 << m_pradius_disks[1]*std::cos(-M_PI/6), 138 | m_pradius_disks[1]*std::sin(-M_PI/6), 139 | 0; 140 | 141 | tendon3 << m_pradius_disks[1]*std::cos(7*M_PI/6), 142 | m_pradius_disks[1]*std::sin(7*M_PI/6), 143 | 0; 144 | 145 | routing.push_back(tendon1); 146 | routing.push_back(tendon2); 147 | routing.push_back(tendon3); 148 | 149 | m_routing = routing; 150 | 151 | //Set stiffness matrices 152 | double G = m_youngs_modulus/(2*(1.3)); 153 | double I = 0.25*M_PI*(m_ro*m_ro*m_ro*m_ro); 154 | double A = M_PI*(m_ro*m_ro); 155 | 156 | m_Kse << G*A, 0, 0, 157 | 0, G*A, 0, 158 | 0, 0, m_youngs_modulus*A; 159 | 160 | m_Kbt << m_youngs_modulus*I, 0, 0, 161 | 0, m_youngs_modulus*I, 0, 162 | 0, 0, 2*G*I; 163 | 164 | 165 | 166 | //Setup ODE stuff 167 | m_ode_sys.function = differential_equations_cr; 168 | m_ode_sys.jacobian = nullptr; 169 | m_ode_sys.dimension = 19; 170 | m_ode_sys.params = this; 171 | 172 | m_ode_step_type = gsl_odeiv2_step_rkf45; 173 | 174 | m_ode_step = gsl_odeiv2_step_alloc (m_ode_step_type, 19); 175 | m_ode_controller = gsl_odeiv2_control_y_new (1e-6, 1e-3); 176 | m_ode_evolve = gsl_odeiv2_evolve_alloc (19); 177 | 178 | m_keep_inits = false; 179 | m_default_inits.resize(1,6); 180 | m_default_inits << 0, 0, 1, 0, 0, 0; 181 | m_last_inits = m_default_inits; 182 | 183 | m_current_segment = 1; 184 | 185 | m_tau.setZero(); 186 | 187 | } 188 | 189 | void CosseratRodModel::setRobotParameters(std::array length, double youngs_modulus, std::vector routing, std::array number_disks, std::array pradius_disks, double ro) 190 | { 191 | //Update parameters 192 | m_length[0] = length[0]; 193 | m_length[1] = length[1]; 194 | m_youngs_modulus = youngs_modulus; 195 | m_number_disks[0] = number_disks[0]; 196 | m_number_disks[1] = number_disks[1]; 197 | m_pradius_disks[0] = pradius_disks[0]; 198 | m_pradius_disks[1] = pradius_disks[1]; 199 | m_ro = ro; 200 | m_routing = routing; 201 | 202 | //Set stiffness matrices 203 | double G = m_youngs_modulus/(2*(1.3)); 204 | double I = 0.25*M_PI*(m_ro*m_ro*m_ro*m_ro); 205 | double A = M_PI*(m_ro*m_ro); 206 | 207 | m_Kse << G*A, 0, 0, 208 | 0, G*A, 0, 209 | 0, 0, m_youngs_modulus*A; 210 | 211 | m_Kbt << m_youngs_modulus*I, 0, 0, 212 | 0, m_youngs_modulus*I, 0, 213 | 0, 0, 2*G*I; 214 | 215 | } 216 | 217 | void CosseratRodModel::setDefaultInitValues(Eigen::MatrixXd inits) 218 | { 219 | m_default_inits = inits; 220 | } 221 | 222 | Eigen::MatrixXd CosseratRodModel::getFinalInitValues() 223 | { 224 | return m_last_inits; 225 | } 226 | 227 | double CosseratRodModel::getFinalResudial() 228 | { 229 | return m_residual_error; 230 | } 231 | 232 | CosseratRodModel::~CosseratRodModel() 233 | { 234 | gsl_odeiv2_evolve_free (m_ode_evolve); 235 | gsl_odeiv2_control_free (m_ode_controller); 236 | gsl_odeiv2_step_free (m_ode_step); 237 | } 238 | 239 | void CosseratRodModel::run_IVP(Eigen::MatrixXd &states, Eigen::MatrixXd init_state) 240 | { 241 | 242 | double t = 0; 243 | double t1 = m_length[m_current_segment-1]; 244 | double y[19]; 245 | 246 | //Variable step length 247 | //double h = 0.005; 248 | //Fixed step length 249 | double h = t1/m_number_disks[m_current_segment-1]; 250 | double h_init = t1/m_number_disks[m_current_segment-1]; 251 | 252 | 253 | 254 | 255 | 256 | for(int i = 0; i < 19; i++) 257 | { 258 | y[i] = init_state(0,i); 259 | } 260 | 261 | 262 | //Reset ODE stuff from previous run 263 | 264 | std::vector y_stacked; 265 | for(int i = 0; i < m_number_disks[m_current_segment-1]; i++) 266 | { 267 | h = h_init; 268 | t1 = t+h_init; 269 | int status; 270 | while (t < t1) 271 | { 272 | //Variable step length 273 | status = gsl_odeiv2_evolve_apply (m_ode_evolve, m_ode_controller, m_ode_step, &m_ode_sys, &t, t1, &h, y); 274 | //Fixed step length 275 | //status = gsl_odeiv2_evolve_apply_fixed_step (m_ode_evolve, m_ode_controller, m_ode_step, &m_ode_sys, &t, h, y); 276 | 277 | if (status != GSL_SUCCESS) 278 | break; 279 | 280 | 281 | 282 | } 283 | if (!(status != GSL_SUCCESS)) 284 | { 285 | Eigen::Matrix temp; 286 | for(int k = 0; k < 19; k++) 287 | { 288 | temp(k) = y[k]; 289 | } 290 | 291 | if(t <= t1) //Only add the last step if it's still within the given boundaries 292 | y_stacked.push_back(temp); 293 | } 294 | } 295 | 296 | 297 | if(m_current_segment == 1) // If first segment add the initial state as first disk (disk that is fixed at the base) 298 | { 299 | states.resize(y_stacked.size()+1,19); 300 | states.setZero(); 301 | states.row(0) = init_state; 302 | for(int i = 0; i < y_stacked.size(); i++) 303 | { 304 | states.row(i+1) = y_stacked.at(i).transpose(); 305 | } 306 | } 307 | else // Not needed for second segment, last disk of first segment is the same as first for second segment 308 | { 309 | states.resize(y_stacked.size(),19); 310 | states.setZero(); 311 | for(int i = 0; i < y_stacked.size(); i++) 312 | { 313 | states.row(i) = y_stacked.at(i).transpose(); 314 | } 315 | } 316 | 317 | //Reset stuff for next run 318 | gsl_odeiv2_step_reset(m_ode_step); 319 | gsl_odeiv2_evolve_reset(m_ode_evolve); 320 | 321 | } 322 | 323 | bool CosseratRodModel::forwardKinematics(Eigen::MatrixXd &diskFrames, Eigen::MatrixXd q, Eigen::Vector3d f_ext, Eigen::Vector3d l_ext) 324 | { 325 | m_tau = q; 326 | m_f_ext = f_ext; 327 | m_l_ext = l_ext; 328 | 329 | // Run BVP Shooting method 330 | Eigen::MatrixXd inits; 331 | 332 | if(m_keep_inits) 333 | { 334 | inits = m_last_inits; 335 | } 336 | else 337 | { 338 | inits = m_default_inits; 339 | } 340 | 341 | const size_t n = 6; //Number of equations (boundary conditions) 342 | const size_t p = 6; //Number of parameters (initial guesses) 343 | gsl_vector *x0 = gsl_vector_alloc(p); 344 | gsl_multifit_nlinear_fdf fdf; 345 | gsl_multifit_nlinear_parameters fdf_params = gsl_multifit_nlinear_default_parameters(); 346 | 347 | fdf.f = evaluate_boundary_conditions_cr; 348 | fdf.df = NULL; //Provide your own Jacobian 349 | fdf.fvv = NULL; //Provide your own Hessian 350 | fdf.n = n; 351 | fdf.p = p; 352 | fdf.params = this; 353 | 354 | //Set initial values 355 | for(int i = 0; i < 6; i++) 356 | { 357 | gsl_vector_set(x0,i,inits(0,i)); 358 | } 359 | 360 | //Choose solver (Levenberg-Marquardt) 361 | fdf_params.trs = gsl_multifit_nlinear_trs_lm; 362 | //Choose solver (Levenberg-Marquardt with acceleration) 363 | //fdf_params.trs = gsl_multifit_nlinear_trs_lmaccel; 364 | //Choose solver (dogleg) 365 | //fdf_params.trs = gsl_multifit_nlinear_trs_dogleg; 366 | 367 | //Set scaling parameter 368 | fdf_params.scale = gsl_multifit_nlinear_scale_more; 369 | 370 | //Set solver for trust region 371 | fdf_params.solver = gsl_multifit_nlinear_solver_qr; 372 | 373 | //Set trust region factor 374 | fdf_params.factor_up = 2.5; //default 3 375 | fdf_params.factor_down = 4; //default 2 376 | 377 | 378 | ///---Solve the system--- 379 | const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust; 380 | const size_t max_iter = 1000; 381 | const double xtol = 1.0e-10; 382 | const double gtol = 1.0e-10; 383 | const double ftol = 1.0e-10; 384 | gsl_multifit_nlinear_workspace *work = gsl_multifit_nlinear_alloc(T,&fdf_params, n, p); 385 | gsl_vector * f = gsl_multifit_nlinear_residual(work); 386 | gsl_vector * x = gsl_multifit_nlinear_position(work); 387 | int info; 388 | double chisq0, chisq, rcond; 389 | 390 | /* initialize solver */ 391 | gsl_multifit_nlinear_init(x0, &fdf, work); 392 | 393 | /* store initial cost */ 394 | gsl_blas_ddot(f, f, &chisq0); 395 | 396 | /* iterate until convergence */ 397 | //gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,callback_cr, NULL, &info, work); 398 | //Without Callback 399 | gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,NULL, NULL, &info, work); 400 | 401 | /* store final cost */ 402 | gsl_blas_ddot(f, f, &chisq); 403 | m_residual_error = chisq; 404 | 405 | /* store cond(J(x)) */ 406 | gsl_multifit_nlinear_rcond(&rcond, work); 407 | 408 | ///---End Solve the system--- 409 | 410 | Eigen::Matrix final_values; 411 | Eigen::Matrix final_boundcon; 412 | 413 | 414 | for(int i = 0; i < 6; i ++) 415 | { 416 | final_values(0,i) = gsl_vector_get (x, i); 417 | final_boundcon(0,i) = gsl_vector_get (f, i); 418 | } 419 | 420 | m_last_inits = final_values; 421 | gsl_vector_free (x0); 422 | gsl_multifit_nlinear_free(work); 423 | 424 | //Get the last (initial) states 425 | Eigen::MatrixXd states1 = m_states1; 426 | Eigen::MatrixXd states2 = m_states2; 427 | 428 | Eigen::MatrixXd tempDisks; 429 | 430 | if(states1.rows() == 0 || states1.rows() == 1) 431 | return false; 432 | 433 | tempDisks.resize(states1.rows()+states2.rows(),12); 434 | tempDisks << states1.block(0,0,states1.rows(),12), 435 | states2.block(0,0,states2.rows(),12); 436 | 437 | 438 | diskFrames.resize(4,4*tempDisks.rows()); 439 | 440 | for(int i = 0; i < tempDisks.rows(); i++) 441 | { 442 | Eigen::Matrix3d R; 443 | Eigen::Vector3d p; 444 | R << tempDisks.row(i).block(0,3,1,3), 445 | tempDisks.row(i).block(0,6,1,3), 446 | tempDisks.row(i).block(0,9,1,3); 447 | p = tempDisks.row(i).block(0,0,1,3).transpose(); 448 | Eigen::Matrix4d frame; 449 | frame << R, p, 450 | 0,0,0,1; 451 | diskFrames.block(0,4*i,4,4) = frame; 452 | } 453 | 454 | 455 | 456 | if(m_residual_error > 1e-6) 457 | { 458 | return false; 459 | } 460 | 461 | return true; 462 | 463 | } 464 | 465 | 466 | void CosseratRodModel::cosserat_ode(Eigen::MatrixXd &dy, Eigen::MatrixXd y) 467 | { 468 | dy.resize(1,19); 469 | dy.setZero(); 470 | 471 | 472 | Eigen::Vector3d u = y.block(0,15,1,3).transpose(); 473 | Eigen::Vector3d v = y.block(0,12,1,3).transpose(); 474 | 475 | 476 | Eigen::Matrix3d u_hat; 477 | u_hat << 0, -u(2), u(1), 478 | u(2), 0, -u(0), 479 | -u(1), u(0), 0; 480 | 481 | Eigen::Matrix3d v_hat; 482 | v_hat << 0, -v(2), v(1), 483 | v(2), 0, -v(0), 484 | -v(1), v(0), 0; 485 | 486 | Eigen::Matrix3d R; 487 | R << y.block(0,3,1,3), 488 | y.block(0,6,1,3), 489 | y.block(0,9,1,3); 490 | 491 | 492 | std::vector r_hat; 493 | for(int i = 0; i < 6; i++) // Calculate stuff for each tendon 494 | { 495 | Eigen::Matrix3d temp; 496 | temp << 0, -m_routing.at(i)(2), m_routing.at(i)(1), 497 | m_routing.at(i)(2), 0, -m_routing.at(i)(0), 498 | -m_routing.at(i)(1), m_routing.at(i)(0), 0; 499 | 500 | r_hat.push_back(temp); 501 | } 502 | 503 | Eigen::MatrixXd pb_dot; 504 | pb_dot.resize(3,6); 505 | 506 | std::vector A; 507 | 508 | Eigen::Matrix3d A_total; 509 | A_total.setZero(); 510 | 511 | for(int i = 0; i < 6; i++) 512 | { 513 | pb_dot.block(0,i,3,1) = u_hat*m_routing.at(i) + v; 514 | 515 | Eigen::Matrix3d pb_dot_hat; 516 | pb_dot_hat <<0, -pb_dot(2,i), pb_dot(1,i), 517 | pb_dot(2,i), 0, -pb_dot(0,i), 518 | -pb_dot(1,i), pb_dot(0,i), 0; 519 | 520 | A.push_back(-(m_tau(i)/std::pow(pb_dot.block(0,i,3,1).norm(),3))*pb_dot_hat*pb_dot_hat); 521 | 522 | if(m_current_segment == 1 || i > 2) // Take into account all tendons for first segment, but only last four for second segment 523 | A_total += A.at(i); 524 | } 525 | 526 | std::vector B; 527 | 528 | Eigen::Matrix3d B_total; 529 | B_total.setZero(); 530 | 531 | for(int i = 0; i < 6; i++) 532 | { 533 | B.push_back(r_hat.at(i)*A.at(i)); 534 | if(m_current_segment == 1 || i > 2) // Take into account all tendons for first segment, but only last four for second segment 535 | B_total += B.at(i); 536 | } 537 | 538 | Eigen::Matrix3d G; 539 | G.setZero(); 540 | for(int i = 0; i < 6; i++) 541 | { 542 | if(m_current_segment == 1 || i > 2) // Take into account all tendons for first segment, but only last four for second segment 543 | G += -A.at(i)*r_hat.at(i); 544 | } 545 | 546 | Eigen::Matrix3d H; 547 | H.setZero(); 548 | for(int i = 0; i < 6; i++) 549 | { 550 | if(m_current_segment == 1 || i > 2) // Take into account all tendons for first segment, but only last four for second segment 551 | H += -B.at(i)*r_hat.at(i); 552 | } 553 | 554 | std::vector a; 555 | Eigen::Vector3d a_total; 556 | a_total.setZero(); 557 | for(int i = 0; i < 6; i++) 558 | { 559 | a.push_back(A.at(i)*u_hat*pb_dot.block(0,i,3,1)); 560 | if(m_current_segment == 1 || i > 2) // Take into account all tendons for first segment, but only last four for second segment 561 | a_total += a.at(i); 562 | } 563 | 564 | std::vector b; 565 | Eigen::Vector3d b_total; 566 | b_total.setZero(); 567 | for(int i = 0; i < 6; i++) 568 | { 569 | b.push_back(r_hat.at(i)*a.at(i)); 570 | if(m_current_segment == 1 || i > 2) // Take into account all tendons for first segment, but only last four for second segment 571 | b_total += b.at(i); 572 | } 573 | 574 | Eigen::Vector3d le; 575 | le.setZero(); 576 | 577 | Eigen::Vector3d fe; 578 | fe.setZero(); 579 | 580 | Eigen::Vector3d c; 581 | c = -u_hat*m_Kbt*u-v_hat*m_Kse*(v - Eigen::Vector3d(0,0,1))-R.transpose()*le-b_total; 582 | 583 | Eigen::Vector3d d; 584 | d = -u_hat*m_Kse*(v - Eigen::Vector3d(0,0,1))-R.transpose()*fe-a_total; 585 | 586 | 587 | Eigen::VectorXd vu_dot; 588 | vu_dot.resize(6); 589 | Eigen::VectorXd dc; 590 | dc.resize(6); 591 | dc << d, 592 | c; 593 | 594 | Eigen::MatrixXd vu_mat; 595 | vu_mat.resize(6,6); 596 | 597 | vu_mat << m_Kse + A_total, G, 598 | B_total, m_Kbt+ H; 599 | 600 | vu_dot = vu_mat.jacobiSvd(Eigen::ComputeThinU|Eigen::ComputeThinV).solve(dc); 601 | 602 | Eigen::Vector3d p_dot; 603 | p_dot = R*v; 604 | 605 | Eigen::Matrix3d R_dot; 606 | R_dot = R*u_hat; 607 | 608 | Eigen::Matrix R_row(R_dot); 609 | Eigen::Map R_flat(R_row.data(), R_row.size()); 610 | 611 | 612 | dy << p_dot.transpose(),R_flat,vu_dot.transpose(),1; 613 | 614 | 615 | 616 | 617 | } 618 | 619 | void CosseratRodModel::get_res(Eigen::MatrixXd &output, Eigen::MatrixXd input) 620 | { 621 | //Define variables 622 | Eigen::MatrixXd states1; 623 | Eigen::MatrixXd states2; 624 | 625 | //Run IVP for first segment 626 | Eigen::Matrix init_state_segment1; //p, R, v, u, s 627 | init_state_segment1 << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, input.transpose(), 0; 628 | m_current_segment = 1; // Set current segment to first for integration along backbone 629 | run_IVP(states1,init_state_segment1); 630 | m_states1 = states1; 631 | 632 | //RUN IVP for second segment with initial values according to transition conditions between segments 633 | Eigen::Matrix init_state_segment2; //p, R, v, u, s 634 | Eigen::Vector3d v_new; 635 | Eigen::Vector3d u_new; 636 | //calculate initial v and u for next segment by evaluating states at the end of the first segment and respecting transition/boundary conditions 637 | 638 | Eigen::Matrix3d R_end1; 639 | R_end1 << states1.bottomRows(1).block(0,3,1,3), 640 | states1.bottomRows(1).block(0,6,1,3), 641 | states1.bottomRows(1).block(0,9,1,3); 642 | 643 | Eigen::Vector3d u_end1 = states1.bottomRows(1).block(0,15,1,3).transpose(); 644 | Eigen::Vector3d v_end1 = states1.bottomRows(1).block(0,12,1,3).transpose(); 645 | 646 | Eigen::Vector3d p_dot1 = R_end1*(u_end1.cross(m_routing.at(0)) + v_end1); 647 | Eigen::Vector3d p_dot2 = R_end1*(u_end1.cross(m_routing.at(1)) + v_end1); 648 | Eigen::Vector3d p_dot3 = R_end1*(u_end1.cross(m_routing.at(2)) + v_end1); 649 | 650 | Eigen::Vector3d F_end1 = -m_tau(0)/p_dot1.norm()*p_dot1 -m_tau(1)/p_dot2.norm()*p_dot2 -m_tau(2)/p_dot3.norm()*p_dot3; 651 | Eigen::Vector3d L_end1 = -m_tau(0)/p_dot1.norm()*(R_end1*m_routing.at(0)).cross(p_dot1) -m_tau(1)/p_dot2.norm()*(R_end1*m_routing.at(1)).cross(p_dot2) -m_tau(2)/p_dot3.norm()*(R_end1*m_routing.at(2)).cross(p_dot3); 652 | 653 | v_new = v_end1 - m_Kse.inverse()*R_end1.transpose()*F_end1; 654 | u_new = u_end1 - m_Kbt.inverse()*R_end1.transpose()*L_end1; 655 | 656 | 657 | init_state_segment2 << states1.bottomRows(1).block(0,0,1,12), v_new.transpose(), u_new.transpose(), states1.bottomRows(1).block(0,18,1,1); 658 | m_current_segment = 2; // Set current segment to second for integration along backbone 659 | run_IVP(states2,init_state_segment2); 660 | m_states2 = states2; 661 | 662 | //Get states at the distal end of the robot 663 | Eigen::Vector3d n_end2; 664 | Eigen::Vector3d m_end2; 665 | Eigen::Vector3d u_end2; 666 | Eigen::Vector3d v_end2; 667 | Eigen::Vector3d F_end2; 668 | Eigen::Vector3d L_end2; 669 | Eigen::Matrix3d R_end2; 670 | 671 | v_end2 = states2.bottomRows(1).block(0,12,1,3).transpose(); 672 | u_end2 = states2.bottomRows(1).block(0,15,1,3).transpose(); 673 | R_end2 << states2.bottomRows(1).block(0,3,1,3), 674 | states2.bottomRows(1).block(0,6,1,3), 675 | states2.bottomRows(1).block(0,9,1,3); 676 | 677 | Eigen::Vector3d p_dot1_2 = R_end2*(u_end2.cross(m_routing.at(3)) + v_end2); 678 | Eigen::Vector3d p_dot2_2 = R_end2*(u_end2.cross(m_routing.at(4)) + v_end2); 679 | Eigen::Vector3d p_dot3_2 = R_end2*(u_end2.cross(m_routing.at(5)) + v_end2); 680 | 681 | 682 | F_end2 = -m_tau(3)/p_dot1_2.norm()*p_dot1_2 -m_tau(4)/p_dot2_2.norm()*p_dot2_2 -m_tau(5)/p_dot3_2.norm()*p_dot3_2; 683 | L_end2 = -m_tau(3)/p_dot1_2.norm()*(R_end2*m_routing.at(3)).cross(p_dot1_2) -m_tau(4)/p_dot2_2.norm()*(R_end2*m_routing.at(4)).cross(p_dot2_2) -m_tau(5)/p_dot3_2.norm()*(R_end2*m_routing.at(5)).cross(p_dot3_2); 684 | 685 | n_end2 = R_end2*m_Kse*(v_end2 - Eigen::Vector3d(0,0,1)); 686 | m_end2 = R_end2*m_Kbt*u_end2; 687 | 688 | // 1) Force eqilibrium 689 | Eigen::Vector3d force_eq = n_end2 - F_end2 - m_f_ext; 690 | 691 | // 2) Moment eqilibrium 692 | Eigen::Vector3d moment_eq = m_end2-L_end2 - m_l_ext; 693 | 694 | // Set output to resudial error 695 | output.resize(6,1); 696 | output << force_eq, 697 | moment_eq; 698 | 699 | 700 | } 701 | 702 | void CosseratRodModel::setKeepInits(bool keep) 703 | { 704 | m_keep_inits = keep; 705 | } 706 | 707 | 708 | -------------------------------------------------------------------------------- /c++/src/piecewiseconstantcurvaturemodel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | #include "piecewiseconstantcurvaturemodel.h" 15 | 16 | // Wrapper function for non-linear boundary conditions for GNU::GSL 17 | int evaluate_boundary_conditions_pcc(const gsl_vector * x, void *param, gsl_vector * f) 18 | { 19 | 20 | 21 | PiecewiseConstantCurvatureModel *model = (PiecewiseConstantCurvatureModel*)param; 22 | 23 | Eigen::MatrixXd inputs; 24 | 25 | inputs.resize(3*model->getNumberOfTotalDisks(),1); 26 | 27 | Eigen::MatrixXd outputs; 28 | 29 | for(int i = 0; i < 3*model->getNumberOfTotalDisks(); i++) 30 | { 31 | inputs(i,0) = gsl_vector_get (x, i); 32 | } 33 | 34 | 35 | 36 | model->get_res(outputs, inputs); 37 | 38 | 39 | 40 | 41 | for(int i = 0; i < 3*model->getNumberOfTotalDisks(); i++) 42 | { 43 | 44 | gsl_vector_set (f, i, outputs(i,0)); 45 | } 46 | 47 | 48 | return GSL_SUCCESS; 49 | } 50 | 51 | // Callback function that can be used during non-linear squares solving 52 | void 53 | callback_pcc(const size_t iter, void *param, 54 | const gsl_multifit_nlinear_workspace *w) 55 | { 56 | 57 | PiecewiseConstantCurvatureModel *model = (PiecewiseConstantCurvatureModel*)param; 58 | 59 | //Print current iteration 60 | std::cout << "Iteration " << iter << std::endl; 61 | 62 | gsl_vector * x = gsl_multifit_nlinear_position(w); 63 | gsl_vector * r = gsl_multifit_nlinear_residual(w); 64 | 65 | std::cout << "Current values:" << std::endl; 66 | for(int i = 0; i < 3*model->getNumberOfTotalDisks(); i++) 67 | { 68 | std::cout << " " << gsl_vector_get(x, i); 69 | } 70 | std::cout << std::endl << "Current residuals:" << std::endl; 71 | for(int i = 0; i < 3*model->getNumberOfTotalDisks(); i++) 72 | { 73 | std::cout << " " << gsl_vector_get(r, i); 74 | } 75 | 76 | double chisq; 77 | gsl_blas_ddot(r, r, &chisq); 78 | 79 | std::cout << std::endl << "Current cost: "<< chisq << std::endl << std::endl; 80 | 81 | } 82 | 83 | PiecewiseConstantCurvatureModel::PiecewiseConstantCurvatureModel() 84 | { 85 | //Initialize member variables and set up default parameters for TDCR 86 | m_length[0] = 0.1; 87 | m_length[1] = 0.1; 88 | m_youngs_modulus = 60e9; 89 | m_number_disks[0] = 10; 90 | m_number_disks[1] = 10; 91 | m_pradius_disks[0] = 0.008; 92 | m_pradius_disks[1] = 0.006; 93 | m_ro = 0.0005; 94 | 95 | std::vector routing; 96 | 97 | Eigen::Vector3d tendon1; 98 | tendon1 << 0, 99 | m_pradius_disks[0], 100 | 0; 101 | 102 | Eigen::Vector3d tendon2; 103 | tendon2 << m_pradius_disks[0]*std::cos(-M_PI/6), 104 | m_pradius_disks[0]*std::sin(-M_PI/6), 105 | 0; 106 | Eigen::Vector3d tendon3; 107 | tendon3 << m_pradius_disks[0]*std::cos(7*M_PI/6), 108 | m_pradius_disks[0]*std::sin(7*M_PI/6), 109 | 0; 110 | 111 | routing.push_back(tendon1); 112 | routing.push_back(tendon2); 113 | routing.push_back(tendon3); 114 | 115 | tendon1 << 0, 116 | m_pradius_disks[1], 117 | 0; 118 | 119 | tendon2 << m_pradius_disks[1]*std::cos(-M_PI/6), 120 | m_pradius_disks[1]*std::sin(-M_PI/6), 121 | 0; 122 | 123 | tendon3 << m_pradius_disks[1]*std::cos(7*M_PI/6), 124 | m_pradius_disks[1]*std::sin(7*M_PI/6), 125 | 0; 126 | 127 | routing.push_back(tendon1); 128 | routing.push_back(tendon2); 129 | routing.push_back(tendon3); 130 | 131 | m_routing = routing; 132 | 133 | 134 | 135 | m_keep_inits = false; 136 | m_default_inits.resize(1,3*(m_number_disks[0]+m_number_disks[1])); 137 | for(int i = 0; i < m_default_inits.cols(); i++) 138 | { 139 | m_default_inits(0,i) = 0.01; 140 | } 141 | m_last_inits = m_default_inits; 142 | 143 | 144 | m_tau.setZero(); 145 | 146 | } 147 | 148 | void PiecewiseConstantCurvatureModel::setRobotParameters(std::array length, double youngs_modulus, std::vector routing, std::array number_disks, std::array pradius_disks, double ro) 149 | { 150 | m_length[0] = length[0]; 151 | m_length[1] = length[1]; 152 | m_youngs_modulus = youngs_modulus; 153 | m_number_disks[0] = number_disks[0]; 154 | m_number_disks[1] = number_disks[1]; 155 | m_pradius_disks[0] = pradius_disks[0]; 156 | m_pradius_disks[1] = pradius_disks[1]; 157 | m_ro = ro; 158 | m_routing = routing; 159 | 160 | 161 | m_default_inits.resize(1,3*(m_number_disks[0]+m_number_disks[1])); 162 | for(int i = 0; i < m_default_inits.cols(); i++) 163 | { 164 | m_default_inits(0,i) = 0.01; 165 | } 166 | m_last_inits = m_default_inits; 167 | 168 | 169 | } 170 | 171 | double PiecewiseConstantCurvatureModel::getNumberOfTotalDisks() 172 | { 173 | return m_number_disks[0] + m_number_disks[1]; 174 | } 175 | 176 | void PiecewiseConstantCurvatureModel::setDefaultInitValues(Eigen::MatrixXd inits) 177 | { 178 | m_default_inits = inits; 179 | } 180 | 181 | Eigen::MatrixXd PiecewiseConstantCurvatureModel::getFinalInitValues() 182 | { 183 | return m_last_inits; 184 | } 185 | 186 | double PiecewiseConstantCurvatureModel::getFinalResudial() 187 | { 188 | return m_residual_error; 189 | } 190 | 191 | Eigen::Matrix4d PiecewiseConstantCurvatureModel::getDiskTransform(Eigen::MatrixXd var, Eigen::VectorXd length_ss, int idx_from, int idx_to) 192 | { 193 | Eigen::Matrix4d diskTransform = Eigen::Matrix4d::Identity(); 194 | Eigen::Matrix4d tempTransform = Eigen::Matrix4d::Identity(); 195 | 196 | 197 | for(int i = idx_to + 1; i <= idx_from; i++) 198 | { 199 | double beta = var(3*i-3); 200 | double gamma = var(3*i-2); 201 | double epsi = var(3*i-1); 202 | double k = std::sqrt(beta*beta+gamma*gamma); 203 | double phi = std::atan2(gamma,beta); 204 | double theta = k*length_ss(i-1); 205 | Eigen::Vector3d p; 206 | p << (1-std::cos(theta))*std::cos(phi)/k, 207 | (1-std::cos(theta))*std::sin(phi)/k, 208 | std::sin(theta)/k; 209 | Eigen::Matrix3d Rz; 210 | Rz << std::cos(phi), -1*std::sin(phi), 0, 211 | std::sin(phi), std::cos(phi), 0, 212 | 0, 0, 1; 213 | Eigen::Matrix3d Ry; 214 | Ry << std::cos(theta), 0, std::sin(theta), 215 | 0, 1, 0, 216 | -1*std::sin(theta), 0, std::cos(theta); 217 | Eigen::Matrix3d Rz2; 218 | Rz2 << std::cos(epsi-phi), -1*std::sin(epsi-phi), 0, 219 | std::sin(epsi-phi), std::cos(epsi-phi), 0, 220 | 0, 0, 1; 221 | tempTransform << Rz*Ry*Rz2, p, 222 | 0, 0, 0, 1; 223 | 224 | diskTransform = diskTransform*tempTransform; 225 | 226 | } 227 | 228 | return diskTransform; 229 | 230 | } 231 | 232 | PiecewiseConstantCurvatureModel::~PiecewiseConstantCurvatureModel() 233 | { 234 | 235 | } 236 | 237 | 238 | bool PiecewiseConstantCurvatureModel::forwardKinematics(Eigen::MatrixXd &diskFrames, Eigen::MatrixXd q, Eigen::Vector3d f_ext, Eigen::Vector3d l_ext) 239 | { 240 | m_tau = q; 241 | m_f_ext = f_ext; 242 | m_l_ext = l_ext; 243 | 244 | // Run BVP Shooting method 245 | Eigen::MatrixXd inits; 246 | 247 | if(m_keep_inits) 248 | { 249 | inits = m_last_inits; 250 | } 251 | else 252 | { 253 | inits = m_default_inits; 254 | } 255 | 256 | const size_t n = 3*(m_number_disks[0]+m_number_disks[1]); //Number of equations (boundary conditions) 257 | const size_t p = 3*(m_number_disks[0]+m_number_disks[1]); //Number of parameters (initial guesses) 258 | gsl_vector *x0 = gsl_vector_alloc(p); 259 | gsl_multifit_nlinear_fdf fdf; 260 | gsl_multifit_nlinear_parameters fdf_params = gsl_multifit_nlinear_default_parameters(); 261 | 262 | fdf.f = evaluate_boundary_conditions_pcc; 263 | fdf.df = NULL; //Provide your own Jacobian 264 | fdf.fvv = NULL; //Provide your own Hessian 265 | fdf.n = n; 266 | fdf.p = p; 267 | fdf.params = this; 268 | 269 | //Set initial values 270 | for(int i = 0; i < 3*(m_number_disks[0]+m_number_disks[1]); i++) 271 | { 272 | gsl_vector_set(x0,i,inits(0,i)); 273 | } 274 | 275 | //Choose solver (Levenberg-Marquardt) 276 | fdf_params.trs = gsl_multifit_nlinear_trs_lm; 277 | //Choose solver (Levenberg-Marquardt with acceleration) 278 | //fdf_params.trs = gsl_multifit_nlinear_trs_lmaccel; 279 | //Choose solver (dogleg) 280 | //fdf_params.trs = gsl_multifit_nlinear_trs_dogleg; 281 | 282 | //Set scaling parameter 283 | fdf_params.scale = gsl_multifit_nlinear_scale_more; 284 | 285 | //Set solver for trust region 286 | fdf_params.solver = gsl_multifit_nlinear_solver_qr; 287 | 288 | //Set trust region factor 289 | fdf_params.factor_up = 2.5; //default 3 290 | fdf_params.factor_down = 4; //default 2 291 | 292 | 293 | ///---Solve the system--- 294 | const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust; 295 | const size_t max_iter = 1000; 296 | const double xtol = 1.0e-10; 297 | const double gtol = 1.0e-10; 298 | const double ftol = 1.0e-10; 299 | gsl_multifit_nlinear_workspace *work = gsl_multifit_nlinear_alloc(T,&fdf_params, n, p); 300 | gsl_vector * f = gsl_multifit_nlinear_residual(work); 301 | gsl_vector * x = gsl_multifit_nlinear_position(work); 302 | int info; 303 | double chisq0, chisq, rcond; 304 | 305 | /* initialize solver */ 306 | gsl_multifit_nlinear_init(x0, &fdf, work); 307 | 308 | /* store initial cost */ 309 | gsl_blas_ddot(f, f, &chisq0); 310 | 311 | /* iterate until convergence */ 312 | //gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,callback_pcc, this, &info, work); 313 | //Without Callback 314 | gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,NULL, NULL, &info, work); 315 | 316 | /* store final cost */ 317 | gsl_blas_ddot(f, f, &chisq); 318 | m_residual_error = chisq; 319 | 320 | /* store cond(J(x)) */ 321 | gsl_multifit_nlinear_rcond(&rcond, work); 322 | 323 | ///---End Solve the system--- 324 | 325 | Eigen::MatrixXd final_values; 326 | final_values.resize(1,3*(m_number_disks[0]+m_number_disks[1])); 327 | 328 | Eigen::MatrixXd final_boundcon; 329 | final_boundcon.resize(1,3*(m_number_disks[0]+m_number_disks[1])); 330 | 331 | 332 | for(int i = 0; i < 3*(m_number_disks[0]+m_number_disks[1]); i ++) 333 | { 334 | final_values(0,i) = gsl_vector_get (x, i); 335 | final_boundcon(0,i) = gsl_vector_get (f, i); 336 | } 337 | 338 | m_last_inits = final_values; 339 | gsl_vector_free (x0); 340 | gsl_multifit_nlinear_free(work); 341 | 342 | 343 | 344 | 345 | Eigen::VectorXd length_ss; 346 | length_ss.resize(m_number_disks[0]+m_number_disks[1]); 347 | 348 | for(int i = 0; i < m_number_disks[0]; i++) 349 | { 350 | length_ss(i) = m_length[0]/m_number_disks[0]; 351 | } 352 | 353 | for(int i = m_number_disks[0]; i < m_number_disks[0]+m_number_disks[1]; i++) 354 | { 355 | length_ss(i) = m_length[1]/m_number_disks[1]; 356 | } 357 | 358 | diskFrames.resize(4,4*(m_number_disks[0]+m_number_disks[1]+1)); 359 | 360 | diskFrames.block(0,0,4,4) = Eigen::Matrix4d::Identity(); 361 | 362 | for(int i = 1; i < m_number_disks[0]+m_number_disks[1] + 1; i++) 363 | { 364 | Eigen::Matrix4d frame = getDiskTransform(final_values.transpose(),length_ss,i,0); 365 | diskFrames.block(0,4*i,4,4) = frame; 366 | } 367 | 368 | 369 | 370 | if(m_residual_error > 1e-6) 371 | { 372 | return false; 373 | } 374 | 375 | return true; 376 | 377 | } 378 | 379 | void PiecewiseConstantCurvatureModel::get_res(Eigen::MatrixXd &output, Eigen::MatrixXd input) 380 | { 381 | 382 | Eigen::VectorXd length_ss; 383 | length_ss.resize(m_number_disks[0]+m_number_disks[1]); 384 | 385 | for(int i = 0; i < m_number_disks[0]; i++) 386 | { 387 | length_ss(i) = m_length[0]/m_number_disks[0]; 388 | } 389 | 390 | for(int i = m_number_disks[0]; i < m_number_disks[0]+m_number_disks[1]; i++) 391 | { 392 | length_ss(i) = m_length[1]/m_number_disks[1]; 393 | } 394 | 395 | output.resize(3*(m_number_disks[0]+m_number_disks[1]),1); 396 | 397 | Eigen::Vector4d F_prev; 398 | F_prev.setZero(); 399 | Eigen::Vector3d M_prev; 400 | M_prev.setZero(); 401 | 402 | for(int ss_i = m_number_disks[0]+m_number_disks[1]; ss_i > 0; ss_i--) 403 | { 404 | 405 | //Get transformation matrix between disk i and disk i-1 406 | Eigen::Matrix4d diskTransform; 407 | double beta = input(3*ss_i-3); 408 | double gamma = input(3*ss_i-2); 409 | double epsi = input(3*ss_i-1); 410 | double k = std::sqrt(beta*beta+gamma*gamma); 411 | double phi = std::atan2(gamma,beta); 412 | double theta = k*length_ss(ss_i-1); 413 | Eigen::Vector3d p; 414 | p << (1-std::cos(theta))*std::cos(phi)/k, 415 | (1-std::cos(theta))*std::sin(phi)/k, 416 | std::sin(theta)/k; 417 | Eigen::Matrix3d Rz; 418 | Rz << std::cos(phi), -1*std::sin(phi), 0, 419 | std::sin(phi), std::cos(phi), 0, 420 | 0, 0, 1; 421 | Eigen::Matrix3d Ry; 422 | Ry << std::cos(theta), 0, std::sin(theta), 423 | 0, 1, 0, 424 | -1*std::sin(theta), 0, std::cos(theta); 425 | Eigen::Matrix3d Rz2; 426 | Rz2 << std::cos(epsi-phi), -1*std::sin(epsi-phi), 0, 427 | std::sin(epsi-phi), std::cos(epsi-phi), 0, 428 | 0, 0, 1; 429 | diskTransform << Rz*Ry*Rz2, p, 430 | 0, 0, 0, 1; 431 | 432 | Eigen::Matrix tendon_pos; 433 | for(int i = 0; i < 6; i++) 434 | { 435 | tendon_pos.col(i) = diskTransform.block(0,0,3,3)*m_routing.at(i) + diskTransform.block(0,3,3,1); 436 | } 437 | 438 | Eigen::MatrixXd pt_mat; 439 | pt_mat.resize(4,6); 440 | pt_mat << m_routing.at(0) - tendon_pos.col(0), m_routing.at(1) - tendon_pos.col(1), m_routing.at(2) - tendon_pos.col(2), m_routing.at(3) - tendon_pos.col(3), m_routing.at(4) - tendon_pos.col(4), m_routing.at(5) - tendon_pos.col(5), 441 | 0, 0, 0, 0, 0, 0; 442 | 443 | Eigen::VectorXd norm_ct1; 444 | norm_ct1.resize(6); 445 | for(int i = 0; i < 6; i++) 446 | { 447 | norm_ct1(i) = (m_routing.at(i) - tendon_pos.col(i)).norm(); 448 | } 449 | 450 | Eigen::MatrixXd ct1_mat; 451 | ct1_mat.resize(4,6); 452 | ct1_mat << norm_ct1.transpose(), 453 | norm_ct1.transpose(), 454 | norm_ct1.transpose(), 455 | norm_ct1.transpose(); 456 | 457 | Eigen::MatrixXd F_disk; 458 | F_disk.resize(m_number_disks[0]+m_number_disks[1],6); 459 | F_disk.setZero(); 460 | 461 | for(int i = 0; i < m_number_disks[0]; i++) 462 | { 463 | F_disk.block(i,0,1,6) = m_tau.transpose(); 464 | } 465 | 466 | for(int i = m_number_disks[0]; i < m_number_disks[0] + m_number_disks[1]; i++) 467 | { 468 | F_disk.block(i,3,1,3) = m_tau.block(3,0,3,1).transpose(); 469 | } 470 | 471 | Eigen::MatrixXd F_disk_mat; 472 | F_disk_mat.resize(4,6); 473 | F_disk_mat << F_disk.row(ss_i-1), 474 | F_disk.row(ss_i-1), 475 | F_disk.row(ss_i-1), 476 | F_disk.row(ss_i-1); 477 | 478 | Eigen::MatrixXd F_rel; 479 | F_rel.resize(4,6); 480 | F_rel.setZero(); 481 | 482 | Eigen::MatrixXd M_rel; 483 | M_rel.resize(3,6); 484 | M_rel.setZero(); 485 | 486 | Eigen::Vector4d zi = diskTransform.col(2); 487 | 488 | if(ss_i < m_number_disks[0]+m_number_disks[1]) 489 | { 490 | Eigen::Matrix4d tempTransform = getDiskTransform(input,length_ss,ss_i+1,ss_i-1); 491 | 492 | Eigen::Matrix tendon_pos_temp; 493 | for(int i = 0; i < 6; i++) 494 | { 495 | tendon_pos_temp.col(i) = tempTransform.block(0,0,3,3)*m_routing.at(i) + tempTransform.block(0,3,3,1); 496 | } 497 | 498 | Eigen::MatrixXd pt1_mat; 499 | pt1_mat.resize(4,6); 500 | pt1_mat <getNumberOfJoints()+1)*model->getNumberOfTotalDisks(),1); 26 | 27 | Eigen::MatrixXd outputs; 28 | 29 | for(int i = 0; i < (model->getNumberOfJoints()+1)*model->getNumberOfTotalDisks(); i++) 30 | { 31 | inputs(i,0) = gsl_vector_get (x, i); 32 | } 33 | 34 | 35 | //Function that needs to be minimized goes here 36 | 37 | model->get_res(outputs, inputs); 38 | 39 | 40 | 41 | //Functions that needs to be minimized ends here 42 | 43 | for(int i = 0; i < (model->getNumberOfJoints()+1)*model->getNumberOfTotalDisks(); i++) 44 | { 45 | 46 | gsl_vector_set (f, i, outputs(i,0)); 47 | } 48 | 49 | 50 | return GSL_SUCCESS; 51 | } 52 | 53 | // Callback function that can be used during non-linear squares solving 54 | void 55 | callback_prb(const size_t iter, void *param, 56 | const gsl_multifit_nlinear_workspace *w) 57 | { 58 | 59 | PseudoRigidBodyModel *model = (PseudoRigidBodyModel*)param; 60 | 61 | //Print current iteration 62 | std::cout << "Iteration " << iter << std::endl; 63 | 64 | gsl_vector * x = gsl_multifit_nlinear_position(w); 65 | gsl_vector * r = gsl_multifit_nlinear_residual(w); 66 | 67 | std::cout << "Current values:" << std::endl; 68 | for(int i = 0; i < (model->getNumberOfJoints()+1)*model->getNumberOfTotalDisks(); i++) 69 | { 70 | std::cout << " " << gsl_vector_get(x, i); 71 | } 72 | std::cout << std::endl << "Current residuals:" << std::endl; 73 | for(int i = 0; i < (model->getNumberOfJoints()+1)*model->getNumberOfTotalDisks(); i++) 74 | { 75 | std::cout << " " << gsl_vector_get(r, i); 76 | } 77 | 78 | double chisq; 79 | gsl_blas_ddot(r, r, &chisq); 80 | 81 | std::cout << std::endl << "Current cost: "<< chisq << std::endl << std::endl; 82 | 83 | } 84 | 85 | PseudoRigidBodyModel::PseudoRigidBodyModel() 86 | { 87 | //Initialize member variables and set up default parameters for TDCR 88 | m_length[0] = 0.1; 89 | m_length[1] = 0.1; 90 | m_youngs_modulus = 60e9; 91 | m_number_disks[0] = 10; 92 | m_number_disks[1] = 10; 93 | m_pradius_disks[0] = 0.008; 94 | m_pradius_disks[1] = 0.006; 95 | m_ro = 0.0005; 96 | 97 | std::vector routing; 98 | 99 | Eigen::Vector3d tendon1; 100 | tendon1 << 0, 101 | m_pradius_disks[0], 102 | 0; 103 | 104 | Eigen::Vector3d tendon2; 105 | tendon2 << m_pradius_disks[0]*std::cos(-M_PI/6), 106 | m_pradius_disks[0]*std::sin(-M_PI/6), 107 | 0; 108 | Eigen::Vector3d tendon3; 109 | tendon3 << m_pradius_disks[0]*std::cos(7*M_PI/6), 110 | m_pradius_disks[0]*std::sin(7*M_PI/6), 111 | 0; 112 | 113 | routing.push_back(tendon1); 114 | routing.push_back(tendon2); 115 | routing.push_back(tendon3); 116 | 117 | tendon1 << 0, 118 | m_pradius_disks[1], 119 | 0; 120 | 121 | tendon2 << m_pradius_disks[1]*std::cos(-M_PI/6), 122 | m_pradius_disks[1]*std::sin(-M_PI/6), 123 | 0; 124 | 125 | tendon3 << m_pradius_disks[1]*std::cos(7*M_PI/6), 126 | m_pradius_disks[1]*std::sin(7*M_PI/6), 127 | 0; 128 | 129 | routing.push_back(tendon1); 130 | routing.push_back(tendon2); 131 | routing.push_back(tendon3); 132 | 133 | m_routing = routing; 134 | 135 | 136 | 137 | 138 | 139 | 140 | //4 joints per subsegment with relative positions/lengths 141 | m_joint_pos.resize(4); 142 | m_joint_pos << 0.125, 143 | 0.35, 144 | 0.388, 145 | 0.136; 146 | 147 | m_joint_pos = m_joint_pos/m_joint_pos.sum(); 148 | 149 | m_keep_inits = false; 150 | m_default_inits.resize(1,(m_joint_pos.size()+1)*(m_number_disks[0]+m_number_disks[1])); 151 | for(int i = 0; i < m_default_inits.cols(); i++) 152 | { 153 | m_default_inits(0,i) = 0; 154 | } 155 | m_last_inits = m_default_inits; 156 | 157 | 158 | 159 | m_tau.setZero(); 160 | 161 | } 162 | 163 | void PseudoRigidBodyModel::setRobotParameters(std::array length, double youngs_modulus, std::vector routing, std::array number_disks, std::array pradius_disks, double ro) 164 | { 165 | m_length[0] = length[0]; 166 | m_length[1] = length[1]; 167 | m_youngs_modulus = youngs_modulus; 168 | m_number_disks[0] = number_disks[0]; 169 | m_number_disks[1] = number_disks[1]; 170 | m_pradius_disks[0] = pradius_disks[0]; 171 | m_pradius_disks[1] = pradius_disks[1]; 172 | m_ro = ro; 173 | m_routing = routing; 174 | 175 | m_default_inits.resize(1,(m_joint_pos.size()+1)*(m_number_disks[0]+m_number_disks[1])); 176 | for(int i = 0; i < m_default_inits.cols(); i++) 177 | { 178 | m_default_inits(0,i) = 0; 179 | } 180 | m_last_inits = m_default_inits; 181 | 182 | 183 | } 184 | 185 | double PseudoRigidBodyModel::getNumberOfTotalDisks() 186 | { 187 | return m_number_disks[0] + m_number_disks[1]; 188 | } 189 | 190 | double PseudoRigidBodyModel::getNumberOfJoints() 191 | { 192 | return m_joint_pos.size(); 193 | } 194 | 195 | void PseudoRigidBodyModel::setDefaultInitValues(Eigen::MatrixXd inits) 196 | { 197 | m_default_inits = inits; 198 | } 199 | 200 | Eigen::MatrixXd PseudoRigidBodyModel::getFinalInitValues() 201 | { 202 | return m_last_inits; 203 | } 204 | 205 | double PseudoRigidBodyModel::getFinalResudial() 206 | { 207 | return m_residual_error; 208 | } 209 | 210 | Eigen::Matrix4d PseudoRigidBodyModel::getDiskTransform(Eigen::MatrixXd &Trb, Eigen::MatrixXd var, Eigen::VectorXd length_ss, int idx_from, int idx_to) 211 | { 212 | Eigen::Matrix4d diskTransform = Eigen::Matrix4d::Identity(); 213 | 214 | int nrb = m_joint_pos.size(); 215 | 216 | Trb.resize(4,4*nrb*(idx_from-idx_to)); 217 | 218 | 219 | for(int i = idx_to + 1; i <= idx_from; i++) 220 | { 221 | Eigen::MatrixXd theta; 222 | theta = var.block((nrb+1)*i-(nrb+1),0,nrb-1,1); 223 | double phi = var((nrb+1)*i-(nrb+1) + 3,0); 224 | double epsi = var((nrb+1)*i-(nrb+1) + 4,0); 225 | 226 | Eigen::Matrix4d Rphi; 227 | Rphi << std::cos(phi), -1*std::sin(phi), 0, 0, 228 | std::sin(phi), std::cos(phi), 0, 0, 229 | 0, 0, 1, 0, 230 | 0, 0, 0, 1; 231 | 232 | Eigen::Matrix4d Rphi_epsi; 233 | Rphi_epsi << std::cos(epsi-phi), -1*std::sin(epsi-phi), 0, 0, 234 | std::sin(epsi-phi), std::cos(epsi-phi), 0, 0, 235 | 0, 0, 1, 0, 236 | 0, 0, 0, 1; 237 | 238 | 239 | Eigen::Matrix4d gamma_mat; 240 | gamma_mat << 1, 0, 0, 0, 241 | 0, 1, 0, 0, 242 | 0, 0, 1, m_joint_pos(0)*length_ss(i-1), 243 | 0, 0, 0, 1; 244 | 245 | Eigen::Matrix4d tempTransform; 246 | tempTransform = Rphi*gamma_mat; 247 | Trb.block(0,4*(nrb*(i-(idx_to+1))),4,4) = diskTransform*tempTransform; 248 | 249 | for(int k = 0; k < nrb-1; k++) 250 | { 251 | Eigen::Matrix4d theta_mat; 252 | theta_mat << std::cos(theta(k)), 0, std::sin(theta(k)), m_joint_pos(k+1)*length_ss(i-1)*std::sin(theta(k)), 253 | 0, 1, 0, 0, 254 | -1*std::sin(theta(k)), 0, std::cos(theta(k)), m_joint_pos(k+1)*length_ss(i-1)*std::cos(theta(k)), 255 | 0, 0, 0, 1; 256 | tempTransform = tempTransform*theta_mat; 257 | Trb.block(0,4*(nrb*(i-(idx_to+1))+k+1),4,4) = diskTransform*tempTransform; 258 | } 259 | diskTransform = diskTransform*tempTransform*Rphi_epsi; 260 | 261 | } 262 | 263 | return diskTransform; 264 | 265 | } 266 | 267 | PseudoRigidBodyModel::~PseudoRigidBodyModel() 268 | { 269 | 270 | } 271 | 272 | 273 | bool PseudoRigidBodyModel::forwardKinematics(Eigen::MatrixXd &diskFrames, Eigen::MatrixXd q, Eigen::Vector3d f_ext, Eigen::Vector3d l_ext) 274 | { 275 | m_tau = q; 276 | m_f_ext = f_ext; 277 | m_l_ext = l_ext; 278 | 279 | // Run BVP Shooting method 280 | Eigen::MatrixXd inits; 281 | 282 | if(m_keep_inits) 283 | { 284 | inits = m_last_inits; 285 | } 286 | else 287 | { 288 | inits = m_default_inits; 289 | } 290 | 291 | const size_t n = (m_joint_pos.size()+1)*(m_number_disks[0]+m_number_disks[1]); //Number of equations (boundary conditions) 292 | const size_t p = (m_joint_pos.size()+1)*(m_number_disks[0]+m_number_disks[1]); //Number of parameters (initial guesses) 293 | gsl_vector *x0 = gsl_vector_alloc(p); 294 | gsl_multifit_nlinear_fdf fdf; 295 | gsl_multifit_nlinear_parameters fdf_params = gsl_multifit_nlinear_default_parameters(); 296 | 297 | fdf.f = evaluate_boundary_conditions_prb; 298 | fdf.df = NULL; //Provide your own Jacobian 299 | fdf.fvv = NULL; //Provide your own Hessian 300 | fdf.n = n; 301 | fdf.p = p; 302 | fdf.params = this; 303 | 304 | //Set initial values 305 | for(int i = 0; i < (m_joint_pos.size()+1)*(m_number_disks[0]+m_number_disks[1]); i++) 306 | { 307 | gsl_vector_set(x0,i,inits(0,i)); 308 | } 309 | 310 | //Choose solver (Levenberg-Marquardt) 311 | fdf_params.trs = gsl_multifit_nlinear_trs_lm; 312 | //Choose solver (Levenberg-Marquardt with acceleration) 313 | //fdf_params.trs = gsl_multifit_nlinear_trs_lmaccel; 314 | //Choose solver (dogleg) 315 | //fdf_params.trs = gsl_multifit_nlinear_trs_dogleg; 316 | 317 | //Set scaling parameter 318 | fdf_params.scale = gsl_multifit_nlinear_scale_more; 319 | 320 | //Set solver for trust region 321 | fdf_params.solver = gsl_multifit_nlinear_solver_qr; 322 | 323 | //Set trust region factor 324 | fdf_params.factor_up = 2.5; //default 3 325 | fdf_params.factor_down = 4; //default 2 326 | 327 | 328 | ///---Solve the system--- 329 | const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust; 330 | const size_t max_iter = 1000; 331 | const double xtol = 1.0e-10; 332 | const double gtol = 1.0e-10; 333 | const double ftol = 1.0e-10; 334 | gsl_multifit_nlinear_workspace *work = gsl_multifit_nlinear_alloc(T,&fdf_params, n, p); 335 | gsl_vector * f = gsl_multifit_nlinear_residual(work); 336 | gsl_vector * x = gsl_multifit_nlinear_position(work); 337 | int info; 338 | double chisq0, chisq, rcond; 339 | 340 | /* initialize solver */ 341 | gsl_multifit_nlinear_init(x0, &fdf, work); 342 | 343 | /* store initial cost */ 344 | gsl_blas_ddot(f, f, &chisq0); 345 | 346 | /* iterate until convergence */ 347 | //gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,callback_prb, this, &info, work); 348 | //Without Callback 349 | gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,NULL, NULL, &info, work); 350 | 351 | /* store final cost */ 352 | gsl_blas_ddot(f, f, &chisq); 353 | m_residual_error = chisq; 354 | 355 | /* store cond(J(x)) */ 356 | gsl_multifit_nlinear_rcond(&rcond, work); 357 | 358 | ///---End Solve the system--- 359 | 360 | Eigen::MatrixXd final_values; 361 | final_values.resize(1,(m_joint_pos.size()+1)*(m_number_disks[0]+m_number_disks[1])); 362 | 363 | Eigen::MatrixXd final_boundcon; 364 | final_boundcon.resize(1,(m_joint_pos.size()+1)*(m_number_disks[0]+m_number_disks[1])); 365 | 366 | 367 | for(int i = 0; i < (m_joint_pos.size()+1)*(m_number_disks[0]+m_number_disks[1]); i ++) 368 | { 369 | final_values(0,i) = gsl_vector_get (x, i); 370 | final_boundcon(0,i) = gsl_vector_get (f, i); 371 | } 372 | 373 | m_last_inits = final_values; 374 | gsl_vector_free (x0); 375 | gsl_multifit_nlinear_free(work); 376 | 377 | 378 | 379 | 380 | Eigen::VectorXd length_ss; 381 | length_ss.resize(m_number_disks[0]+m_number_disks[1]); 382 | 383 | for(int i = 0; i < m_number_disks[0]; i++) 384 | { 385 | length_ss(i) = m_length[0]/m_number_disks[0]; 386 | } 387 | 388 | for(int i = m_number_disks[0]; i < m_number_disks[0]+m_number_disks[1]; i++) 389 | { 390 | length_ss(i) = m_length[1]/m_number_disks[1]; 391 | } 392 | 393 | diskFrames.resize(4,4*(m_number_disks[0]+m_number_disks[1]+1)); 394 | 395 | diskFrames.block(0,0,4,4) = Eigen::Matrix4d::Identity(); 396 | 397 | Eigen::MatrixXd Trb; 398 | 399 | for(int i = 1; i < m_number_disks[0]+m_number_disks[1] + 1; i++) 400 | { 401 | Eigen::Matrix4d frame = getDiskTransform(Trb, final_values.transpose(),length_ss,i,0); 402 | diskFrames.block(0,4*i,4,4) = frame; 403 | } 404 | 405 | 406 | if(m_residual_error > 1e-6) 407 | { 408 | return false; 409 | } 410 | 411 | 412 | return true; 413 | 414 | } 415 | 416 | void PseudoRigidBodyModel::get_res(Eigen::MatrixXd &output, Eigen::MatrixXd input) 417 | { 418 | 419 | Eigen::VectorXd length_ss; 420 | length_ss.resize(m_number_disks[0]+m_number_disks[1]); 421 | 422 | for(int i = 0; i < m_number_disks[0]; i++) 423 | { 424 | length_ss(i) = m_length[0]/m_number_disks[0]; 425 | } 426 | 427 | for(int i = m_number_disks[0]; i < m_number_disks[0]+m_number_disks[1]; i++) 428 | { 429 | length_ss(i) = m_length[1]/m_number_disks[1]; 430 | } 431 | 432 | output.resize((m_joint_pos.size()+1)*(m_number_disks[0]+m_number_disks[1]),1); 433 | 434 | int nrb = m_joint_pos.size(); 435 | 436 | Eigen::Vector4d F_prev; 437 | F_prev.setZero(); 438 | Eigen::Vector3d M_prev; 439 | M_prev.setZero(); 440 | 441 | for(int ss_i = m_number_disks[0]+m_number_disks[1]; ss_i > 0; ss_i--) 442 | { 443 | 444 | //Get transformation matrix between disk i and disk i-1 445 | Eigen::Matrix4d diskTransform; 446 | Eigen::MatrixXd Trb; 447 | diskTransform = getDiskTransform(Trb,input,length_ss,ss_i,ss_i-1); 448 | 449 | Eigen::MatrixXd theta; 450 | theta = input.block((nrb+1)*ss_i-(nrb+1),0,nrb-1,1); 451 | double phi = input((nrb+1)*ss_i-(nrb+1) + 3,0); 452 | double epsi = input((nrb+1)*ss_i-(nrb+1) + 4,0); 453 | Eigen::Vector3d ni(std::cos(phi+M_PI/2),std::sin(phi+M_PI/2),0); 454 | 455 | Eigen::Matrix tendon_pos; 456 | for(int i = 0; i < 6; i++) 457 | { 458 | tendon_pos.col(i) = diskTransform.block(0,0,3,3)*m_routing.at(i) + diskTransform.block(0,3,3,1); 459 | } 460 | 461 | Eigen::MatrixXd pt_mat; 462 | pt_mat.resize(4,6); 463 | pt_mat << m_routing.at(0) - tendon_pos.col(0), m_routing.at(1) - tendon_pos.col(1), m_routing.at(2) - tendon_pos.col(2), m_routing.at(3) - tendon_pos.col(3), m_routing.at(4) - tendon_pos.col(4), m_routing.at(5) - tendon_pos.col(5), 464 | 1, 1, 1, 1, 1, 1; 465 | 466 | Eigen::VectorXd norm_ct1; 467 | norm_ct1.resize(6); 468 | for(int i = 0; i < 6; i++) 469 | { 470 | norm_ct1(i) = (m_routing.at(i) - tendon_pos.col(i)).norm(); 471 | } 472 | 473 | Eigen::MatrixXd ct1_mat; 474 | ct1_mat.resize(4,6); 475 | ct1_mat << norm_ct1.transpose(), 476 | norm_ct1.transpose(), 477 | norm_ct1.transpose(), 478 | norm_ct1.transpose(); 479 | 480 | Eigen::MatrixXd Pi; 481 | Pi.resize(4,nrb); 482 | for(int k = 0; k < nrb; k++) 483 | { 484 | Pi.col(k) = Trb.block(0,4*k+3,4,1); 485 | } 486 | 487 | Eigen::MatrixXd F_disk; 488 | F_disk.resize(m_number_disks[0]+m_number_disks[1],6); 489 | F_disk.setZero(); 490 | 491 | for(int i = 0; i < m_number_disks[0]; i++) 492 | { 493 | F_disk.block(i,0,1,6) = m_tau.transpose(); 494 | } 495 | 496 | for(int i = m_number_disks[0]; i < m_number_disks[0] + m_number_disks[1]; i++) 497 | { 498 | F_disk.block(i,3,1,3) = m_tau.block(3,0,3,1).transpose(); 499 | } 500 | 501 | Eigen::MatrixXd F_disk_mat; 502 | F_disk_mat.resize(4,6); 503 | F_disk_mat << F_disk.row(ss_i-1), 504 | F_disk.row(ss_i-1), 505 | F_disk.row(ss_i-1), 506 | F_disk.row(ss_i-1); 507 | 508 | Eigen::MatrixXd Fi; 509 | Fi.resize(4,6); 510 | 511 | Eigen::Vector4d zi = diskTransform.col(2); 512 | 513 | 514 | Eigen::Matrix4d diskTransform1; 515 | Eigen::MatrixXd Trb1; 516 | 517 | if(ss_i < m_number_disks[0]+m_number_disks[1]) 518 | { 519 | diskTransform1 = getDiskTransform(Trb1,input,length_ss,ss_i+1,ss_i-1); 520 | 521 | Eigen::Matrix tendon_pos1; 522 | for(int i = 0; i < 6; i++) 523 | { 524 | tendon_pos1.col(i) = diskTransform1.block(0,0,3,3)*m_routing.at(i) + diskTransform1.block(0,3,3,1); 525 | } 526 | 527 | Eigen::MatrixXd pt_mat1; 528 | pt_mat1.resize(4,6); 529 | pt_mat1 << tendon_pos1.col(0) - tendon_pos.col(0), tendon_pos1.col(1) - tendon_pos.col(1), tendon_pos1.col(2) - tendon_pos.col(2), tendon_pos1.col(3) - tendon_pos.col(3), tendon_pos1.col(4) - tendon_pos.col(4), tendon_pos1.col(5) - tendon_pos.col(5), 530 | 1, 1, 1, 1, 1, 1; 531 | 532 | Eigen::VectorXd norm_ct2; 533 | norm_ct2.resize(6); 534 | for(int i = 0; i < 6; i++) 535 | { 536 | norm_ct2(i) = (tendon_pos1.col(i) - tendon_pos.col(i)).norm(); 537 | } 538 | 539 | Eigen::MatrixXd ct2_mat; 540 | ct2_mat.resize(4,6); 541 | ct2_mat << norm_ct2.transpose(), 542 | norm_ct2.transpose(), 543 | norm_ct2.transpose(), 544 | norm_ct2.transpose(); 545 | 546 | Eigen::MatrixXd F_disk_mat1; 547 | F_disk_mat1.resize(4,6); 548 | F_disk_mat1 << F_disk.row(ss_i), 549 | F_disk.row(ss_i), 550 | F_disk.row(ss_i), 551 | F_disk.row(ss_i); 552 | 553 | Fi = pt_mat.array()/ct1_mat.array()*F_disk_mat.array() + pt_mat1.array()/ct2_mat.array()*F_disk_mat1.array(); 554 | if(ss_i == m_number_disks[0]) 555 | { 556 | for(int i = 3; i < 6; i++) 557 | { 558 | Fi.col(i) = Fi.col(i) - zi.dot(Fi.col(i))*zi/zi.norm()/zi.norm(); 559 | } 560 | 561 | } 562 | else 563 | { 564 | for(int i = 0; i < 6; i++) 565 | { 566 | Fi.col(i) = Fi.col(i) - zi.dot(Fi.col(i))*zi/zi.norm()/zi.norm(); 567 | } 568 | } 569 | 570 | } 571 | else 572 | { 573 | 574 | Fi = pt_mat.array()/ct1_mat.array()*F_disk_mat.array(); 575 | F_prev.setZero(); 576 | M_prev.setZero(); 577 | } 578 | 579 | 580 | Eigen::MatrixXd Mi; 581 | Mi.resize(3,6); 582 | 583 | for(int i = 0; i < 6; i++) 584 | { 585 | Eigen::Vector3d pos = Pi.block(0,nrb-1,3,1) - tendon_pos.col(i); 586 | Eigen::Vector3d F = Fi.block(0,i,3,1); 587 | Mi.col(i) = -1*pos.cross(F); 588 | } 589 | 590 | Eigen::Vector4d F_ext; 591 | F_ext.setZero(); 592 | Eigen::Vector3d M_ext; 593 | M_ext.setZero(); 594 | 595 | Eigen::MatrixXd temp; 596 | Eigen::Matrix4d Rt = getDiskTransform(temp,input,length_ss,ss_i-1,0); 597 | F_ext = Rt.inverse()*Eigen::Vector4d(m_f_ext(0),m_f_ext(1),m_f_ext(2),0); 598 | Eigen::Matrix4d R_ex = getDiskTransform(temp,input,length_ss,m_number_disks[0]+m_number_disks[1],ss_i-1); 599 | Eigen::Vector3d p_ex = R_ex.block(0,3,3,1); 600 | 601 | Eigen::Vector3d pos = Pi.block(0,nrb-1,3,1) - p_ex; 602 | Eigen::Vector3d F_ext_temp = F_ext.block(0,0,3,1); 603 | 604 | M_ext = Rt.block(0,0,3,3).transpose()*m_l_ext - pos.cross(F_ext_temp); 605 | 606 | F_prev = diskTransform*F_prev; 607 | M_prev = diskTransform.block(0,0,3,3)*M_prev; 608 | 609 | Eigen::Vector4d F_net = Fi.rowwise().sum() + F_prev; 610 | F_net(3) = 0; 611 | 612 | Eigen::Vector3d pos1 = diskTransform1.block(0,3,3,1) - Pi.block(0,3,3,1); 613 | Eigen::Vector3d F_temp1 = F_prev.block(0,0,3,1); 614 | Eigen::Vector3d M_net = Mi.rowwise().sum() + M_prev + pos1.cross(F_temp1); 615 | 616 | F_prev = F_net; 617 | M_prev = M_net; 618 | 619 | F_net += F_ext; 620 | F_net(3) = 0; 621 | M_net += M_ext; 622 | 623 | 624 | double I = 0.25*M_PI*(m_ro*m_ro*m_ro*m_ro); 625 | double stiffness = m_youngs_modulus*I/length_ss(ss_i-1); 626 | 627 | double G = m_youngs_modulus/(2*1.3); 628 | 629 | Eigen::Vector3d K(3.25*stiffness,2.84*stiffness,2.95*stiffness); 630 | 631 | Eigen::Vector3d F_temp = F_net.block(0,0,3,1); 632 | for(int k = 0; k < nrb - 1; k++) 633 | { 634 | Eigen::Vector3d pos_temp = Pi.block(0,nrb-1,3,1) - Pi.block(0,k,3,1); 635 | Eigen::Matrix3d Rb = Trb.block(0,4*(k+1),3,3); 636 | Eigen::Vector3d M_netb = Rb.transpose()*(pos_temp.cross(F_temp)+M_net); 637 | output((nrb+1)*ss_i-(nrb+1)+k) = K(k)*theta(k) - M_netb(1); 638 | } 639 | 640 | Eigen::Vector3d posi = diskTransform.block(0,3,3,1); 641 | // Net moment applied at the base of the subsegment 642 | Eigen::Vector3d M_net2 = posi.cross(F_temp)+M_net; 643 | // Moment used for the comptation of phi 644 | Eigen::Vector3d M_phi = M_net2; 645 | M_phi(2) = 0; 646 | 647 | output((nrb+1)*ss_i-(nrb+1) + 3) = ni.transpose()*M_phi - M_phi.norm(); //perpendicularity condition 648 | 649 | // Moment used for the computation of epsi 650 | Eigen::Matrix3d Ri = diskTransform.block(0,0,3,3); 651 | Eigen::Vector3d M_epsi = Ri.transpose()*M_net2; 652 | 653 | output((nrb+1)*ss_i-(nrb+1) + 4) = M_epsi(2)-2*I*G/length_ss(ss_i-1)*epsi; 654 | 655 | 656 | 657 | 658 | 659 | } 660 | 661 | 662 | 663 | } 664 | 665 | void PseudoRigidBodyModel::setKeepInits(bool keep) 666 | { 667 | m_keep_inits = keep; 668 | } 669 | 670 | 671 | -------------------------------------------------------------------------------- /c++/src/tendondrivenrobot.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This code implements different approaches to model the kinematics/statics 3 | of a two segment tendon driven continuum robot and is part of the following 4 | publication: 5 | 6 | How to model tendon-driven continuum robots and benchmark modelling performance 7 | Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | frontiers in Robotics and AI 2021 9 | DOI: 10.3389/frobt.2020.630245 10 | 11 | Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | */ 13 | 14 | #include "tendondrivenrobot.h" 15 | 16 | 17 | 18 | TendonDrivenRobot::TendonDrivenRobot() 19 | { 20 | //Initialize member variables and set up default parameters for TDCR 21 | 22 | mp_cr_model = new CosseratRodModel(); 23 | mp_cc_model = new ConstantCurvatureModel(); 24 | mp_pcc_model = new PiecewiseConstantCurvatureModel(); 25 | mp_prb_model = new PseudoRigidBodyModel(); 26 | mp_sscr_model = new SubsegmentCosseratRodModel(); 27 | 28 | m_keep_inits = false; 29 | 30 | m_length[0] = 0.1; 31 | m_length[1] = 0.1; 32 | m_youngs_modulus = 60e9; 33 | m_number_disks[0] = 10; 34 | m_number_disks[1] = 10; 35 | m_pradius_disks[0] = 0.008; 36 | m_pradius_disks[1] = 0.006; 37 | m_ro = 0.0005; 38 | 39 | std::vector routing; 40 | 41 | Eigen::Vector3d tendon1; 42 | tendon1 << 0, 43 | m_pradius_disks[0], 44 | 0; 45 | 46 | Eigen::Vector3d tendon2; 47 | tendon2 << m_pradius_disks[0]*std::cos(-M_PI/6), 48 | m_pradius_disks[0]*std::sin(-M_PI/6), 49 | 0; 50 | Eigen::Vector3d tendon3; 51 | tendon3 << m_pradius_disks[0]*std::cos(7*M_PI/6), 52 | m_pradius_disks[0]*std::sin(7*M_PI/6), 53 | 0; 54 | 55 | routing.push_back(tendon1); 56 | routing.push_back(tendon2); 57 | routing.push_back(tendon3); 58 | 59 | tendon1 << 0, 60 | m_pradius_disks[1], 61 | 0; 62 | 63 | tendon2 << m_pradius_disks[1]*std::cos(-M_PI/6), 64 | m_pradius_disks[1]*std::sin(-M_PI/6), 65 | 0; 66 | 67 | tendon3 << m_pradius_disks[1]*std::cos(7*M_PI/6), 68 | m_pradius_disks[1]*std::sin(7*M_PI/6), 69 | 0; 70 | 71 | routing.push_back(tendon1); 72 | routing.push_back(tendon2); 73 | routing.push_back(tendon3); 74 | 75 | m_routing = routing; 76 | 77 | m_two_tendons = false; 78 | 79 | mp_cr_model->setRobotParameters(m_length,m_youngs_modulus,m_routing,m_number_disks,m_pradius_disks,m_ro); 80 | mp_cc_model->setRobotParameters(m_length,m_number_disks,m_pradius_disks,m_two_tendons); 81 | mp_pcc_model->setRobotParameters(m_length,m_youngs_modulus,m_routing,m_number_disks,m_pradius_disks,m_ro); 82 | mp_prb_model->setRobotParameters(m_length,m_youngs_modulus,m_routing,m_number_disks,m_pradius_disks,m_ro); 83 | mp_sscr_model->setRobotParameters(m_length,m_youngs_modulus,m_routing,m_number_disks,m_pradius_disks,m_ro); 84 | 85 | m_actuation_values.resize(6,1); 86 | m_actuation_values.setZero(); 87 | m_ext_forces.setZero(); 88 | m_ext_moments.setZero(); 89 | 90 | 91 | } 92 | 93 | 94 | void TendonDrivenRobot::setRobotParameters(std::array length, double youngs_modulus, std::vector routing, std::array number_disks, std::array pradius_disks, double ro, bool two_tendons) 95 | { 96 | 97 | //Update parameters 98 | m_length[0] = length[0]; 99 | m_length[1] = length[1]; 100 | m_youngs_modulus = youngs_modulus; 101 | m_number_disks[0] = number_disks[0]; 102 | m_number_disks[1] = number_disks[1]; 103 | m_pradius_disks[0] = pradius_disks[0]; 104 | m_pradius_disks[1] = pradius_disks[1]; 105 | m_ro = ro; 106 | m_routing = routing; 107 | m_two_tendons = two_tendons; 108 | 109 | 110 | //Update Parameters in model as well 111 | mp_cr_model->setRobotParameters(m_length,m_youngs_modulus,m_routing,m_number_disks,m_pradius_disks,m_ro); 112 | mp_cc_model->setRobotParameters(m_length,m_number_disks,m_pradius_disks,m_two_tendons); 113 | mp_pcc_model->setRobotParameters(m_length,m_youngs_modulus,m_routing,m_number_disks,m_pradius_disks,m_ro); 114 | mp_prb_model->setRobotParameters(m_length,m_youngs_modulus,m_routing,m_number_disks,m_pradius_disks,m_ro); 115 | mp_sscr_model->setRobotParameters(m_length,m_youngs_modulus,m_routing,m_number_disks,m_pradius_disks,m_ro); 116 | 117 | 118 | 119 | } 120 | 121 | TendonDrivenRobot::~TendonDrivenRobot() 122 | { 123 | delete mp_cr_model; 124 | delete mp_cc_model; 125 | delete mp_pcc_model; 126 | delete mp_prb_model; 127 | delete mp_sscr_model; 128 | 129 | } 130 | 131 | bool TendonDrivenRobot::forwardKinematics(Eigen::Matrix4d &ee_frame, Eigen::MatrixXd q, Eigen::Vector3d f_ext, Eigen::Vector3d l_ext, Model model) 132 | { 133 | 134 | //Set current actuation values 135 | m_actuation_values = q; 136 | m_ext_forces = f_ext; 137 | m_ext_moments = l_ext; 138 | 139 | Eigen::MatrixXd diskFrames; 140 | 141 | bool success; 142 | 143 | 144 | //Choose correct model 145 | switch(model) { 146 | case CosseratRod: success = mp_cr_model->forwardKinematics(diskFrames,q,f_ext,l_ext); 147 | break; 148 | 149 | case ConstantCurvature: success = mp_cc_model->forwardKinematics(diskFrames,q); 150 | break; 151 | 152 | case PiecewiseConstantCurvature: success = mp_pcc_model->forwardKinematics(diskFrames,q,f_ext,l_ext); 153 | break; 154 | 155 | case PseudoRigidBody: success = mp_prb_model->forwardKinematics(diskFrames,q,f_ext,l_ext); 156 | break; 157 | 158 | case SubsegmentCosseratRod: success = mp_sscr_model->forwardKinematics(diskFrames,q,f_ext,l_ext); 159 | break; 160 | 161 | default: success = mp_cr_model->forwardKinematics(diskFrames,q,f_ext,l_ext); 162 | break; 163 | } 164 | 165 | 166 | 167 | m_disk_frames = diskFrames; 168 | 169 | m_ee_frame = m_disk_frames.rightCols(4); 170 | 171 | ee_frame = m_ee_frame; 172 | 173 | 174 | return success; 175 | 176 | } 177 | 178 | void TendonDrivenRobot::keepInits(bool keep) 179 | { 180 | m_keep_inits = keep; 181 | mp_cr_model->setKeepInits(keep); 182 | mp_pcc_model->setKeepInits(keep); 183 | mp_prb_model->setKeepInits(keep); 184 | mp_sscr_model->setKeepInits(keep); 185 | } 186 | 187 | Eigen::Matrix4d TendonDrivenRobot::getEEFrame() 188 | { 189 | return m_ee_frame; 190 | } 191 | 192 | 193 | 194 | Eigen::MatrixXd TendonDrivenRobot::getCurrentConfig() 195 | { 196 | return m_actuation_values; 197 | } 198 | 199 | Eigen::MatrixXd TendonDrivenRobot::getDiskFrames() 200 | { 201 | return m_disk_frames; 202 | } 203 | 204 | Eigen::MatrixXd TendonDrivenRobot::getTendonDisplacements() 205 | { 206 | Eigen::MatrixXd tendon_length; 207 | tendon_length.resize(6,1); 208 | tendon_length.setZero(); 209 | 210 | //Iterate through all the disk frames 211 | for(int k = 1; k < m_disk_frames.cols()/4; k++) 212 | { 213 | Eigen::Matrix4d disk_frame; 214 | disk_frame = m_disk_frames.block(0,4*k,4,4); 215 | 216 | Eigen::Matrix4d disk_frame_prev; 217 | disk_frame_prev = m_disk_frames.block(0,4*(k-1),4,4); 218 | 219 | Eigen::Matrix4d routing1 = Eigen::Matrix4d::Identity(); 220 | Eigen::Matrix4d routing2 = Eigen::Matrix4d::Identity(); 221 | Eigen::Matrix4d routing3 = Eigen::Matrix4d::Identity(); 222 | Eigen::Matrix4d routing4 = Eigen::Matrix4d::Identity(); 223 | Eigen::Matrix4d routing5 = Eigen::Matrix4d::Identity(); 224 | Eigen::Matrix4d routing6 = Eigen::Matrix4d::Identity(); 225 | 226 | routing1.block(0,3,3,1) = m_routing.at(0); 227 | routing2.block(0,3,3,1) = m_routing.at(1); 228 | routing3.block(0,3,3,1) = m_routing.at(2); 229 | routing4.block(0,3,3,1) = m_routing.at(3); 230 | routing5.block(0,3,3,1) = m_routing.at(4); 231 | routing6.block(0,3,3,1) = m_routing.at(5); 232 | 233 | if(k < m_number_disks[0] + 1) 234 | { 235 | tendon_length(0) += ((disk_frame*routing1).block(0,3,3,1) - (disk_frame_prev*routing1).block(0,3,3,1)).norm(); 236 | tendon_length(1) += ((disk_frame*routing2).block(0,3,3,1) - (disk_frame_prev*routing2).block(0,3,3,1)).norm(); 237 | tendon_length(2) += ((disk_frame*routing3).block(0,3,3,1) - (disk_frame_prev*routing3).block(0,3,3,1)).norm(); 238 | } 239 | else 240 | { 241 | tendon_length(3) += ((disk_frame*routing4).block(0,3,3,1) - (disk_frame_prev*routing4).block(0,3,3,1)).norm(); 242 | tendon_length(4) += ((disk_frame*routing5).block(0,3,3,1) - (disk_frame_prev*routing5).block(0,3,3,1)).norm(); 243 | tendon_length(5) += ((disk_frame*routing6).block(0,3,3,1) - (disk_frame_prev*routing6).block(0,3,3,1)).norm(); 244 | } 245 | } 246 | 247 | 248 | tendon_length(0) = m_length[0] - tendon_length(0); 249 | tendon_length(1) = m_length[0] - tendon_length(1); 250 | tendon_length(2) = m_length[0] - tendon_length(2); 251 | 252 | tendon_length(3) = m_length[1] - tendon_length(3); 253 | tendon_length(4) = m_length[1] - tendon_length(4); 254 | tendon_length(5) = m_length[1] - tendon_length(5); 255 | 256 | return tendon_length; 257 | } 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | -------------------------------------------------------------------------------- /matlab/Models/CC/CC_solver.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | 14 | function [ var_cc ] = CC_solver( l_d,n,r_disk ) 15 | 16 | %section 1 17 | [kappa1,phi1,l1]=configuration(l_d(1,:),n(1),r_disk); 18 | 19 | %section 2 20 | [kappa2,phi2,l2]=configuration(l_d(2,:),n(2),r_disk); 21 | 22 | var_cc = [kappa1 kappa2;phi1 phi2;l1 l2]; 23 | end 24 | 25 | -------------------------------------------------------------------------------- /matlab/Models/CC/calctendonlength.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | 14 | function [delta_l] = calctendonlength(g,s,param,l) 15 | %Calculates tendon lengths (spacer disk location neglected) 16 | 17 | %tendon location 18 | r1=[param.tendon.r1; 1]; 19 | r2=[param.tendon.r2; 1]; 20 | r3=[param.tendon.r3; 1]; 21 | 22 | %total cable length 23 | l1=l(1); 24 | l2=sum(l(1:2)); 25 | 26 | %indices 27 | si=[1:1:length(s)]; 28 | end1=find(abs(s-l1)3 107 | force1=force1-(zi'*force1)*zi; %removing the z component of force for the second set of tendons 108 | end 109 | pos=tendon_pos_cur(:,m)-p_c; 110 | F_tendon=F_tendon+force1; 111 | M_tendon=M_tendon+cross(pos,force1); 112 | end 113 | res_mat((k-1)*6+1:(k-1)*6+3)=res_mat((k-1)*6+1:(k-1)*6+3)-F_tendon; 114 | res_mat((k-1)*6+4:(k-1)*6+6)=res_mat((k-1)*6+4:(k-1)*6+6)-M_tendon; 115 | else 116 | for m=1:6 117 | dir1=tendon_pos_prev(:,m)-tendon_pos_cur(:,m); 118 | dir2=tendon_pos_next(:,m)-tendon_pos_cur(:,m); 119 | force1=Fdisk(k,m)*dir1./norm(dir1)+Fdisk(k,m)*dir2./norm(dir2); 120 | force1=force1-(zi'*force1)*zi; 121 | F_tendon=F_tendon+force1; 122 | pos=tendon_pos_cur(:,m)-p_c; 123 | M_tendon=M_tendon+cross(pos,force1); 124 | end 125 | res_mat((k-1)*6+1:(k-1)*6+3)=res_mat((k-1)*6+1:(k-1)*6+3)-F_tendon; 126 | res_mat((k-1)*6+4:(k-1)*6+6)=res_mat((k-1)*6+4:(k-1)*6+6)-M_tendon; 127 | end 128 | end 129 | 130 | res=res_mat; 131 | end 132 | 133 | function [b] = run_IVP(y,k) 134 | %Runs initial value problem 135 | 136 | %Break Points (section ends) 137 | 138 | 139 | %initial values 140 | 141 | 142 | %numerical integration routine 143 | options=odeset('AbsTol',1e-6,'RelTol',1e-3,'InitialStep',0.005); 144 | 145 | [~,b]=ode45(@(s,y) deriv(y,k),[Break_Points(k),Break_Points(k+1)],y,options); 146 | 147 | b= unique(b,'rows','stable'); %deleting double lines, 'stable': same order as before 148 | end 149 | 150 | function [dy] = deriv(y,k) 151 | %This function contains the differential equations describing the system 152 | %Overview of variables 153 | % u: angular rate of change of g 154 | % v: linear rate of change of g 155 | % R: orientation of backbone 156 | 157 | u = [y(16);y(17);y(18)]; 158 | v = [y(13);y(14);y(15)]; 159 | R = reshape(y(4:12),3,3); 160 | 161 | %external force- and momentdistribution 162 | % fe=param.roh*sum(A_tube(k:2))*9.81*[0;0;1]; 163 | fe=[0;0;0]; 164 | le=[0;0;0]; 165 | 166 | % Differential equations 167 | v_dot=-Kse\(lie(u)*Kse*(v-[0;0;1])+R'*fe); 168 | u_dot=-Kbt\(lie(u)*Kbt*u+lie(v)*Kse*(v-[0;0;1])+R'*le); 169 | p_dot=R*v; 170 | R_dot=R*lie(u); 171 | s_dot=1; 172 | 173 | dy=[p_dot;reshape(R_dot,9,1);v_dot;u_dot;s_dot]; 174 | 175 | end 176 | 177 | 178 | end 179 | 180 | -------------------------------------------------------------------------------- /matlab/Models/VC/VCref_solver.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | 14 | function [y_total,m_states] = VCref_solver(init_guess,F,Ftex,Mtex,L,param,n) 15 | 16 | Break_Points=[0:L(1)/n(1):L(1) L(1)+L(2)/n(2):L(2)/n(2):sum(L(1:2))]; 17 | [Kbt,Kse]=get_stiffness(1,param); 18 | 19 | %options for fsolve 20 | options1 = optimset('Display','iter','TolFun',1e-4); 21 | % tendon locations 22 | r=[param.tendon.r1 param.tendon.r2 param.tendon.r3 param.tendon.r1 param.tendon.r2 param.tendon.r3]; 23 | 24 | y_total = []; 25 | 26 | tic; 27 | [init,res,exitflag,output]=fsolve(@optim_f, init_guess, options1); 28 | toc; 29 | 30 | %% solver 31 | 32 | function [res] = optim_f(input) 33 | % Iterate to find an init guess so that the boundary conditions at the tip 34 | % of the tube are satisfied 35 | 36 | m_states=zeros(sum(n)+1,19); 37 | %solve initial value problem 38 | res_mat=zeros(6*sum(n),1); 39 | s_init=0; 40 | R_init=[1,0,0;0,1,0;0,0,1]; 41 | p_init=[0,0,0]; 42 | 43 | init_state_segment=[p_init,reshape(R_init,1,9),input(1:6)',s_init]; 44 | m_states(1,:)=init_state_segment; 45 | 46 | y_total = []; 47 | 48 | for k=1:sum(n) 49 | 50 | [y]=run_IVP(init_state_segment,k); 51 | y_total = [y_total;y]; 52 | m_states(k+1,:)=y(end,:); 53 | 54 | p_c = y(end,1:3)'; 55 | R_c = reshape(y(end,4:12),3,3); 56 | v_c = y(end,13:15)'; 57 | u_c = y(end,16:18)'; 58 | 59 | u_n=[0;0;0]; v_n=[0;0;1]; 60 | if k3 129 | force1=force1-(zi'*force1)*zi; %removing the z component of force for the second set of tendons 130 | end 131 | pos=tendon_pos_cur(:,m)-p_c; 132 | F_tendon=F_tendon+force1; 133 | M_tendon=M_tendon+cross(pos,force1); 134 | end 135 | res_mat((k-1)*6+1:(k-1)*6+3)=res_mat((k-1)*6+1:(k-1)*6+3)-F_tendon; 136 | res_mat((k-1)*6+4:(k-1)*6+6)=res_mat((k-1)*6+4:(k-1)*6+6)-M_tendon; 137 | else 138 | for m=1:6 139 | dir1=tendon_pos_prev(:,m)-tendon_pos_cur(:,m); 140 | dir2=tendon_pos_next(:,m)-tendon_pos_cur(:,m); 141 | force1=Fdisk(k,m)*dir1./norm(dir1)+Fdisk(k,m)*dir2./norm(dir2); 142 | force1=force1-(zi'*force1)*zi; 143 | F_tendon=F_tendon+force1; 144 | pos=tendon_pos_cur(:,m)-p_c; 145 | M_tendon=M_tendon+cross(pos,force1); 146 | end 147 | res_mat((k-1)*6+1:(k-1)*6+3)=res_mat((k-1)*6+1:(k-1)*6+3)-F_tendon; 148 | res_mat((k-1)*6+4:(k-1)*6+6)=res_mat((k-1)*6+4:(k-1)*6+6)-M_tendon; 149 | end 150 | end 151 | 152 | res = res_mat; 153 | end 154 | 155 | function [y] = run_IVP(y_init,k) 156 | %Runs initial value problem 157 | 158 | %numerical integration routine 159 | options=odeset('AbsTol',1e-6,'RelTol',1e-3,'InitialStep',0.005); 160 | 161 | [~,y]=ode45(@(s,y) deriv(y,k),[Break_Points(k),Break_Points(k+1)],y_init,options); 162 | 163 | y= unique(y,'rows','stable'); %deleting double lines, 'stable': same order as before 164 | end 165 | 166 | function [dy] = deriv(y,k) 167 | %This function contains the differential equations describing the system 168 | %Overview of variables 169 | % u: angular rate of change of g 170 | % v: linear rate of change of g 171 | % R: orientation of backbone 172 | 173 | u = [y(16);y(17);y(18)]; 174 | v = [y(13);y(14);y(15)]; 175 | R = reshape(y(4:12),3,3); 176 | 177 | %external force- and momentdistribution 178 | fe=[0;0;0]; 179 | le=[0;0;0]; 180 | 181 | % Differential equations 182 | v_dot=-Kse\(lie(u)*Kse*(v-[0;0;1])+R'*fe); 183 | u_dot=-Kbt\(lie(u)*Kbt*u+lie(v)*Kse*(v-[0;0;1])+R'*le); 184 | p_dot=R*v; 185 | R_dot=R*lie(u); 186 | s_dot=1; 187 | 188 | dy=[p_dot;reshape(R_dot,9,1);v_dot;u_dot;s_dot]; 189 | 190 | end 191 | 192 | 193 | end 194 | 195 | -------------------------------------------------------------------------------- /matlab/Models/VC/boundcond.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | function [res,F_sigma,L_sigma] = boundcond(sigma_ind,tau,F,sect,y,r,param) 14 | %Boundary conditions due to tendon termination 15 | % sigma_ind: index of y for tendon termination at s=sigma 16 | % tau: tendon tension for current section 17 | % lastsect: 1 if last section, 0 if not 18 | % y: state variable set 19 | % Kse,Kbt: stiffness matrices 20 | % refconfig: undeformed reference configuration 21 | % r: tendon locations 22 | 23 | %kinematic variables at section end 24 | v_sigma=y(sigma_ind,13:15)'; 25 | u_sigma=y(sigma_ind,16:18)'; 26 | R_sigma=reshape(y(sigma_ind,4:12),3,3); 27 | 28 | %forces and moments applied by tendon loads at section end 29 | % p_dot=R*(lie(u)*r+v) 30 | p1_dot_sigma=R_sigma*([-u_sigma(3)*r(2,1);u_sigma(3)*r(1,1);-u_sigma(2)*r(1,1)+u_sigma(1)*r(2,1)]+v_sigma); 31 | p2_dot_sigma=R_sigma*([-u_sigma(3)*r(2,2);u_sigma(3)*r(1,2);-u_sigma(2)*r(1,2)+u_sigma(1)*r(2,2)]+v_sigma); 32 | p3_dot_sigma=R_sigma*([-u_sigma(3)*r(2,3);u_sigma(3)*r(1,3);-u_sigma(2)*r(1,3)+u_sigma(1)*r(2,3)]+v_sigma); 33 | 34 | % F_sigma=F1_sigma+F2_sigma+F3_sigma; 35 | F_sigma=-(tau(1)/norm(p1_dot_sigma))*p1_dot_sigma... 36 | -(tau(2)/norm(p2_dot_sigma))*p2_dot_sigma... 37 | -(tau(3)/norm(p3_dot_sigma))*p3_dot_sigma; 38 | 39 | % L_sigma=L1_sigma+L2_sigma+L3_sigma; 40 | L_sigma=-tau(1)*lie(R_sigma*r(:,1))*(1/norm(p1_dot_sigma))*p1_dot_sigma... 41 | -tau(2)*lie(R_sigma*r(:,2))*(1/norm(p2_dot_sigma))*p2_dot_sigma... 42 | -tau(3)*lie(R_sigma*r(:,3))*(1/norm(p3_dot_sigma))*p3_dot_sigma; 43 | 44 | 45 | if sect<2 %not last section 46 | %kinematic variables and orientation just before and just after section end 47 | v_sigmamin=y(sigma_ind-1,13:15)'; 48 | v_sigmaplu=y(sigma_ind+1,13:15)'; 49 | u_sigmamin=y(sigma_ind-1,16:18)'; 50 | u_sigmaplu=y(sigma_ind+1,16:18)'; 51 | R_sigmamin=reshape(y(sigma_ind-1,4:12),3,3); 52 | R_sigmaplu=reshape(y(sigma_ind+1,4:12),3,3); 53 | 54 | %constitutive laws just before and just after section end: internal force, internal moment 55 | [Kbt,Kse] = get_stiffness(sect,param); 56 | n_sigmamin=R_sigmamin*Kse*(v_sigmamin-[0;0;1]); 57 | m_sigmamin=R_sigmamin*Kbt*u_sigmamin; 58 | 59 | [Kbt,Kse] = get_stiffness(sect+1,param); 60 | n_sigmaplu=R_sigmaplu*Kse*(v_sigmaplu-[0;0;1]); 61 | m_sigmaplu=R_sigmaplu*Kbt*u_sigmaplu; 62 | 63 | %difference between internal load and external load should be minimized 64 | res = [n_sigmamin-n_sigmaplu-F_sigma;m_sigmamin-m_sigmaplu-L_sigma]; 65 | 66 | else %last section, sigma+ doesn't exist 67 | %constitutive laws at section end: internal force, internal moment 68 | [Kbt,Kse] = get_stiffness(sect,param); 69 | n_sigma=R_sigma*Kse*(v_sigma-[0;0;1]); 70 | m_sigma=R_sigma*Kbt*u_sigma; 71 | 72 | %difference between internal load and external load should be minimized 73 | res = [n_sigma-F_sigma-F;m_sigma-L_sigma]; 74 | 75 | end 76 | 77 | end 78 | 79 | -------------------------------------------------------------------------------- /matlab/Models/VC/get_stiffness.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | function [Kbtges,Kseges] = get_stiffness(k,param) 14 | % Assign Stiffnesses based on which tubes are present 15 | % k:number of current section 16 | 17 | if k==1 18 | Kbtges=param.tube1.Kbt+param.tube2.Kbt; 19 | Kseges=param.tube1.Kse+param.tube2.Kse; 20 | elseif k==2 21 | Kbtges=param.tube2.Kbt; 22 | Kseges=param.tube2.Kse; 23 | end 24 | 25 | 26 | end 27 | 28 | -------------------------------------------------------------------------------- /matlab/Models/VC/intermedquant.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | function [A,B,G,H,c,d] = intermedquant2(u,v,R,r,Kse,Kbt,tau_stell,fe,le,k) 14 | %Calculates intermediate vector and matrix quantities depending on which 15 | %sections are present 16 | %----INPUT---- 17 | % k: numer of section 18 | % u,v:angular and linear rate of change 19 | % R: orientation 20 | % param: parameters of manipulator 21 | % Kse,Kbt: stiffness matrices 22 | % refconfig: reference configuartion 23 | % tau_stell: tendon tension 24 | % fe,le: external force- and momentdistribution 25 | %----OUTPUT---- 26 | % A,B,G,H: intermediate matrix quantities 27 | % c,d: intermediate vector quantities 28 | 29 | 30 | 31 | % tendon paths in body frame 32 | % pib_dot=lie(u)*ri+v; 33 | p1b_dot=[-u(3)*r(2,1);u(3)*r(1,1);-u(2)*r(1,1)+u(1)*r(2,1)]+v; 34 | p2b_dot=[-u(3)*r(2,2);u(3)*r(1,2);-u(2)*r(1,2)+u(1)*r(2,2)]+v; 35 | p3b_dot=[-u(3)*r(2,3);u(3)*r(1,3);-u(2)*r(1,3)+u(1)*r(2,3)]+v; 36 | 37 | %% For all sections 38 | % (tendons of section 2 are always present) 39 | 40 | % Ai=-tau_i*((lie(pib_dot))^2/norm(pib_dot)^3) 41 | A21=-(tau_stell(1,2)/norm(p1b_dot)^3)*[0 -p1b_dot(3) p1b_dot(2);p1b_dot(3) 0 -p1b_dot(1);-p1b_dot(2) p1b_dot(1) 0]*[0 -p1b_dot(3) p1b_dot(2);p1b_dot(3) 0 -p1b_dot(1);-p1b_dot(2) p1b_dot(1) 0]; 42 | A22=-(tau_stell(2,2)/norm(p2b_dot)^3)*[0 -p2b_dot(3) p2b_dot(2);p2b_dot(3) 0 -p2b_dot(1);-p2b_dot(2) p2b_dot(1) 0]*[0 -p2b_dot(3) p2b_dot(2);p2b_dot(3) 0 -p2b_dot(1);-p2b_dot(2) p2b_dot(1) 0]; 43 | A23=-(tau_stell(3,2)/norm(p3b_dot)^3)*[0 -p3b_dot(3) p3b_dot(2);p3b_dot(3) 0 -p3b_dot(1);-p3b_dot(2) p3b_dot(1) 0]*[0 -p3b_dot(3) p3b_dot(2);p3b_dot(3) 0 -p3b_dot(1);-p3b_dot(2) p3b_dot(1) 0]; 44 | A=A21+A22+A23; 45 | 46 | % Bi=lie(ri)*Ai 47 | B21=[0 0 r(2,1);0 0 -r(1,1);-r(2,1) r(1,1) 0]*A21; 48 | B22=[0 0 r(2,2);0 0 -r(1,2);-r(2,2) r(1,2) 0]*A22; 49 | B23=[0 0 r(2,3);0 0 -r(1,3);-r(2,3) r(1,3) 0]*A23; 50 | B=B21+B22+B23; 51 | 52 | % G= -sum Ai*lie(ri) 53 | G =-(A21*[0 0 r(2,1);0 0 -r(1,1);-r(2,1) r(1,1) 0]+... 54 | A22*[0 0 r(2,2);0 0 -r(1,2);-r(2,2) r(1,2) 0]+... 55 | A23*[0 0 r(2,3);0 0 -r(1,3);-r(2,3) r(1,3) 0]); 56 | % H= -sum Bi*lie(ri) 57 | H =-(B21*[0 0 r(2,1);0 0 -r(1,1);-r(2,1) r(1,1) 0]+... 58 | B22*[0 0 r(2,2);0 0 -r(1,2);-r(2,2) r(1,2) 0]+... 59 | B23*[0 0 r(2,3);0 0 -r(1,3);-r(2,3) r(1,3) 0]); 60 | 61 | % ai=Ai*lie(u)*pib_dot 62 | a21=A21*[0 -u(3) u(2);u(3) 0 -u(1); -u(2) u(1) 0]*p1b_dot; 63 | a22=A22*[0 -u(3) u(2);u(3) 0 -u(1); -u(2) u(1) 0]*p2b_dot; 64 | a23=A23*[0 -u(3) u(2);u(3) 0 -u(1); -u(2) u(1) 0]*p3b_dot; 65 | a=a21+a22+a23; 66 | 67 | % b=lie(ri)*ai 68 | b=[r(2,1)*a21(3);-r(1,1)*a21(3);-r(2,1)*a21(1)+r(1,1)*a21(2)]+... 69 | [r(2,2)*a22(3);-r(1,2)*a22(3);-r(2,2)*a22(1)+r(1,2)*a22(2)]+... 70 | [r(2,3)*a23(3);-r(1,3)*a23(3);-r(2,3)*a23(1)+r(1,3)*a23(2)]; 71 | 72 | %% Additions depending on current section 73 | 74 | if k==1 %section 1 75 | 76 | % Ai=-tau_i*((lie(pib_dot))^2/norm(pib_dot)^3) 77 | A11=-(tau_stell(1,1)/norm(p1b_dot)^3)*[0 -p1b_dot(3) p1b_dot(2);p1b_dot(3) 0 -p1b_dot(1);-p1b_dot(2) p1b_dot(1) 0]*[0 -p1b_dot(3) p1b_dot(2);p1b_dot(3) 0 -p1b_dot(1);-p1b_dot(2) p1b_dot(1) 0]; 78 | A12=-(tau_stell(2,1)/norm(p2b_dot)^3)*[0 -p2b_dot(3) p2b_dot(2);p2b_dot(3) 0 -p2b_dot(1);-p2b_dot(2) p2b_dot(1) 0]*[0 -p2b_dot(3) p2b_dot(2);p2b_dot(3) 0 -p2b_dot(1);-p2b_dot(2) p2b_dot(1) 0]; 79 | A13=-(tau_stell(3,1)/norm(p3b_dot)^3)*[0 -p3b_dot(3) p3b_dot(2);p3b_dot(3) 0 -p3b_dot(1);-p3b_dot(2) p3b_dot(1) 0]*[0 -p3b_dot(3) p3b_dot(2);p3b_dot(3) 0 -p3b_dot(1);-p3b_dot(2) p3b_dot(1) 0]; 80 | A=A+A11+A12+A13; 81 | 82 | % Bi=lie(ri)*Ai 83 | B11=[0 0 r(2,1);0 0 -r(1,1);-r(2,1) r(1,1) 0]*A11; 84 | B12=[0 0 r(2,2);0 0 -r(1,2);-r(2,2) r(1,2) 0]*A12; 85 | B13=[0 0 r(2,3);0 0 -r(1,3);-r(2,3) r(1,3) 0]*A13; 86 | B=B+B11+B12+B13; 87 | 88 | % G= -sum Ai*lie(ri) 89 | G =G-(A11*[0 0 r(2,1);0 0 -r(1,1);-r(2,1) r(1,1) 0]+... 90 | A12*[0 0 r(2,2);0 0 -r(1,2);-r(2,2) r(1,2) 0]+... 91 | A13*[0 0 r(2,3);0 0 -r(1,3);-r(2,3) r(1,3) 0]); 92 | 93 | % H= -sum Bi*lie(ri) 94 | H =H-(B11*[0 0 r(2,1);0 0 -r(1,1);-r(2,1) r(1,1) 0]+... 95 | B12*[0 0 r(2,2);0 0 -r(1,2);-r(2,2) r(1,2) 0]+... 96 | B13*[0 0 r(2,3);0 0 -r(1,3);-r(2,3) r(1,3) 0]); 97 | 98 | % ai=Ai*lie(u)*pib_dot 99 | a11=A11*[0 -u(3) u(2);u(3) 0 -u(1); -u(2) u(1) 0]*p1b_dot; 100 | a12=A12*[0 -u(3) u(2);u(3) 0 -u(1); -u(2) u(1) 0]*p2b_dot; 101 | a13=A13*[0 -u(3) u(2);u(3) 0 -u(1); -u(2) u(1) 0]*p3b_dot; 102 | a=a+a11+a12+a13; 103 | 104 | % b=lie(ri)*ai 105 | b=b+[r(2,1)*a11(3);-r(1,1)*a11(3);-r(2,1)*a11(1)+r(1,1)*a11(2)]+... 106 | [r(2,2)*a12(3);-r(1,2)*a12(3);-r(2,2)*a12(1)+r(1,2)*a12(2)]+... 107 | [r(2,3)*a13(3);-r(1,3)*a13(3);-r(2,3)*a13(1)+r(1,3)*a13(2)]; 108 | 109 | end 110 | 111 | % v = [0;0;1]; 112 | c=-lie(u)*Kbt*u-lie(v)*Kse*(v-[0;0;1])-R'*le-b; 113 | d=-lie(u)*Kse*(v-[0;0;1])-R'*fe-a; 114 | 115 | 116 | end -------------------------------------------------------------------------------- /matlab/Models/VC/lie.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | function [xmat] = lie(xvec) 14 | %Converts a vector xvec (element of R3) to a matrix xmat (element of so(3)) 15 | 16 | xmat= [0 -xvec(3) xvec(2); ... 17 | xvec(3) 0 -xvec(1); ... 18 | -xvec(2) xvec(1) 0]; 19 | 20 | end 21 | 22 | -------------------------------------------------------------------------------- /matlab/Models/VC/plot_tdcr_vc.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | function [Fig] = plot_tdcr_vc(g,s,l) 14 | %Visualization of the backbone of the continuum robot for 2 sections 15 | fig=gcf; 16 | hold on 17 | 18 | radius1=4; 19 | radius2=3; 20 | 21 | lmax=l(1)+l(2); 22 | tube_ind1 = find(s<=l(1)); 23 | tube_end1=tube_ind1(end); 24 | 25 | % draw backbone 26 | plot3(g(1:tube_end1,13), g(1:tube_end1,14),g(1:tube_end1,15),... 27 | 'LineWidth',radius1,'Color',[0 0 1]); 28 | plot3(g(tube_end1:end,13), g(tube_end1:end,14),g(tube_end1:end,15),... 29 | 'LineWidth',radius2,'Color',[0 1 0]); 30 | 31 | %Ground plate 32 | color = [1 1 1]*0.9; 33 | squaresize = 0.02; 34 | fill3([1 1 -1 -1]*squaresize,[-1 1 1 -1]*squaresize,[0 0 0 0],color); 35 | 36 | axis([-lmax lmax -lmax lmax 0 lmax]) 37 | xlabel('x (m)') 38 | ylabel('y (m)') 39 | zlabel('z (m)') 40 | grid on 41 | view([0.5 0.5 0.5]) 42 | % view([90,0]) 43 | daspect([1 1 1]) 44 | 45 | end 46 | 47 | -------------------------------------------------------------------------------- /matlab/Models/VC/setup_param_vc.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | function [param] = setup_param_vc(L1,ro1,ri1,m1,L2,ro2,ri2,m2,E,nu,roh,r_disk,m_disk,n_disk,l,r1,r2,r3) 14 | %Sorts parameters in a struct 15 | % E=60*10^9;%60*10^9; % [Pa] 54? 16 | % nu=0.3; 17 | % roh=6450; %[kg/m^3] 18 | 19 | G=E/(2*(1+nu)); 20 | 21 | %second moments of area I=Ixx=Iyy 22 | I1=1/4*pi*(ro1^4-ri1^4); 23 | I2=1/4*pi*(ro2^4-ri2^4); 24 | 25 | %areas of cross section 26 | A1=pi*(ro1^2-ri1^2); 27 | A2=pi*(ro2^2-ri2^2); 28 | 29 | %stiffness matrices 30 | Kbt1=diag([E*I1,E*I1,G*2*I1]); 31 | Kbt2=diag([E*I2,E*I2,G*2*I2]); 32 | Kse1=diag([G*A1,G*A1,E*A1]); 33 | Kse2=diag([G*A2,G*A2,E*A2]); 34 | % Kse1 = zeros(3); 35 | % Kse2 = zeros(3); 36 | 37 | 38 | tube1=struct('L',L1,'ro',ro1,'ri',ri1,'A',A1,'Kbt',Kbt1,'Kse',Kse1,'q',m1*9.81); 39 | tube2=struct('L',L2,'ro',ro2,'ri',ri2,'A',A2,'Kbt',Kbt2,'Kse',Kse2,'q',m2*9.81); 40 | 41 | %position of tendon ri=[xi yi 0]' 42 | % r1=[0 r_disk 0]'; 43 | % r2=[cos(30*pi/180)*r_disk -sin(30*pi/180)*r_disk 0]'; 44 | % r3=[-cos(30*pi/180)*r_disk -sin(30*pi/180)*r_disk 0]'; 45 | 46 | tendon=struct('r1',r1(1:3),'r2',r2(1:3),'r3',r3(1:3)); 47 | 48 | param = struct('tube1',tube1,'tube2',tube2,'tendon',tendon,'roh',roh,'q_disks',(n_disk*m_disk*9.81)*ones(size(l))./l); 49 | end 50 | 51 | -------------------------------------------------------------------------------- /matlab/Parameters/tdcr_2segments.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | 14 | % Geometrical and mechanical properties of the 2 sections TDCR 15 | 16 | % Subsegment number and length 17 | n = [10;10]; %number of spacerdisks on the 2 sections, n(1) corresponds to proximal section 18 | n_disk=sum(n); %number of spacerdisks 19 | L = [200e-3;200e-3]; % Lenght of the section in m 20 | l=[L(1)/n(1)*ones(1,n(1)) L(2)/n(2)*ones(1,n(2))]; %array of segment lengths of each subsegment 21 | 22 | % Tendon position on the disks 23 | r_disk=10*1e-3; %distance of routing channels to backbone-center[m] 24 | % Tendons actuating the proximal segment 25 | r1=[r_disk 0 0 1]'; 26 | r2=[0 r_disk 0 1]'; 27 | r3=[-r_disk 0 0 1]'; 28 | r4=[0 -r_disk 0 1]'; 29 | % Tendons actuating the distal segment 30 | r5=[r_disk*cos(pi/4) r_disk*sin(pi/4) 0 1]'; 31 | r6=[r_disk*cos(3*pi/4) r_disk*sin(3*pi/4) 0 1]'; 32 | r7=[r_disk*cos(5*pi/4) r_disk*sin(5*pi/4) 0 1]'; 33 | r8=[r_disk*cos(-pi/4) r_disk*sin(-pi/4) 0 1]'; 34 | p_tendon=[r1 r2 r3 r4 r5 r6 r7 r8]; %additional tendons can be added through additional columns 35 | 36 | % Tendons actuating the proximal segment 37 | r1=[0 r_disk 0 1]'; 38 | r2=[r_disk*cos(-pi/6) r_disk*sin(-pi/6) 0 1]'; 39 | r3=[r_disk*cos(7*pi/6) r_disk*sin(7*pi/6) 0 1]'; 40 | % Tendons actuating the distal segment 41 | r4=[0 r_disk 0 1]'; 42 | r5=[r_disk*cos(-pi/6) r_disk*sin(-pi/6) 0 1]'; 43 | r6=[r_disk*cos(7*pi/6) r_disk*sin(7*pi/6) 0 1]'; 44 | p_tendon=[r1 r2 r3 r4 r5 r6]; 45 | 46 | % Tendon tension 47 | % F = [8 2 8 2 0 0 0 0]; 48 | F = [8 0 0 0 0 0]; 49 | 50 | % Backbone mechanical and geometrical properties 51 | E=54*10^9;% Youngs modulus 52 | nu=0.3; %Poissons ratio 53 | G=E/(2*(1+nu)); %Shear modulus 54 | ro=1.4/2*10^-3; %outer radius of bb 55 | ri=0; %inner radius of bb 56 | I=1/4*pi*(ro^4-ri^4); %moment of inertia 57 | g=0; %9.81; %acceleration due to gravity 58 | m_bb=0.0115*l*g; %mass of backbone %weight of the backbone expressed in kg/m multiplied by length and g 59 | m_disk=0.2*1e-3*ones(1,n_disk)*g; %array of masses of each spacerdisk 60 | 61 | % External tip forces and moments 62 | Ftex = [0;0;0;0]; % Force applied at the tip, expressed in global frame 63 | Mtex = [0;0;0;0]; % Moment applied at the tip -------------------------------------------------------------------------------- /matlab/main.asv: -------------------------------------------------------------------------------- 1 | %% Matlab code for the case study of the TDCR modeling review 2 | clear all; 3 | close all; 4 | 5 | addpath('Parameters/'); 6 | addpath('Models/CCsub/'); 7 | addpath('Models/Cosserat_2s/'); 8 | addpath('Models/PRBM/'); 9 | addpath('Models/CC_kin/'); 10 | 11 | %% Load TDCR parameters 12 | tdcr_2segments; 13 | F = [2 0 0 0 1 0]; 14 | Ftex = [0;0;0;0]; 15 | 16 | %% Solving the CCsub model 17 | var0 = 0.01*ones(1,3*n_disk); 18 | var_ccsub = CCsub_solver(n,l,F,p_tendon,m_disk,m_bb,E,I,G,Ftex,Mtex,var0); 19 | 20 | %% plotting 21 | figure(1); 22 | [p_plot,p_last] = plot_tdcr_ccsub(l,p_tendon(1:3,:),n_disk,var_pcca,r_disk); 23 | view(90,0); 24 | 25 | % Construct robot shape with CCsub output 26 | s_disk = [0 L(1)/n(1):L(1)/(n(1)):L(1) L(1)+L(2)/n(2):L(2)/n(2):sum(L)]; 27 | [stest,pos_ccsub] = construct_tdcr_ccsub(l,p_tendon(1:3,:),n_disk,var_pcca); 28 | 29 | %% Solving the VC model 30 | 31 | % Set up TDCR parameters 32 | param=setup_param_vc(0,0,0,0,sum(L),ro,ri,m_bb(1),E,nu,0,r_disk,m_disk(1),n_disk,L,r1(1:3),r2(1:3),r3(1:3)); 33 | Fc = [F(1:3);F(4:6)]'; 34 | 35 | % Shooting method for model solution 36 | 37 | %initial guess for linear and angular rate of change 38 | v_init_guess=[0;0;1]; 39 | u_init_guess=[0;0;0]; 40 | init_guess=[v_init_guess;u_init_guess]; 41 | 42 | %solving the boundary value problem 43 | [y,vu_f] = VC_solver(init_guess,Fc,Ftex(1:3),L,param); 44 | sc= y(:,19); 45 | g = [y(:,4:6) zeros(size(y,1),1) y(:,7:9) zeros(size(y,1),1) y(:,10:12) zeros(size(y,1),1) y(:,1:3) ones(size(y,1),1)]; 46 | 47 | 48 | %% Plotting 49 | figure(2); 50 | plot_tdcr_vc(g,sc,L) 51 | view(90,0); 52 | 53 | %% Solving the Cosserat Rod model with discrete tendon force 54 | 55 | % Shooting method for model solution 56 | 57 | %initial guess for linear and angular rate of change 58 | n_init_guess=[0;0;0]; 59 | u_init_guess=[0;0;0]; 60 | 61 | init_guess=[n_init_guess;u_init_guess]; 62 | 63 | % The model is solved first of all with half of the tendon and external 64 | % forces for it to converge more easily, and then the full forces are 65 | % applied 66 | 67 | % Step 0.5 68 | Fk = F*0.5; 69 | Ftexk = Ftex*0.5; 70 | %solving initial value problem 71 | tic 72 | display('Cosserat model with partially constrained tendon: Step 0.5'); 73 | [y,nu_f] = RT_IVP_2_discrete_5(init_guess,Fk,Ftexk(1:3),L,param,n,l); 74 | % Step 1 75 | display('Cosserat model with partially constrained tendon: Step 1'); 76 | [y,nu_f] = RT_IVP_2_discrete_5(nu_f,F,Ftex(1:3),L,param,n,l); 77 | sc= y(:,19); 78 | g = [y(:,4:6) zeros(size(y,1),1) y(:,7:9) zeros(size(y,1),1) y(:,10:12) zeros(size(y,1),1) y(:,1:3) ones(size(y,1),1)]; 79 | tip = reshape(g(end,:),4,4); 80 | toc 81 | 82 | %% Plotting 83 | 84 | figure(3); 85 | drawBackbone2(g,sc,L) 86 | view(90,0); 87 | 88 | 89 | %% Solving the PRBM model 90 | 91 | % Number of rigid bodies 92 | nrb = 4; 93 | % Length of rigid bodies, optimized with particle swarm algorithm in Chen 94 | % 2011 95 | gamma= [0.125 0.35 0.388 0.136]/sum([0.136 0.388 0.35 0.125]); 96 | 97 | %phi0 = pi/2; 98 | phi0 = 0; 99 | 100 | % Initialization 101 | var0 = repmat([phi0 0*ones(1,nrb)],1,n_disk); 102 | var_pcca_2 = reshape(var_pcca,[3,n_disk]); 103 | 104 | [var_prbm,exitflag,res] = prbm_solver(n,nrb,gamma,l,F,p_tendon,m_disk,m_bb,E,I,G,Ftex,Mtex,var0); 105 | 106 | %% Plotting 107 | figure(3); 108 | p_f = plot_tdcr_prbm(nrb,gamma,l,p_tendon,n_disk,var_prbm,r_disk); 109 | 110 | % Construct robot shape from the PRBM model solution 111 | [stest,pos_prbm] = construct_tdcr_prbm(nrb,gamma,l,p_tendon,n_disk,var_prbm); 112 | 113 | 114 | %% Solving the constant curvature model (geometric) 115 | 116 | % Compute tendon length from the robot shape obtained with Cosserat rod 117 | % model 118 | [delta_l] = calctendonlength2(g,sc,param,L); 119 | l_d = [-delta_l(1:3)/1000+L(1);-(delta_l(4:6)-delta_l(1:3))/1000+L(2)]; 120 | 121 | var_cc = CC_solver(l_d,n,r_disk); 122 | 123 | % Construct robot shape with CC output 124 | [T1_cc,T2_cc] = construct_tdcr_cc(var_c 125 | 126 | %% Plotting 127 | figure(6); 128 | hold on; 129 | plot3(T2c_cc(:,13),T2c_cc(:,14),T2c_cc(:,15),'r'); 130 | plot3(T1_cc(:,13),T1_cc(:,14),T1_cc(:,15),'r'); 131 | hold off; 132 | xlabel('x'); ylabel('y'); zlabel('z'); 133 | grid on; 134 | % axis equal; 135 | 136 | 137 | -------------------------------------------------------------------------------- /matlab/main.m: -------------------------------------------------------------------------------- 1 | 2 | % This code implements different approaches to model the kinematics/statics 3 | % of a two segment tendon driven continuum robot and is part of the following 4 | % publication: 5 | 6 | % How to model tendon-driven continuum robots and benchmark modelling performance 7 | % Priyanka Rao, Quentin Peyron, Sven Lilge, Jessica Burgner-Kahrs 8 | % frontiers in Robotics and AI 2021 9 | % DOI: 10.3389/frobt.2020.630245 10 | 11 | % Copyright (C) 2021 Continuum Robotics Laboratory, University of Toronto Mississauga 12 | 13 | clear all; 14 | close all; 15 | 16 | addpath('Parameters/'); 17 | addpath('Models/CCsub/'); 18 | addpath('Models/VC/'); 19 | addpath('Models/PRBM/'); 20 | addpath('Models/CC/'); 21 | 22 | %% Load TDCR parameters 23 | tdcr_2segments; 24 | 25 | % Tendon forces 26 | F = [2 0 0 0 1 0]; 27 | 28 | % External forces and moments at the tip 29 | Ftex = [0;0;0;0]; 30 | Mtex = [0;0;0;0]; 31 | 32 | %% Solving the CCsub model 33 | var0 = 0.01*ones(1,3*n_disk); 34 | var_ccsub = CCsub_solver(n,l,F,p_tendon,m_disk,m_bb,E,I,G,Ftex,Mtex,var0); 35 | 36 | %% Plotting 37 | figure(1); 38 | plot_tdcr_ccsub(l,p_tendon(1:3,:),n_disk,var_ccsub,r_disk); 39 | view(90,0); 40 | 41 | % Construct the robot shape with CCsub output, using 30 intermediate 42 | % points along each subsegment 43 | s_disk = [0 L(1)/n(1):L(1)/(n(1)):L(1) L(1)+L(2)/n(2):L(2)/n(2):sum(L)]; 44 | [stest,pos_ccsub] = construct_tdcr_ccsub(l,p_tendon(1:3,:),n_disk,var_ccsub); 45 | 46 | %% Solving the VC models 47 | 48 | % Set up TDCR parameters 49 | param=setup_param_vc(0,0,0,0,sum(L),ro,ri,m_bb(1),E,nu,0,r_disk,m_disk(1),n_disk,L,r1(1:3),r2(1:3),r3(1:3)); 50 | Fc = [F(1:3);F(4:6)]'; 51 | 52 | % Shooting method for model solution 53 | 54 | %initial guess for linear and angular rate of change 55 | v_init_guess=[0;0;1]; 56 | u_init_guess=[0;0;0]; 57 | init_guess=[v_init_guess;u_init_guess]; 58 | 59 | % Solving the VC model 60 | [y,vu_f] = VC_solver(init_guess,Fc,Ftex(1:3),L,param); 61 | sc_vc= y(:,19); 62 | g_vc = [y(:,4:6) zeros(size(y,1),1) y(:,7:9) zeros(size(y,1),1) y(:,10:12) zeros(size(y,1),1) y(:,1:3) ones(size(y,1),1)]; 63 | 64 | % Solving the VCref model 65 | init_guess = repmat(init_guess,sum(n),1); 66 | [y,vu_f] = VCref_solver(init_guess,F,Ftex(1:3),Mtex(1:3),L,param,n); 67 | sc_vcref= y(:,19); 68 | g_vcref = [y(:,4:6) zeros(size(y,1),1) y(:,7:9) zeros(size(y,1),1) y(:,10:12) zeros(size(y,1),1) y(:,1:3) ones(size(y,1),1)]; 69 | 70 | 71 | %% Plotting 72 | figure(2); 73 | plot_tdcr_vc(g_vc,sc_vc,L) 74 | view(90,0); 75 | 76 | figure(3); 77 | plot_tdcr_vc(g_vcref,sc_vcref,L) 78 | view(90,0); 79 | 80 | %% Solving the PRBM model 81 | 82 | % Number of rigid bodies 83 | nrb = 4; 84 | % Length of rigid bodies, optimized with particle swarm algorithm in Chen 85 | % 2011 86 | gamma= [0.125 0.35 0.388 0.136]/sum([0.136 0.388 0.35 0.125]); 87 | 88 | %phi0 = pi/2; 89 | phi0 = 0; 90 | 91 | % Initialization 92 | var0 = repmat([phi0 0*ones(1,nrb)],1,n_disk); 93 | 94 | [var_prbm,exitflag,res] = PRBM_solver(n,nrb,gamma,l,F,p_tendon,m_disk,m_bb,E,I,G,Ftex,Mtex,var0); 95 | 96 | %% Plotting 97 | figure(4); 98 | plot_tdcr_prbm(nrb,gamma,l,p_tendon,n_disk,var_prbm,r_disk); 99 | 100 | % Compute the position of each disk and each node of the PRB model 101 | [stest,pos_prbm] = construct_tdcr_prbm(nrb,gamma,l,n_disk,var_prbm); 102 | 103 | 104 | %% Solving the constant curvature model (geometric) 105 | 106 | % Compute tendon length from the robot shape obtained with Cosserat rod 107 | % model 108 | [delta_l] = calctendonlength(g_vc,sc_vc,param,L); 109 | l_d = [-delta_l(1:3)/1000+L(1);-(delta_l(4:6)-delta_l(1:3))/1000+L(2)]; 110 | 111 | var_cc = CC_solver(l_d,n,r_disk); 112 | 113 | % Construct the robot shape with CC output, using 50 points along each 114 | % segment 115 | [T1_cc,T2_cc] = construct_tdcr_cc(var_cc); 116 | 117 | %% Plotting 118 | figure(5); 119 | plot_tdcr_cc(T1_cc,T2_cc,L); 120 | 121 | 122 | 123 | --------------------------------------------------------------------------------