Processing机械臂末端臂段局部Y轴旋转异常问题求助

我正在Processing中模拟机械臂，大部分功能已正常运行，但最后一段臂段（不含夹爪）需要绕自身相对Y轴旋转。当前实现为平移后进行Y轴旋转，仅当臂段处于{x:0,y,z:0}位置时该方案有效；当臂段偏离该位置时，Y轴旋转似乎是相对于臂段底部执行的，不确定问题成因。

臂段位置计算代码 #

public float[] getArmPiecePos(ArmPiece parent, ArmPiece child){
float x_child = sin(child.total_rotation[2]) * (child.size[1] / 2) + sin(parent.total_rotation[2]) * (parent.size[1] / 2);
float y_child = - cos(child.total_rotation[2]) * (child.size[1] / 2) - cos(parent.total_rotation[2]) * (parent.size[1] / 2);
float z_child = sin(child.total_rotation[1]) * (x_child);
x_child *= cos(child.total_rotation[1]);
float x = x_child + parent.x;
float y = y_child + parent.y;
float z = -z_child + parent.z;
float[] final_pos = {x, y, z};

return final_pos;
}

臂段平移+旋转代码 #

pushMatrix();
float[] piece_pos = mf.getArmPiecePos(this.parent, this);
this.x = piece_pos[0];
this.y = piece_pos[1];
this.z = piece_pos[2];
translate(this.x, this.y, this.z);
rotateX(this.total_rotation[0]);
rotateY(this.total_rotation[1]);
rotateZ(this.total_rotation[2]);
fill(100);
box(this.size[0], this.size[1], this.size[2]);
popMatrix();

效果对比 #

• 机械臂默认位置：
• 预期效果：
• 实际效果：

项目仓库：https://github.com/clodon2/LeArmSimulator

内容的提问来源于stack exchange，提问作者clodon2