CTR.hpp

// ***************************************************************************************** //
// *  This file is part of NIH_CSTAR, a CTR kinematics library for Concentric Tube Robots  * //
// *                                                                                       * //
// *  ----------- # Copyright (C) 2021 Filipe C. Pedrosa <fpedrosa@uwo.ca> # -----------   * //
// *                                                                                       * //
// *  Project developed under the supervision of Prof Dr Rajni Patel <rvpatel@uwo.ca>      * //
// *              CSTAR (Canadian Surgical Technologies & Advanced Robotics)               * //
// *                   Western University, London, ON, Canada                              * //
// ***************************************************************************************** //
 
#pragma once
 
#include "Tube.hpp"
#include "Segment.hpp"
#include "ODESystem.hpp"
#include "Observer.hpp"
#include "mathOperations.hpp"
#include <boost/numeric/odeint.hpp>
#include <memory>
#include <tuple>
 
class CTR
{
public:
    CTR() = delete;
 
    CTR(const std::array<std::shared_ptr<Tube>, 3UL> &Tb, blaze::StaticVector<double, 6UL> &q, const double Tol, const mathOp::rootFindingMethod method);
 
    CTR(const CTR &rhs);
 
    CTR(CTR &&rhs) noexcept;
 
    ~CTR() = default;
 
    CTR &operator=(const CTR &rhs);
 
    CTR &operator=(CTR &&rhs) noexcept;
 
    void reset(const blaze::StaticVector<double, 5UL> &initGuess);
 
    blaze::StaticVector<double, 5UL> ODESolver(const blaze::StaticVector<double, 5UL> &initGuess);
 
    blaze::StaticMatrix<double, 5UL, 5UL> jac_BVP(const blaze::StaticVector<double, 5UL> &initGuess, const blaze::StaticVector<double, 5UL> &residue);
 
    blaze::StaticMatrix<double, 3UL, 6UL> jacobian(const blaze::StaticVector<double, 5UL> &initGuess, const blaze::StaticVector<double, 3UL> &tipPos);
 
    bool PowellDogLeg(blaze::StaticVector<double, 5UL> &initGuess);
 
    bool Levenberg_Marquardt(blaze::StaticVector<double, 5UL> &initGuess);
 
    bool Broyden(blaze::StaticVector<double, 5UL> &initGuess);
 
    bool Broyden_II(blaze::StaticVector<double, 5UL> &initGuess);
 
    bool Newton_Raphson(blaze::StaticVector<double, 5UL> &initGuess);
 
    bool Modified_Newton_Raphson(blaze::StaticVector<double, 5UL> &initGuess);
 
    bool actuate_CTR(blaze::StaticVector<double, 5UL> &initGuess, const blaze::StaticVector<double, 6UL> &q_input);
 
    bool posCTRL(blaze::StaticVector<double, 5UL> &initGuess, const blaze::StaticVector<double, 3UL> &target, const double Tol);
 
    std::array<std::shared_ptr<Tube>, 3UL> getTubes();
 
    blaze::StaticVector<double, 3UL> getBeta();
 
    blaze::StaticVector<double, 6UL> getConfiguration();
 
    blaze::StaticVector<double, 3UL> getTipPos();
 
    blaze::StaticVector<double, 3UL> getDistalEnds();
 
    std::tuple<blaze::HybridMatrix<double, 3UL, 1000UL, blaze::columnMajor>, blaze::HybridMatrix<double, 3UL, 1000UL, blaze::columnMajor>, blaze::HybridMatrix<double, 3UL, 1000UL, blaze::columnMajor>> getTubeShapes();
 
    std::tuple<std::vector<double>, std::vector<double>, std::vector<double>> getShape();
 
    void setConfiguration(const blaze::StaticVector<double, 6UL> &q);
 
    void setBVPMethod(mathOp::rootFindingMethod mthd);
 
    void setDistalMoment(const blaze::StaticVector<double, 3UL> &moment);
 
    void setDistalForce(const blaze::StaticVector<double, 3UL> &force);
 
private:
    double m_accuracy;
 
    mathOp::rootFindingMethod m_method;
 
    std::array<std::shared_ptr<Tube>, 3UL> m_Tubes;
 
    blaze::StaticVector<double, 3UL> m_beta;
 
    blaze::StaticVector<double, 6UL> m_q;
 
    blaze::StaticVector<double, 3UL> m_theta_0;
 
    blaze::StaticVector<double, 4UL> m_h_0;
 
    blaze::StaticVector<double, 3UL> m_wf;
 
    blaze::StaticVector<double, 3UL> m_wm;
 
    blaze::StaticVector<double, 3UL> m_e3;
 
    std::unique_ptr<Segment> m_segment;
 
    std::vector<state_type> m_y;
 
    std::vector<double> m_s;
 
    std::unique_ptr<ODESystem> m_stateEquations;
 
    std::unique_ptr<Observer> m_stateObserver;
};