auto q = robot->getJointPositions(); std::cout << "第" << i + 1 << "次标定关节角数据: " << q << std::endl; if (ft_sensor_KunWei != nullptr) { sensor_data = ft_sensor_KunWei->getFTSensorData(); for (int i = 0; i < WORKSPACE_DIM; i++) { sensor_data[i] *= GRAVITY; // 坤维传感器测量值单位为Kg } } else if (ft_sensor_ATI != nullptr) sensor_data = ft_sensor_ATI->getFTSensorData(); memcpy(calib_measure[i].data(), sensor_data.data(), WORKSPACE_DIM * sizeof(double)); aral->kdCalForwardPosition(toArray(q), false, false, calib_pose[i]); std::cout << "第" << i + 1 << "次标定传感器数据: " << calib_measure[i] << std::endl; return 1;

获取机器人的关节角度数据，并将其存储在变量 q 中。然后将关节角度数据打印输出。

如果 ft_sensor_KunWei 不为空指针，执行以下操作：
- 获取 ft_sensor_KunWei 的力传感器数据，并将其存储在变量 sensor_data 中。
- 对于每个维度，将传感器数据乘以 GRAVITY，以将其单位转换为千克。

如果 ft_sensor_ATI 不为空指针，执行以下操作：
- 获取 ft_sensor_ATI 的力传感器数据，并将其存储在变量 sensor_data 中。

将 sensor_data 的数据复制到 calib_measure[i]。

调用 aral->kdCalForwardPosition 方法，传入关节角度数据的数组形式 toArray(q)，并将结果存储在 calib_pose[i] 中。

最后，打印输出第 i + 1 次的标定传感器数据 calib_measure[i]，并返回值为 1。