基本信息

---

下载

   友情提示：（无需注册或充值，赞助后即可获取资源下载链接）

---

     嘿，亲！知识可是无价之宝呢，但咱这精心整理的资料也耗费了不少心血呀。小小地破费一下，绝对物超所值哦！如有下载和支付问题，请联系我们QQ(微信同号)：813200300

本次赞助数额为： 2 元　

支付宝支付

微信支付

   源码介绍

---

有笛卡尔轨迹规划和关节轨迹规划

static void zyz2rot(PosClass startPos, double* mStartMatrixData)
{
double startAngle[3];
startAngle[0] = startPos.yaw * PI / 180;
startAngle[1] = startPos.pitch * PI / 180;
startAngle[2] = startPos.roll * PI / 180;

mStartMatrixData[0] = cos(startAngle[0]) * cos(startAngle[1]) * cos(startAngle[2]) - sin(startAngle[0]) * sin(startAngle[2]);
mStartMatrixData[1] = -cos(startAngle[0]) * cos(startAngle[1]) * sin(startAngle[2]) - sin(startAngle[0]) * cos(startAngle[2]);
mStartMatrixData[2] = cos(startAngle[0]) * sin(startAngle[1]);
mStartMatrixData[3] = startPos.x / 1000;

mStartMatrixData[4] = sin(startAngle[0]) * cos(startAngle[1]) * cos(startAngle[2]) cos(startAngle[0]) * sin(startAngle[2]);
mStartMatrixData[5] = -sin(startAngle[0]) * cos(startAngle[1]) * sin(startAngle[2]) cos(startAngle[0]) * cos(startAngle[2]);
mStartMatrixData[6] = sin(startAngle[0]) * sin(startAngle[1]);
mStartMatrixData[7] = startPos.y / 1000;

mStartMatrixData[8] = -sin(startAngle[1]) * cos(startAngle[2]);
mStartMatrixData[9] = sin(startAngle[1]) * sin(startAngle[2]);
mStartMatrixData[10] = cos(startAngle[1]);
mStartMatrixData[11] = startPos.z / 1000;

mStartMatrixData[12] = 0;
mStartMatrixData[13] = 0;
mStartMatrixData[14] = 0;
mStartMatrixData[15] = 1;
}