static size_t i = 0; if (app_type) { memcpy(q_ref.data(), point[0].data(), sizeof(double) * ROBOT_DOF); memcpy(qd_ref.data(), reference_vel.data(), sizeof(double) * ROBOT_DOF); memcpy(qdd_ref.data(), reference_vel.data(), sizeof(double) * ROBOT_DOF); } else { if (i < offline_traj_size) { memcpy(q_ref.data(), offline_traj[i].data(), sizeof(double) * ROBOT_DOF); memcpy(qd_ref.data(), offline_traj_vel[i].data(), sizeof(double) * ROBOT_DOF); memcpy(qdd_ref.data(), offline_traj_acc[i++].data(), sizeof(double) * ROBOT_DOF); } else { if(point_sz != 1) i = 0; else return 1; } } aral->kdCalForwardPosition(toArray(q_ref), false, true, X_ref);// 末端参考位姿在Base坐标系描述 aral->kdCalForwardVelocity(toArray(q_ref), qd_ref, Xd_ref);// 末端参考速度在Base坐标系描述 if (not app_type && point_sz != 1) { if (control_mode == MiniControl::Position) X_ref = aral->kdChangeReferenceFrame(X_ref, aral->kdInverseFrame(X_ref)); // 末端参考位姿在Tool坐标系描述 else Xd_ref = aral->kdChangeWrenchBase(Xd_ref, aral->kdInverseFrame(X_ref)); // 末端参考速度在Tool坐标系描述 }
时间: 2024-02-14 07:27:00 浏览: 115
DLL_Test.rar_cuda_cuda dll_hkbu.exe
这段代码是一个条件语句,根据变量`app_type`的值来执行不同的操作。首先,如果`app_type`为真,则将`point[0]`的数据拷贝到`q_ref`、`qd_ref`和`qdd_ref`中。然后,调用`aral->kdCalForwardPosition()`和`aral->kdCalForwardVelocity()`函数计算末端参考位姿和速度。
如果`app_type`为假,则进入下一个条件语句。首先判断变量`i`是否小于`offline_traj_size`,如果是,则将`offline_traj[i]`、`offline_traj_vel[i]`和`offline_traj_acc[i]`的数据拷贝到`q_ref`、`qd_ref`和`qdd_ref`中,并将`i`的值加1。如果`i`大于等于`offline_traj_size`,则进入下一个条件语句。
在下一个条件语句中,判断变量`point_sz`是否不等于1。如果是,则将`i`重置为0。否则,返回1。
最后,根据变量`app_type`和`control_mode`的值,对`X_ref`和`Xd_ref`进行一些处理。如果`control_mode`为MiniControl::Position,则调用`aral->kdChangeReferenceFrame()`函数将末端参考位姿转换到Tool坐标系描述。否则,调用`aral->kdChangeWrenchBase()`函数将末端参考速度转换到Tool坐标系描述。
阅读全文