snake_visualization/continuum_robot.cpp at master · nimasarli/snake_visualization · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
#include "continuum_robot.h"

Eigen::Vector3d continuum_robot::directKinSingleSegPosition(continuum_robot::ConfigSingleSeg config,
                                                            double length)
{
    double theta = config.theta;
    double delta = config.delta;
    double delTheta = theta - ::PI/2.0;
    Eigen::Vector3d position;

    if (std::abs(delTheta) <= 0.2*(::PI/180.0)) //less than 0.2 degree
    {
        position(0) = 0.0;
        position(1) = 0.0;
        position(2) = length;
    }
    else
    {
        position(0) = -(length/delTheta)*std::cos(delta)*(1-std::sin(theta));
        position(1) = +(length/delTheta)*std::sin(delta)*(1-std::sin(theta));
        position(2) = -(length/delTheta)*std::cos(theta);
    }

    return position;
}

Eigen::Matrix3d continuum_robot::directKinSingleSegRot(continuum_robot::ConfigSingleSeg config)
{
    //    rotMat = rotZ(-delta_t)*rotY(THETA_0-theta_tL)*rotZ(delta_t);
    double theta = config.theta;
    double delta = config.delta;

    //Eigen::Matrix3d rotMat = robotic::rotZ(-delta) * robotic::rotY(theta - ::PI/2.0) * robotic::rotZ(delta);
    Eigen::Matrix3d rotMat = robotic::rotZ(-delta) * robotic::rotY(::PI/2.0 - theta) * robotic::rotZ(delta);

    return rotMat;
}

Eigen::Matrix4d continuum_robot::directKinSingleSeg(continuum_robot::ConfigSingleSeg config, double length)
{
    Eigen::Vector3d position = directKinSingleSegPosition(config,length);
    Eigen::Matrix3d rotMat = directKinSingleSegRot(config);
    Eigen::Matrix4d xForm;
    xForm << rotMat, position,
            0, 0, 0, 1;

    return xForm;
}

Eigen::Vector3d continuum_robot::getBackboneLengthVecSingleSeg(continuum_robot::ConfigSingleSeg config,
                                                               continuum_robot::MbcrParameter mbcrParameter)
{
    Eigen::Vector3d backboneLengthVec;
    backboneLengthVec(0) = mbcrParameter.length + mbcrParameter.kin_Radius*std::cos(config.delta)*(config.theta-::PI/2.0);
    backboneLengthVec(1) = mbcrParameter.length + mbcrParameter.kin_Radius*std::cos(config.delta+(2.0/3.0)*::PI)*(config.theta-::PI/2.0);
    backboneLengthVec(2) = mbcrParameter.length + mbcrParameter.kin_Radius*std::cos(config.delta+(4.0/3.0)*::PI)*(config.theta-::PI/2.0);

    return backboneLengthVec;
}

double continuum_robot::getBackboneLengthSingleSeg(continuum_robot::ConfigSingleSeg config,
                                                   continuum_robot::MbcrParameter mbcrParameter,
                                                   continuum_robot::BackboneNum backboneNum)
{
    double backboneLength;
    switch (backboneNum)
    {
    case BACKBONE1:
        backboneLength = mbcrParameter.length + mbcrParameter.kin_Radius*std::cos(config.delta)*(config.theta-::PI/2.0);
        break;
    case BACKBONE2:
        backboneLength = mbcrParameter.length + mbcrParameter.kin_Radius*std::cos(config.delta+(2.0/3.0)*::PI)*(config.theta-::PI/2.0);
        break;
    case BACKBONE3:
        backboneLength = mbcrParameter.length + mbcrParameter.kin_Radius*std::cos(config.delta+(4.0/3.0)*::PI)*(config.theta-::PI/2.0);
        break;
    }

    return backboneLength;
}

// Find center of backbone curves. See doc/backbones_curve_center.pdf
Eigen::Vector3d continuum_robot::getBackboneCurveCenterSingleSeg(continuum_robot::ConfigSingleSeg config, continuum_robot::MbcrParameter mbcrParameter, continuum_robot::BackboneNum backboneNum)
{
    double bbLength = getBackboneLengthSingleSeg(config,mbcrParameter,backboneNum);
    double arcRadius = bbLength / abs(::PI/2-config.theta);
    ConfigSingleSeg configFlipped = flipConfig(config);
    Eigen::Vector3d curveCenter;
    curveCenter << mbcrParameter.kin_Radius*std::cos((backboneNum-1)*(2.0*::PI/3.0)) + arcRadius*std::cos(configFlipped.delta),
            mbcrParameter.kin_Radius*std::sin((backboneNum-1)*(2.0*::PI/3.0)) - arcRadius*std::sin(configFlipped.delta),
            0;

    return curveCenter;
}

continuum_robot::ConfigSingleSeg continuum_robot::flipConfig(continuum_robot::ConfigSingleSeg config)
{
    continuum_robot::ConfigSingleSeg configFlipped = config;
    if (config.theta > ::PI/2.0)
    {
        configFlipped.delta = ::PI + config.delta;
        configFlipped.theta = ::PI - config.theta;
    }
    return configFlipped;
}