findway/main.cpp at main · dwx5/findway · 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
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <vector>
#include <cmath>
#include <iomanip>

using namespace Eigen;

struct Pose {
    Vector3d pos;
    Quaterniond rotation;

    Pose(const Vector3d& p, const Quaterniond& r) : pos(p), rotation(r) {}
};

class SixAxisForwardKinematics {
private:
    std::vector<double> _link_lengths = {0.05985, 0.41000, 0.08307, 0.33969, 0.05710};

public:
    SixAxisForwardKinematics(const std::vector<double>& link_lengths = {}) {
        if (!link_lengths.empty()) {
            _link_lengths = link_lengths;
        }
    }

    std::vector<Pose> getPose(const std::vector<double>& angles) {
        std::vector<Pose> ans;

        // 初始位姿
        Vector3d currentPos(0, 0, 0);
        Quaterniond currentRot = Quaterniond::Identity();

        ans.emplace_back(currentPos, currentRot);  // 基座位姿

        // 关节1: alpha=pi/2, a=0, d=0.05985, offset=0
        currentRot = currentRot * Quaterniond(AngleAxisd(angles[0], Vector3d::UnitZ()));
        currentPos = currentPos + currentRot * Vector3d::UnitZ() * _link_lengths[0];
        currentRot = currentRot * Quaterniond(AngleAxisd(M_PI/2, Vector3d::UnitX()));
        ans.emplace_back(currentPos, currentRot);

        // 关节2: alpha=0, a=0.41000, d=0, offset=pi/2
        currentRot = currentRot * Quaterniond(AngleAxisd(angles[1] + M_PI/2, Vector3d::UnitZ()));
        currentPos = currentPos + currentRot * Vector3d::UnitX() * _link_lengths[1];
        ans.emplace_back(currentPos, currentRot);

        // 关节3: alpha=pi/2, a=-0.08307, d=0, offset=0
        currentRot = currentRot * Quaterniond(AngleAxisd(angles[2], Vector3d::UnitZ()));
        currentPos = currentPos + currentRot * Vector3d::UnitX() * -_link_lengths[2];
        currentRot = currentRot * Quaterniond(AngleAxisd(M_PI/2, Vector3d::UnitX()));
        ans.emplace_back(currentPos, currentRot);

        // 关节4: alpha=pi/2, a=0, d=0.33969, offset=0
        currentRot = currentRot * Quaterniond(AngleAxisd(angles[3], Vector3d::UnitZ()));
        currentPos = currentPos + currentRot * Vector3d::UnitZ() * _link_lengths[3];
        currentRot = currentRot * Quaterniond(AngleAxisd(M_PI/2, Vector3d::UnitX()));
        ans.emplace_back(currentPos, currentRot);

        // 关节5: alpha=pi/2, a=0, d=0, offset=pi/2
        currentRot = currentRot * Quaterniond(AngleAxisd(angles[4] + M_PI/2, Vector3d::UnitZ()));
        currentRot = currentRot * Quaterniond(AngleAxisd(M_PI/2, Vector3d::UnitX()));
        ans.emplace_back(currentPos, currentRot);

        // 关节6: alpha=0, a=0, d=-0.05710, offset=0
        currentRot = currentRot * Quaterniond(AngleAxisd(angles[5], Vector3d::UnitZ()));
        currentPos = currentPos + currentRot * Vector3d::UnitZ() * -_link_lengths[4];
        ans.emplace_back(currentPos, currentRot);

        return ans;
    }
};

int main() {
    SixAxisForwardKinematics solver;
    std::vector<double> angles = {0.5, -0.3, 0.7, 0.4, -0.4, 0.6};

    auto poses = solver.getPose(angles);

    // 输出末端位姿（最后一个pose）
    const auto& endPose = poses.back();
    Vector3d position = endPose.pos;
    Matrix3d rotation = endPose.rotation.toRotationMatrix();

    // 使用四元数直接获取欧拉角
    Vector3d euler_zyx_quat = endPose.rotation.toRotationMatrix().eulerAngles(2, 1, 0);

    std::cout << std::fixed << std::setprecision(4);
    std::cout << "末端位置: (" << position.x() << ", "
              << position.y() << ", " << position.z() << ")" << std::endl;

    std::cout << "末端欧拉角 (ZYX) [" << euler_zyx_quat[0] << ", "
              << euler_zyx_quat[1] << ", " << euler_zyx_quat[2] << "] rad" << std::endl;

    return 0;
}