aubo.control(MiniControl::Read, [&](RobotPtr robot, std::vector<double> &) { if (!robot) { return -1; } std::vector<std::vector<double>> temp_point(point_sz); for (unsigned int i = 0; i < point_sz; i++) temp_point[i] = point[i]; if (not app_type) { memcpy(q_init.data(), point[0].data(), sizeof(double) * ROBOT_DOF); aral->kdCalForwardPosition(toArray(q_init), false, true, pose_init); if (point_sz == 1) { for (int i = 0; i < WORKSPACE_DIM; i++) { pose_start[i] = pose_init[i] + rot_error[i]; pose_end[i] = pose_start[i] + trans_error[i]; } // 求逆解，获得起始关节角 IKConfigInfo config; config.goal = pose_start; config.q_ref = toArray(q_init); aral->kdCalInversePosition(config, false, q_start); } else memcpy(q_start.data(), q_init.data(), sizeof(double) * ROBOT_DOF); int loop = 0; while (loop < 2) { //! 延时3s int delay = 0; while (delay < 600) { // 600为在路径开头和结尾需要停留时间的路点个数 if (loop % 2 == 0) offline_traj.push_back(q_start); else offline_traj.push_back(offline_traj[offline_traj.size() - 1]); delay++; }

这段代码是一个关于机器人运动控制的部分。根据代码的逻辑，它首先检查机器人对象是否存在。然后，它创建一个临时的二维数组temp_point来存储一些坐标点。接下来，它根据app_type的值来确定是否需要进行运动规划。如果point_sz等于1，表示只有一个目标点，它将使用kdCalForwardPosition函数来计算起始位姿pose_init。然后，它根据pose_init、rot_error和trans_error计算起始位姿pose_start和目标位姿pose_end。接着，它使用kdCalInversePosition函数来计算起始关节角q_start。如果point_sz大于1，表示有多个目标点，它将q_start初始化为q_init。然后，它进入一个循环，在每次循环中将q_start或者上一个轨迹点添加到offline_traj中，并且延时600个时间单位。这段代码可能用于生成机器人的离线运动轨迹。