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

该代码通过以下步骤将相机坐标系下的螺栓坐标转换为机械臂基座坐标系下的坐标：

1. **获取螺栓在相机坐标系中的位姿**：

   bolt_pose::GetBoltPose SrvBoltPose;
   SerClientGetBoltPose.call(SrvBoltPose);
   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,4,1> PositionCam_Bolt(SrvBoltPose.response.BoltPose[0].data,
                                              SrvBoltPose.response.BoltPose[1].data,
                                              SrvBoltPose.response.BoltPose[2].data, 
                                              1);

2. **计算相机相对于法兰的变换矩阵**：

   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) = -0.04011327097795492;
   TFlane_Cam(1,3) = 0.08832091157775031;
   TFlane_Cam(2,3) = -0.029894986535154933;
   TFlane_Cam(3,3) = 1.0;

3. **获取当前法兰盘在基座坐标系中的位姿**：

   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) = URPoseFlane_Cur[0];
   TBase_Flane_Cur(1,3) = URPoseFlane_Cur[1];
   TBase_Flane_Cur(2,3) = URPoseFlane_Cur[2];
   TBase_Flane_Cur(3,3) = 1.0;

4. **计算相机在基座坐标系中的位姿**：

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

5. **计算螺栓在基座坐标系中的位姿**：

   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);
   Eigen::Matrix<double,4,1> PositionBase_Bolt = TBase_Cam_Cur * PositionCam_Bolt;

6. **构建螺栓在基座坐标系中的变换矩阵**：

   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);

7. **计算目标法兰位姿**：

   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;
   Eigen::Matrix<double,4,4> TBase_Flane_Tar = TBase_Bolt * TSleeve_Flane;

8. **生成机械臂的目标位姿并控制其移动**：

   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);

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