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

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

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

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

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

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;

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

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;

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

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

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

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;

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

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

计算目标法兰位姿：

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;

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

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