正向运动学c++ 代码

以下是一个简单的正向运动学的C++代码示例：

#include <iostream>
#include <cmath>

using namespace std;

// 定义角度转弧度的常量
const double PI = 3.14159265358979323846;
const double DEG_TO_RAD = PI / 180.0;

// 机械臂的构型参数
const double L1 = 1.0;  // 第1个关节的长度
const double L2 = 1.0;  // 第2个关节的长度

// 正向运动学函数
void forward_kinematics(double theta1, double theta2, double &x, double &y)
{
    // 将角度转换为弧度
    theta1 *= DEG_TO_RAD;
    theta2 *= DEG_TO_RAD;

    // 计算机械臂末端的坐标
    x = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
    y = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
}

int main()
{
    // 输入机械臂各关节的角度
    double theta1, theta2;
    cout << "请输入第1个关节的角度（度）：";
    cin >> theta1;
    cout << "请输入第2个关节的角度（度）：";
    cin >> theta2;

    // 计算机械臂末端的坐标
    double x, y;
    forward_kinematics(theta1, theta2, x, y);

    // 输出结果
    cout << "机械臂末端的坐标为：(" << x << ", " << y << ")" << endl;

    return 0;
}

在上面的代码中，我们首先定义了一些常量，包括PI和DEG_TO_RAD。然后定义了机械臂的构型参数L1和L2。接着定义了正向运动学函数forward_kinematics，该函数根据输入的关节角度计算机械臂末端的坐标。最后，在main函数中输入机械臂各关节的角度，调用forward_kinematics函数计算末端坐标，并输出结果。