Concentric Tube Robot Kinematics – C++ Static Library

Introduction

Welcome to the Concentric Tube Robot (CTR) Kinematics library. This repository hosts a C++ static library designed for the forward and inverse kinematics of a three-tube concentric tube robot (CTR), leveraging the Cosserat Theory-based kinematic model as detailed in:

[1] D. C. Rucker, B. A. Jones, and R. J. Webster III, “A Geometrically Exact Model for Externally Loaded Concentric-Tube Continuum Robots,” IEEE Trans. Robot., vol. 26, no. 5, pp. 769–780, Oct. 2010, doi: 10.1109/TRO.2010.2062570.

This project is developed by the Canadian Surgical Technologies & Advanced Robotics (CSTAR) lab at the Western University, London, ON, Canada.

Features

• Forward Kinematics: Implementation for a three-tube CTR.
• Inverse Kinematics: Implementation for a three-tube CTR.
• User-Friendly API: Easy integration into your C++ projects.
• Comprehensive Documentation: Detailed explanation of the kinematic model and library usage.

Installation

You can either clone the repository and build it from source as a CMake project (recommended) or download the precompiled binaries for your platform from the Releases section.

Building Requirements

Ensure you have the following libraries installed:

• Boost
• Blaze Library
• LAPACK
• OpenBLAS
• TBB (Threading Building Blocks)

Building from Source

1. Clone the repository:

   git clone https://github.com/fcpedrosa/Concentric-Tube-Robot

2. Build the library:

   cd Concentric-Tube-Robot
   mkdir build && cd build
   cmake ..
   make -j4

3. The library will be built as a static library (CTR.a).

Using Precompiled Binaries

Download the precompiled binaries for your platform from the Releases section (TBA). Add the library to your C++ project's dependencies and include the appropriate header files.

Usage

To use the library in your C++ project, include the necessary header files and link against the library. Here's a simple example:

#include <iostream>
// include the CTR library header
#include "CTR.hpp"
 
int main()
{
    // Properties of Nitinol Tubes
    constexpr double E = 58.00E9; // Young's modulus in GPa
    constexpr double nu = 0.32;   // Poisson's ratio
    constexpr double G = E / (2.00 * (1.00 + nu)); // Shear modulus in GPa
 
    // Precurvature radii for the tubes
    constexpr double R1 = 40.00E-3;  // 4 cm curvature radius
    constexpr double R2 = 100.00E-3; // 10 cm curvature radius
    constexpr double R3 = 140.00E-3; // 14 cm curvature radius
 
    // Precurvature vectors for the curved portions of the tubes
    constexpr blaze::StaticVector<double, 3UL> u1 = {1.00 / R1, 0.00, 0.00};
    constexpr blaze::StaticVector<double, 3UL> u2 = {1.00 / R2, 0.00, 0.00};
    constexpr blaze::StaticVector<double, 3UL> u3 = {1.00 / R3, 0.00, 0.00};
 
    // Kinematic parameters of the robot
    constexpr blaze::StaticVector<double, 3UL> ls = {190.00E-3, 120.00E-3, 90.00E-3}; // Lengths of the tubes' straight sections
    constexpr blaze::StaticVector<double, 3UL> lc = {60.00E-3, 80.00E-3, 40.00E-3};   // Lengths of the tubes' curved sections
    constexpr blaze::StaticVector<double, 3UL> OD = {0.92e-3, 1.10E-3, 1.40E-3};      // Outer diameters of the tubes
    constexpr blaze::StaticVector<double, 3UL> ID = {0.80E-3, 0.97e-3, 1.20E-3};      // Inner diameters of the tubes
 
    // Instantiating the Tube objects
    std::shared_ptr<Tube> T1 = std::make_shared<Tube>(OD[0UL], ID[0UL], E, G, ls[0UL], lc[0UL], u1); // Innermost tube
    std::shared_ptr<Tube> T2 = std::make_shared<Tube>(OD[1UL], ID[1UL], E, G, ls[1UL], lc[1UL], u2); // Intermediate tube
    std::shared_ptr<Tube> T3 = std::make_shared<Tube>(OD[2UL], ID[2UL], E, G, ls[2UL], lc[2UL], u3); // Outermost tube
 
    // Array of smart pointers to CTR component tubes
    std::array<std::shared_ptr<Tube>, 3UL> Tb = {T1, T2, T3};
 
    // Initial joint actuation values "home position" - q = [Beta Alpha]
    blaze::StaticVector<double, 3UL> Beta_0 = {-120.00E-3, -100.00E-3, -80.00E-3};
    blaze::StaticVector<double, 3UL> Alpha_0 = {mathOp::deg2Rad(0.00), mathOp::deg2Rad(0.00), mathOp::deg2Rad(0.00)};
    blaze::StaticVector<double, 6UL> q_0;
    blaze::subvector<0UL, 3UL>(q_0) = Beta_0;
    blaze::subvector<3UL, 3UL>(q_0) = Alpha_0;
 
    // Accuracy of BVP solutions
    double Tol = 1.00E-6;
    // Tolerance for position control (0.5 mm)
    double pos_tol = 5.00E-4;
 
    // Instantiating the CTR object
    CTR CTR_robot(Tb, q_0, Tol, mathOp::rootFindingMethod::MODIFIED_NEWTON_RAPHSON);
 
    // Initial guess for the BVP
    blaze::StaticVector<double, 5UL> initGuess;
    // External wrenches acting as point loads on the distal ends of the robot
    blaze::StaticVector<double, 3UL> moment = {0.00, -0.05, 0.00}, force = {0.00, -0.15, -0.55};
 
    // Setting the external point moment and force
    CTR_robot.setDistalMoment(moment);
    CTR_robot.setDistalForce(force);
 
    // Actuate the robot to the configuration q_0 and solve the corresponding FK problem
    CTR_robot.actuate_CTR(initGuess, q_0);
 
    // Inspect the distal end's position after actuation
    std::cout << "Tip position: " << blaze::trans(CTR_robot.getTipPos()) << std::endl;
 
    // Differential kinematics-based position control for the CTR
    blaze::StaticVector<double, 3UL> tip_pos, target = {-0.053210, 0.043606, 0.179527};
 
    // Inverse kinematics
    CTR_robot.posCTRL(initGuess, target, pos_tol);
    tip_pos = CTR_robot.getTipPos();
 
    std::cout << "Target is: " << blaze::trans(target)
              << "Tip position is: " << blaze::trans(tip_pos)
              << "Joint values (IK solution): " << blaze::trans(CTR_robot.getConfiguration())
              << "Position error: " << blaze::norm(tip_pos - target) * 1.00E3 << " [mm]"
              << std::endl;
 
    return 0;
}

For detailed documentation on the library's API and usage, refer to the Documentation section.

Examples

The examples directory contains usage examples to help you get started.

Contributing

Contributions to this project are welcome! If you find any issues or have ideas for improvements, please open an issue or submit a pull request.

License

This project is licensed under the MIT License - see the LICENSE file for details.

Contact

For any questions or inquiries, feel free to contact:

Filipe Pedrosa Email: fpedr.nosp@m.osa@.nosp@m.uwo.c.nosp@m.a

Please note that this project is a research-oriented implementation and comes with no warranty or support. Use it at your own risk.