C#使用SharGL实现PUMA560机械臂

news2024/11/25 6:55:09

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轴旋转-旋转半径xSize
                gl.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();
           
           

        }