C#使用SharGL实现PUMA560机械臂

1、四轴机械臂

下载链接：https://download.csdn.net/download/panjinliang066333/89645225

关键代码

public void DrawRobot1(ref OpenGL gl,float[] angle,float[] yLength,bool isPuma560_Six){//坐标系说明：//①X轴正向：屏幕朝右//②Y轴正向：屏幕朝上//③Z轴正向：屏幕朝外float xSize = 2.0f, zSize = 2.0f;float xArm1 = 2.5f;//机器人底盘gl.PushMatrix();{gl.Color(1.0f, 1.0f, 0.0f); //黄色gl.Translate(xSize * 2.5 - 0.5f, -yLength[0], zSize * 2);gl.Scale(xSize * 5, 1.0f, zSize * 5);DrawCube(ref gl, 0.0f, 0.0f, 0.0f, false);}gl.PopMatrix();//画第一关节,偏航轴：绕Y轴旋转gl.PushMatrix();{gl.Color(1.0f, 0.0f, 0.0f);     //红颜色//1、绕自身中心轴旋转->绕Y轴旋转gl.Translate(-xSize / 2, -yLength[0] / 2, -zSize / 2);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(xSize / 2, yLength[0] / 2, zSize / 2);//绘制当前轴gl.Scale(xSize, yLength[0], zSize);DrawCube(ref gl, 0.0f, 0.0f, 0.0f, false);}gl.PopMatrix();//画一关节肩膀：绕Y轴旋转gl.PushMatrix();{gl.Color(1.0f, 1.0f, 1.0f);     //白颜色gl.Translate(0.0f, -0.5f, 0.0f);//绕关节1中心轴旋转gl.Translate(-xSize / 2, -yLength[0] / 2, -zSize / 2);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(xSize / 2, yLength[0] / 2, zSize / 2);gl.Translate(xSize + 0.5f, 0.0f, 0.0f);gl.Scale(xArm1, 1.0f, 1.0f);DrawCube(ref gl, 0.0f, 0.0f, 0.0f, false);}gl.PopMatrix();//画第二关节，俯仰轴：自身绕X轴旋转->再绕关节1绕Y轴旋转gl.PushMatrix();{gl.Color(0.0f, 0.0f, 1.0f);     //蓝色gl.Translate(0.0f, yLength[1] - 1.0f, 0.0f);//2、绕关节1轴旋转->绕Y轴旋转-旋转半径xSizegl.Translate(-xSize / 2, -yLength[1] / 2, -zSize / 2);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(xSize + xArm1, 0.0f, 0.0f);    //到关节1中心轴距离gl.Translate(xSize / 2, yLength[1] / 2, zSize / 2);//1、绕自身轴旋转-绕X轴旋转  gl.Translate(-xSize, -yLength[1], -zSize / 2);gl.Rotate(angle[1], 1.0f, 0.0f, 0.0f);gl.Translate(xSize, yLength[1], zSize / 2);//绘制当前轴gl.Scale(xSize, yLength[1], zSize);DrawCube(ref gl, 0.0f, 0.0f, 0.0f, false);}gl.PopMatrix();//画第三关节，俯仰轴：自身绕X轴旋转->然后绕关节2绕X轴旋转->再绕关节1绕Y轴旋转gl.PushMatrix();{gl.Color(0.0f, 1.0f, 0.0f);     //绿色//gl.Translate(0.0f, 0.0f, zSize / 2);//3、绕关节1轴旋转->绕Y轴旋转gl.Translate(-xSize / 2, -yLength[1] / 2, -zSize / 2);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(xSize * 2 + xArm1, 0.0f, 0.0f);gl.Translate(xSize / 2, yLength[1] / 2, zSize / 2);gl.Translate(0.0f, yLength[1] + yLength[2] - 2.0f, -zSize / 2);   //关节3位置//2、绕关节2轴旋转->绕X轴旋转-旋转半径yLength[2]                gl.Translate(-xSize, -(yLength[1] + yLength[2] - 2.0f), zSize / 2);gl.Rotate(angle[1], 1.0f, 0.0f, 0.0f);gl.Translate(xSize, yLength[1] + yLength[2] - 2.0f, zSize / 2);//1、绕自身轴旋转-绕X轴旋转  gl.Translate(-xSize, -yLength[2], -zSize / 2);gl.Rotate(angle[2], 1.0f, 0.0f, 0.0f);gl.Translate(xSize, yLength[2], zSize / 2);//绘制当前轴gl.Scale(xSize, yLength[2], zSize);DrawCube(ref gl, 0.0f, 0.0f, 0.0f, false);}gl.PopMatrix();//画第四关节,翻滚轴：自身绕Z轴旋转->再绕关节3绕X轴旋转->再绕关节2绕X轴旋转->再绕关节1绕Y轴旋转gl.PushMatrix();{float axis4Xsize = xSize * 3.0f, axis4Ysize = yLength[3], axis4Zsize = zSize * 1.5f;gl.Color(1.0f, 0.0f, 1.0f);     //紫颜色gl.Translate(0.0f, yLength[1] + yLength[2], 0.0f);//4、绕关节1轴旋转-绕Y轴旋转                gl.Translate(-xSize / 2, 0.0f, -zSize / 2);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(xSize * 3 + xArm1, 0.0f, axis4Zsize / 2);    //到关节1中心轴距离gl.Translate(xSize / 2, 0.0f, zSize / 2);//3、绕关节2轴旋转-绕X轴旋转gl.Translate(-axis4Xsize/2, -yLength[1] - yLength[2], -axis4Zsize/2);gl.Rotate(angle[1], 1.0f, 0.0f, 0.0f);gl.Translate(axis4Xsize/2, yLength[1] + yLength[2], axis4Zsize/2);//2、绕关节3轴旋转-绕X轴旋转gl.Translate(-axis4Xsize / 2, -yLength[2] - yLength[3], -axis4Zsize / 2);gl.Rotate(angle[2], 1.0f, 0.0f, 0.0f);gl.Translate(axis4Xsize / 2, yLength[2] + yLength[3], axis4Zsize / 2);                               //1、绕自身轴旋转-绕Y轴旋转 gl.Translate(-axis4Xsize / 2, 0.0f, -axis4Zsize / 2);gl.Rotate(angle[3], 0.0f, 1.0f, 0.0f);                gl.Translate(axis4Xsize / 2, 0.0f, axis4Zsize / 2);//绘制当前轴gl.Scale(axis4Xsize, axis4Ysize, axis4Zsize);DrawCube(ref gl, 0.0f, 0.0f, 0.0f, false);}gl.PopMatrix();}