抓取分析测试

public void Pick_Down(double[] pointt1, double[] pointt2, double[] pointt3)
        {
            Trajectory_yuanhu.trajectory_s(pointt1, pointt2);
            int Row;
            Row = Trajectory_yuanhu.Nn + 1;
            double[,] B11 = new double[Row, 3];
            double[,] B12 = new double[Row, 3];
            double[,] B13 = new double[Row, 3];
            Point_Down(pointt1, pointt2);
            //将轨迹对应的逆解求出
            
            for (int i = 0; i < Row; i = i + 1)
            {
                B11[i, 0] = Inverse.B1[i, 0];
                B11[i, 1] = Inverse.B1[i, 1];
                B11[i, 2] = Inverse.B1[i, 2];
            }
            Point_Down(pointt2, pointt3);
            for (int i = 0; i < Row; i = i + 1)
            {
                B12[i, 0] = Inverse.B1[i, 0];
                B12[i, 1] = Inverse.B1[i, 1];
                B12[i, 2] = Inverse.B1[i, 2];
            }
            Point_Down(pointt3, pointt1);
            for (int i = 0; i < Row; i = i + 1)
            {
                B13[i, 0] = Inverse.B1[i, 0];
                B13[i, 1] = Inverse.B1[i, 1];
                B13[i, 2] = Inverse.B1[i, 2];
            }
            double speed = gear * decimal.ToDouble(run_speed.Value) * (fp / (2 * Math.PI));      //单位：unit/s                
            LTSMC.smc_conti_set_lookahead_mode(CardNum, 0, 0, 0, 0, 0);
            LTSMC.smc_conti_open_list(CardNum, 0, 3, axis_all);
            LTSMC.smc_set_vector_profile_unit(CardNum, 0, speed, speed, acc / 2, dec / 2, speed);//设置插补运动速度参数   
            LTSMC.smc_set_vector_s_profile(CardNum, 0, 0, acc / 2);//设置s曲线平滑时间
            LTSMC.smc_conti_change_speed_ratio(CardNum, 0, 0.5);
            LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { B11[0, 0], B11[0, 1], B11[0, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
            int xh = 1;
            while (xh < 20)
            {
                // LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);
                LTSMC.smc_conti_write_outbit(CardNum, 0, 15, 0, 0);
                LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);
                for (int i = 0; i < Row; i = i + 1)
                {
                    LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { B11[i, 0], B11[i, 1], B11[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
                }
                //  LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);
                LTSMC.smc_conti_write_outbit(CardNum, 0, 15, 1, 0);
                LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);

                for (int i = 0; i < Row; i = i + 1)
                {
                    LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { B12[i, 0], B12[i, 1], B12[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
                }
                //  LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);
                LTSMC.smc_conti_write_outbit(CardNum, 0, 15, 0, 0);
                LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);

                for (int i = 0; i < Row; i = i + 1)
                {
                    LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { B13[i, 0], B13[i, 1], B13[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
                }
                // LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);
                LTSMC.smc_conti_write_outbit(CardNum, 0, 15, 1, 0);
                LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);

                for (int i = 0; i < Row; i = i + 1)
                {
                    LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { B11[i, 0], B11[i, 1], B11[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
                }
                //  LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);
                LTSMC.smc_conti_write_outbit(CardNum, 0, 15, 0, 0);
                LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);

                for (int i = 0; i < Row; i = i + 1)
                {
                    LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { B12[i, 0], B12[i, 1], B12[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
                }
                //  LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);
                LTSMC.smc_conti_write_outbit(CardNum, 0, 15, 1, 0);
                LTSMC.smc_conti_delay(CardNum, 0, 0.1, 0);
                for (int i = 0; i < Row; i = i + 1)
                {
                    LTSMC.smc_conti_line_unit(CardNum, 0, 3, axis_all, new double[] { B13[i, 0], B13[i, 1], B13[i, 2] }, 1, 0);//运动模式，0： 相对坐标模式， 1： 绝对坐标模式
                }

                xh = xh + 1;
            }
            LTSMC.smc_conti_start_list(CardNum, 0);//启动插补
            LTSMC.smc_conti_close_list(CardNum, 0);//关闭插补缓冲区
        }
        //将两点的轨迹离散
        public void Point_Down(double[] pointtt1, double[] pointtt2)
        {
            Trajectory_yuanhu.trajectory_s(pointtt1, pointtt2);
            int Row;
            Row = Trajectory_yuanhu.Nn + 1;
            double[] GX = new double[Row];
            double[] GY = new double[Row];
            double[] GZ = new double[Row];
            double[] V1 = new double[Row];
            double[] V2 = new double[Row];
            double[] V3 = new double[Row];
            double[,] D1 = new double[Row, 3];
            for (int j = 0; j < Row; j++)
            {
                GX[j] = Trajectory_yuanhu.ss[j, 0]; GY[j] = Trajectory_yuanhu.ss[j, 1]; GZ[j] = Trajectory_yuanhu.ss[j, 2]; //把离散的轨迹点存放到
                // S_v[j] = Trajectory_yuanhu.G_v[j]; //离散的轨迹点所对应的速度
            }
            Inverse.inverse_run(GX, GY, GZ);
        }

标签：double,LTSMC,conti,抓取,CardNum,分析测试,smc,Row
来源： https://www.cnblogs.com/charles48789982/p/14348721.html