机械臂关节阻抗模式和笛卡尔阻抗模式c++实现

关节阻抗

#include <iostream>
#include <Eigen/Dense>

// 机械臂关节结构体
struct Joint {
    double position;  // 关节位置
    double velocity;  // 关节速度
    double desiredPosition;  // 期望关节位置
    double stiffness;  // 刚度系数
    double damping;  // 阻尼系数
};

// 计算关节力
double calculateJointForce(const Joint& joint) {
    // 计算位置误差
    double positionError = joint.desiredPosition - joint.position;
    // 计算速度误差
    double velocityError = 0 - joint.velocity;

    // 计算阻抗力
    double force = joint.stiffness * positionError + joint.damping * velocityError;

    return force;
}

// 阻抗控制循环
void impedanceControlLoop(std::vector<Joint>& joints) {
    // 假设的控制周期
    double controlPeriod = 0.01; 

    for (Joint& joint : joints) {
        // 计算关节力
        double force = calculateJointForce(joint);

        // 根据力计算加速度
        double acceleration = force / joint.stiffness; 

        // 更新速度和位置
        joint.velocity += acceleration * controlPeriod;
        joint.position += joint.velocity * controlPeriod;
    }
}

int main() {
    std::vector<Joint> joints(6);

    // 初始化关节状态和参数
    for (int i = 0; i < 6; ++i) {
        joints[i].position = 0.0;
        joints[i].velocity = 0.0;
        joints[i].desiredPosition = 1.0;
        joints[i].stiffness = 100.0;
        joints[i].damping = 10.0;
    }

    // 控制循环次数
    int numIterations = 1000;
    for (int i = 0; i < numIterations; ++i) {
        impedanceControlLoop(joints);

        // 打印当前关节位置
        std::cout << "Iteration " << i << ": ";
        for (const Joint& joint : joints) {
            std::cout << joint.position << " ";
        }
        std::cout << std::endl;
    }

    return 0;
}

笛卡尔阻抗

#include <iostream>
#include <Eigen/Dense>

// 机械臂末端位姿结构体
struct EndEffectorPose {
    Eigen::Vector3d position;  // 位置
    Eigen::Quaterniond orientation;  // 姿态（四元数表示）
};

// 机械臂末端期望位姿结构体
struct DesiredEndEffectorPose {
    Eigen::Vector3d desiredPosition;  // 期望位置
    Eigen::Quaterniond desiredOrientation;  // 期望姿态（四元数表示）
    double stiffnessPosition;  // 位置刚度
    double dampingPosition;  // 位置阻尼
    double stiffnessOrientation;  // 姿态刚度
    double dampingOrientation;  // 姿态阻尼
};

// 计算笛卡尔空间的力和扭矩
Eigen::Vector6d calculateCartesianForceTorque(const EndEffectorPose& currentPose, const DesiredEndEffectorPose& desiredPose) {
    Eigen::Vector3d positionError = desiredPose.desiredPosition - currentPose.position;
    Eigen::Vector3d velocityErrorPosition = Eigen::Vector3d::Zero() - currentPose.position;

    Eigen::Quaterniond orientationError = desiredPose.desiredOrientation * currentPose.orientation.inverse();
    Eigen::Vector3d angularError = orientationError.toRotationMatrix().eulerAngles(0, 1, 2);  // 转换为欧拉角来表示姿态误差
    Eigen::Vector3d velocityErrorOrientation = Eigen::Vector3d::Zero() - angularError;

    Eigen::Vector3d force = desiredPose.stiffnessPosition * positionError + desiredPose.dampingPosition * velocityErrorPosition;
    Eigen::Vector3d torque = desiredPose.stiffnessOrientation * angularError + desiredPose.dampingOrientation * velocityErrorOrientation;

    Eigen::Vector6d forceTorque;
    forceTorque << force, torque;
    return forceTorque;
}

// 笛卡尔阻抗控制循环
void cartesianImpedanceControlLoop(EndEffectorPose& currentPose, DesiredEndEffectorPose& desiredPose) {
    // 假设的控制周期
    double controlPeriod = 0.01; 

    Eigen::Vector6d forceTorque = calculateCartesianForceTorque(currentPose, desiredPose);

    // 假设简单的动力学模型，根据力和扭矩计算加速度
    Eigen::Vector6d acceleration = forceTorque; 

    // 更新速度和位置
    currentPose.position += currentPose.velocity * controlPeriod + 0.5 * acceleration.head<3>() * controlPeriod * controlPeriod;
    currentPose.orientation = currentPose.orientation * Eigen::AngleAxisd(0.5 * acceleration.tail<3>().norm() * controlPeriod, acceleration.tail<3>().normalized());
    currentPose.velocity += acceleration * controlPeriod;
}

int main() {
    EndEffectorPose currentPose;
    currentPose.position = Eigen::Vector3d::Zero();
    currentPose.orientation = Eigen::Quaterniond::Identity();
    currentPose.velocity = Eigen::Vector3d::Zero();

    DesiredEndEffectorPose desiredPose;
    desiredPose.desiredPosition = Eigen::Vector3d(1.0, 1.0, 1.0);
    desiredPose.desiredOrientation = Eigen::Quaterniond::Identity();
    desiredPose.stiffnessPosition = 100.0;
    desiredPose.dampingPosition = 10.0;
    desiredPose.stiffnessOrientation = 50.0;
    desiredPose.dampingOrientation = 5.0;

    // 控制循环次数
    int numIterations = 1000;
    for (int i = 0; i < numIterations; ++i) {
        cartesianImpedanceControlLoop(currentPose, desiredPose);

        // 打印当前末端位姿
        std::cout << "Iteration " << i << ": Position: " << currentPose.position.transpose() << ", Orientation: ";
        std::cout << currentPose.orientation.coeffs().transpose() << std::endl;
    }

    return 0;
}