这段代码计算将相机坐标下的螺栓坐标逆运算到机械臂工作台坐标系的逻辑

这段代码的主要目的是将相机坐标系下的螺栓坐标转换为机械臂基座坐标系下的坐标，并控制机械臂移动到目标位置。以下是关键步骤和逻辑：

1. **初始化ROS节点**：

   ros::init(argc, argv, "impedance_control");
   ros::NodeHandle nh;

2. **定义UR10e的控制和接收接口**：

   ur_rtde::RTDEReceiveInterface rtde_receive_ur10e("192.168.31.210", 125);
   ur_rtde::RTDEControlInterface rtde_control_ur10e("192.168.31.210");

3. **调用服务获取螺栓位姿**：

   ros::ServiceClient SerClientGetBoltPose = nh.serviceClient<bolt_pose::GetBoltPose>("GetBoltPose");
   ros::service::waitForService("GetBoltPose");
   bolt_pose::GetBoltPose SrvBoltPose;
   SerClientGetBoltPose.call(SrvBoltPose);

4. **提取螺栓姿态和位置（相机坐标系）**：

   Eigen::Matrix<double,3,1> VecCam_Bolt(SrvBoltPose.response.BoltPose[3].data, SrvBoltPose.response.BoltPose[4].data, SrvBoltPose.response.BoltPose[5].data);
   Eigen::Matrix<double,3,1> VecZAxiz(0, 0, 1);
   Eigen::Matrix<double,3,3> RCam_Bolt = Eigen::Quaterniond::FromTwoVectors(VecZAxiz, VecCam_Bolt).toRotationMatrix();
   Eigen::Matrix<double,3,3> Rz180 << -1, 0, 0, 0, -1, 0, 0, 0, 1;
   RCam_Bolt = RCam_Bolt * Rz180;

   Eigen::Matrix<double,4,1> PositionCam_Bolt(SrvBoltPose.response.BoltPose[0].data, SrvBoltPose.response.BoltPose[1].data, SrvBoltPose.response.BoltPose[2].data, 1);

5. **定义相机外参矩阵（法兰坐标系）**：

   Eigen::Matrix<double,4,4> TFlane_Cam = Eigen::Matrix<double, 4, 4>::Zero();
   Eigen::Quaterniond QuaFlane_Cam(-0.0001945800004997213, 0.004451595012178061, -0.002148722383039332, 0.9999877641414376);
   TFlane_Cam.block(0,0,3,3) = QuaFlane_Cam.toRotationMatrix();
   TFlane_Cam(0,3) = double(-0.04011327097795492);
   TFlane_Cam(1,3) = double(0.08832091157775031);
   TFlane_Cam(2,3) = double(-0.029894986535154933);
   TFlane_Cam(3,3) = double(1.0);

6. **获取当前法兰盘位姿（基座坐标系）**：

   vector<double> URPoseFlane_Cur = rtde_receive_ur10e.getActualTCPPose();
   Eigen::Matrix<double,4,4> TBase_Flane_Cur = Eigen::Matrix<double, 4, 4>::Zero();
   Eigen::Matrix<double,3,3> RBase_Flane_Cur = RotationVector2RotationMatrix(URPoseFlane_Cur[3], URPoseFlane_Cur[4], URPoseFlane_Cur[5]);
   TBase_Flane_Cur.block(0,0,3,3) = RBase_Flane_Cur;
   TBase_Flane_Cur(0,3) = double(URPoseFlane_Cur[0]);
   TBase_Flane_Cur(1,3) = double(URPoseFlane_Cur[1]);
   TBase_Flane_Cur(2,3) = double(URPoseFlane_Cur[2]);
   TBase_Flane_Cur(3,3) = double(1.0);

7. **计算相机当前位姿（基座坐标系）**：

   Eigen::Matrix<double,4,4> TBase_Cam_Cur = TBase_Flane_Cur * TFlane_Cam;

8. **计算螺栓姿态（基座坐标系）**：

   Eigen::Matrix<double,3,3> RBase_Cam_Cur = TBase_Cam_Cur.block(0,0,3,3);
   Eigen::Matrix<double,3,3> RBase_Bolt = RBase_Cam_Cur * RCam_Bolt;
   vector<double> URRVBase_Bolt = RotationMatrix2RotationVector(RBase_Bolt);

9. **计算螺栓位置（基座坐标系）**：

   Eigen::Matrix<double,4,1> PositionBase_Bolt = TBase_Cam_Cur * PositionCam_Bolt;

10. **构建螺栓位姿（基座坐标系）**：

    Eigen::Matrix<double,4,4> TBase_Bolt = Eigen::Matrix<double, 4, 4>::Identity();
    TBase_Bolt.block(0,0,3,3) = RBase_Bolt;
    TBase_Bolt(0,3) = PositionBase_Bolt(0);
    TBase_Bolt(1,3) = PositionBase_Bolt(1);
    TBase_Bolt(2,3) = PositionBase_Bolt(2);

11. **定义法兰位姿（套筒坐标系）**：

    Eigen::Matrix<double,4,4> TSleeve_Flane = Eigen::Matrix<double, 4, 4>::Identity();
    TSleeve_Flane(0,3) = -0.002;
    TSleeve_Flane(1,3) = 0.008;
    TSleeve_Flane(2,3) = -0.119;

12. **计算目标法兰位姿（基座坐标系）**：

    Eigen::Matrix<double,4,4> TBase_Flane_Tar = TBase_Bolt * TSleeve_Flane;

13. **构建目标位姿并控制机械臂移动**：

    vector<double> VecURMove(6);
    VecURMove[0] = TBase_Flane_Tar(0,3);
    VecURMove[1] = TBase_Flane_Tar(1,3);
    VecURMove[2] = TBase_Flane_Tar(2,3);
    VecURMove[3] = URRVBase_Bolt[0];
    VecURMove[4] = URRVBase_Bolt[1];
    VecURMove[5] = URRVBase_Bolt[2];

    rtde_control_ur10e.moveL(VecURMove, 0.05, 0.01);

通过这些步骤，代码成功地将相机坐标系下的螺栓坐标转换为机械臂基座坐标系下的坐标，并控制机械臂移动到目标位置。