用C#写一个机器人基础知识类


1.工业机器人

运动学

1.DH参数

2.自由度

3.坐标系

4.坐标系转换

5.正算与反算

6.雅可比矩阵

7.轨迹规划

8.Blend算法

9.速度,加速度,位置

 

动力学

1.负载

2.力反馈

3.柔性

4.刚性

 

控制理论

1.boxcar

2.反馈模型

 

硬件

1.马达

2.编码器

3.减速器

4.驱动

5.控制器

6.电源

7.连接部件

 

通讯

1.ftp

2.soap

3.socket

4.profit bus

5.can

6.tcp

7.modbus

 

系统

vxwork系统

 

    /// 
    /// 根据给的点获得数据
    /// 

    public class Val3
    {
        #region 公共变量
        /// 
        /// 产品Frame
        /// 
        public static double[] RecipeFrame = new double[6];
        /// 
        ///产品偏移
        /// 
        public static double[] RecipeOffset = new double[6];
        /// 
        /// 工具信息
        /// 
        public static double[] Tool = new double[6];

        /// 
        /// 路径FRAME
        /// 
        public static double[] TrajFrame = new double[6];
        /// 
        /// 路径偏移
        /// 
        public static double[] TrajOffset = new double[6];
        /// 
        /// 圆的直径
        /// 
        public static double TrajDiameter = 0;

        public static double Blend = 0;

        public static double RecipeSpeed = 0;

        public static double TrajSpeed = 0;


        //

        public static double[][] Point;

        public static double[][] SaftyPoint;

        public static double[][] BeforePoint;

        public static double[][] AfterPoint;

        public static double[][] BeforeSpeed;

        public static double[][] SaftySpeed;

        public static double[,] DH;
        public static double[,] DH160L = new double[,] { { 150, 825, 0,  0,   0,  0 },
                                                         { 0,    0,  0,  0,   0,  0 }, 
                                                         { 0,    0,  0,  925, 0,  110}, 
                                                         { -90,  0,  90, -90, 90, 0 }, 
                                                         { 0,    0,  0,  0,   0,  0 }, 
                                                         { 0,  -90,  90, 0,   0,  0 }
                                                       };
        //a,0,d.

        #endregion

        #region 公共函数
        public static double[] Joint2Point(double[] Joint, double[,] DH)
        {

            if (!IsJoint(Joint) || DH == null || DH.Length != 36)
            {
                return null;
            }
            double[] Trsf0 = new double[] { 0, 0, 0, 0, 0, Joint[0] + DH[5, 0] };
            double[] Trsf1 = new double[] { DH[0, 0], DH[1, 0], DH[2, 0], DH[3, 0], DH[4, 0], Joint[1] + DH[5, 1] };
            double[] Trsf2 = new double[] { DH[0, 1], DH[1, 1], DH[2, 1], DH[3, 1], DH[4, 1], Joint[2] + DH[5, 2] };
            double[] Trsf3 = new double[] { DH[0, 2], DH[1, 2], DH[2, 2], DH[3, 2], DH[4, 2], Joint[3] + DH[5, 3] };
            double[] Trsf4 = new double[] { DH[0, 3], DH[1, 3], DH[2, 3], DH[3, 3], DH[4, 3], Joint[4] + DH[5, 4] };
            double[] Trsf5 = new double[] { DH[0, 4], DH[1, 4], DH[2, 4], DH[3, 4], DH[4, 4], Joint[5] + DH[5, 5] };
            double[] Trsf6 = new double[] { DH[0, 5], DH[1, 5], DH[2, 5], DH[3, 5], DH[4, 5], 0 };

            double[] Buff = TrsfTrsf(Trsf0, Trsf1);
            Buff = TrsfTrsf(Buff, Trsf2);
            Buff = TrsfTrsf(Buff, Trsf3);
            Buff = TrsfTrsf(Buff, Trsf4);
            Buff = TrsfTrsf(Buff, Trsf5);
            return TrsfTrsf(Buff, Trsf6);

        }

        private static double[] StandardTrsf(double[] Trsf)
        {

            if (Trsf == null || Trsf.Length != 6)
                return null;
            double[] Res = new double[6];
            Res[0] = Trsf[0] / 1000.0;
            Res[1] = Trsf[1] / 1000.0;
            Res[2] = Trsf[2] / 1000.0;


            Res[3] = Trsf[3] / 180.0 * Math.PI;
            Res[4] = Trsf[4] / 180.0 * Math.PI;
            Res[5] = Trsf[5] / 180.0 * Math.PI;

            return Res;

        }
        public static double[] Point2Joint(double[] Trsf, double[,] DH, bool[] Pose)
        {

            if (!IsTrsf(Trsf) || DH == null || DH.Length != 36 || Pose == null || Pose.Length < 3)
            {
                return null;
            }
            double[] Joint = new double[6];
            double[] Trsf0 = new double[] { 0, 0, 0, 0, 0, Joint[0] + DH[5, 0] };
            double[] Trsf1 = new double[] { DH[0, 0], DH[1, 0], DH[2, 0], DH[3, 0], DH[4, 0], Joint[1] + DH[5, 1] };
            double[] Trsf2 = new double[] { DH[0, 1], DH[1, 1], DH[2, 1], DH[3, 1], DH[4, 1], Joint[2] + DH[5, 2] };
            double[] Trsf3 = new double[] { DH[0, 2], DH[1, 2], DH[2, 2], DH[3, 2], DH[4, 2], Joint[3] + DH[5, 3] };
            double[] Trsf4 = new double[] { DH[0, 3], DH[1, 3], DH[2, 3], DH[3, 3], DH[4, 3], Joint[4] + DH[5, 4] };
            double[] Trsf5 = new double[] { DH[0, 4], DH[1, 4], DH[2, 4], DH[3, 4], DH[4, 4], Joint[5] + DH[5, 5] };
            double[] Trsf6 = new double[] { DH[0, 5], DH[1, 5], DH[2, 5], DH[3, 5], DH[4, 5], 0 };

            double[] Buff = TrsfTrsf(Trsf, InverseTrsf(Trsf6));
            if (Buff == null)
            {
                return null;

            }
            if (Pose[0])
            {
                Joint[0] = Math.Atan2(Buff[1], Buff[0]) - Math.Atan2(DH[1, 0], -Math.Sqrt(Buff[0] * Buff[0] + Buff[1] * Buff[1] - DH[1, 0] * DH[1, 0]));
            }
            else
            {

                Joint[0] = Math.Atan2(Buff[1], Buff[0]) - Math.Atan2(DH[1, 0], Math.Sqrt(Buff[0] * Buff[0] + Buff[1] * Buff[1] - DH[1, 0] * DH[1, 0]));
            }
            Joint[0] = Joint[0] * 180 / Math.PI;



            return Joint;


        }

        /// 
        /// 三点求用户坐标系,rz求出来有点异常
        /// 
        /// 
        /// 
        /// 
        /// 
        public static double[] GetFrame(double[] Zero, double[] Xaxis, double[] Yaxis)
        {
            //1.参数是否合法
            if (!IsTrsf(Zero) || !IsTrsf(Yaxis) || !IsTrsf(Xaxis))
            {
                return null;
            }
            try
            {
                //2。三点不能共线
                double a = Distance(Xaxis, Yaxis);
                double b = Distance(Zero, Yaxis);
                double c = Distance(Zero, Xaxis);
                if (a == b + c || c == a + b || b == a + c)
                {
                    return null;

                }

                //3建立两个向量
                double[] OX = new double[] { -Zero[0] + Xaxis[0], -Zero[1] + Xaxis[1], -Zero[2] + Xaxis[2] };
                double[] OY = new double[] { -Zero[0] + Yaxis[0], -Zero[1] + Yaxis[1], -Zero[2] + Yaxis[2] };

                double[] Vz = GetNormalVector(OX, OY);
                //  double[] Vx = GetNormalVector(OY, Vz);
                double[] Vx = UnitVer(OX);
                double[] Vy = GetNormalVector(Vz, Vx);

                if (Vz == null || Vx == null || Vy == null)
                {

                    return null;
                }

                double[,] frame = new double[4, 4];

                frame[0, 0] = Vx[0];
                frame[1, 0] = Vx[1];
                frame[2, 0] = Vx[2];
                frame[3, 0] = 0;


                frame[0, 1] = Vy[0];
                frame[1, 1] = Vy[1];
                frame[2, 1] = Vy[2];
                frame[3, 1] = 0;


                frame[0, 2] = Vz[0];
                frame[1, 2] = Vz[1];
                frame[2, 2] = Vz[2];
                frame[3, 2] = 0;

                frame[0, 3] = Zero[0] / 1000.0;
                frame[1, 3] = Zero[1] / 1000.0;
                frame[2, 3] = Zero[2] / 1000.0;
                frame[3, 3] = 1;


                return Matrix2Trsf(frame);

            }
            catch
            {
                return null;

            }



        }


        /// 
        /// Trsf相乘
        /// 
        /// 
        /// 
        /// 
        public static double[] TrsfTrsf(double[] TrsfA, double[] TrsfB)
        {
            if (!IsTrsf(TrsfA) || !IsTrsf(TrsfB))
            {

                return null;
            }

            try
            {

                return Matrix2Trsf(MatrixMatrix(Trsf2Matrix(TrsfA), Trsf2Matrix(TrsfB)));

            }

            catch
            {
                return null;

            }


        }

        /// 
        /// 两向量获得法向量
        /// 
        /// 
        /// 
        /// 

        private static double[] GetNormalVector(double[] VectA, double[] VectB)
        {
            if (VectA == null || VectB == null || VectA.Length < 3 || VectB.Length < 3)
            {
                return null;
            }
            double[] Normal = new double[3];
            Normal[0] = VectA[1] * VectB[2] - VectA[2] * VectB[1];
            Normal[1] = VectA[2] * VectB[0] - VectA[0] * VectB[2];
            Normal[2] = VectA[0] * VectB[1] - VectA[1] * VectB[0];

            return UnitVer(Normal);
        }
        /// 
        /// 两矩阵相乘
        /// 
        /// 
        /// 
        /// 
        private static double[,] MatrixMatrix(double[,] MatrixA, double[,] MatrixB)
        {
            if (MatrixA == null || MatrixB == null || MatrixA.Length != 16 || MatrixB.Length != 16)
            {
                return null;
            }

            try
            {

                double[,] MatrixC = new double[4, 4];
                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < 4; j++)
                    {
                        MatrixC[i, j] = 0;

                        for (int k = 0; k < 4; k++)
                        {
                            MatrixC[i, j] = MatrixC[i, j] + MatrixA[i, k] * MatrixB[k, j];
                        }
                    }
                }
                return MatrixC;
            }
            catch
            {
                return null;
            }




        }
        /// 
        ///  计算两点间的距离
        /// 
        /// 
        /// 
        /// 
        public static double Distance(double[] TrsfA, double[] TrsfB)
        {



            if (!IsTrsf(TrsfA) || !IsTrsf(TrsfB))
            {
                return -1;
            }

            try
            {
                double x = (TrsfA[0] - TrsfB[0]) * (TrsfA[0] - TrsfB[0]);
                double y = (TrsfA[1] - TrsfB[1]) * (TrsfA[1] - TrsfB[1]);
                double z = (TrsfA[2] - TrsfB[2]) * (TrsfA[2] - TrsfB[2]);



                return Math.Sqrt(x + y + z);

            }
            catch
            {
                return -1;
            }



        }
        /// 
        /// Trsf到矩阵
        /// 
        /// 
        /// 
        private static double[,] Trsf2Matrix(double[] Trsf)
        {

            if (!IsTrsf(Trsf))
            {
                return null;
            }

            double angle = 0;
            double[,] data = new double[4, 4];
            data[0, 0] = 1;
            data[0, 1] = 0;
            data[0, 2] = 0;
            data[0, 3] = Trsf[0] / 1000.0;

            data[1, 0] = 0;
            data[1, 1] = 1;
            data[1, 2] = 0;
            data[1, 3] = Trsf[1] / 1000.0;

            data[2, 0] = 0;
            data[2, 1] = 0;
            data[2, 2] = 1;
            data[2, 3] = Trsf[2] / 1000.0;

            data[3, 0] = 0;
            data[3, 1] = 0;
            data[3, 2] = 0;
            data[3, 3] = 1;


            angle = Trsf[3] * Math.PI / 180.0;
            double[,] rx = new double[4, 4];
            rx[0, 0] = 1;
            rx[0, 1] = 0;
            rx[0, 2] = 0;
            rx[0, 3] = 0;

            rx[1, 0] = 0;
            rx[1, 1] = Math.Cos(angle);
            rx[1, 2] = Math.Sin(angle) * (-1);
            rx[1, 3] = 0;

            rx[2, 0] = 0;
            rx[2, 1] = Math.Sin(angle) * (1);
            rx[2, 2] = Math.Cos(angle);
            rx[2, 3] = 0;

            rx[3, 0] = 0;
            rx[3, 1] = 0;
            rx[3, 2] = 0;
            rx[3, 3] = 1;



            angle = Trsf[4] * Math.PI / 180.0;
            double[,] ry = new double[4, 4];
            ry[0, 0] = Math.Cos(angle);
            ry[0, 1] = 0;
            ry[0, 2] = Math.Sin(angle) * (1);
            ry[0, 3] = 0;

            ry[1, 0] = 0;
            ry[1, 1] = 1;
            ry[1, 2] = 0;
            ry[1, 3] = 0;

            ry[2, 0] = Math.Sin(angle) * (-1);
            ry[2, 1] = 0;
            ry[2, 2] = Math.Cos(angle);
            ry[2, 3] = 0;

            ry[3, 0] = 0;
            ry[3, 1] = 0;
            ry[3, 2] = 0;
            ry[3, 3] = 1;

            angle = Trsf[5] * Math.PI / 180.0;
            double[,] rz = new double[4, 4];

            rz[0, 0] = Math.Cos(angle);
            rz[0, 1] = Math.Sin(angle) * (-1);
            rz[0, 2] = 0;
            rz[0, 3] = 0;

            rz[1, 0] = Math.Sin(angle) * (1);
            rz[1, 1] = Math.Cos(angle);
            rz[1, 2] = 0;
            rz[1, 3] = 0;

            rz[2, 0] = 0;
            rz[2, 1] = 0;
            rz[2, 2] = 1;
            rz[2, 3] = 0;

            rz[3, 0] = 0;
            rz[3, 1] = 0;
            rz[3, 2] = 0;
            rz[3, 3] = 1;

            return MatrixMatrix(MatrixMatrix(MatrixMatrix(data, rx), ry), rz);


        }
        /// 
        /// 判断是否trsf
        /// 
        /// 
        /// 
        private static bool IsTrsf(double[] point)
        {

            return point != null && point.Length == 6;

        }
        /// 
        /// 判断是否Joint
        /// 
        /// 
        /// 
        public static bool IsJoint(double[] joint)
        {
            return IsTrsf(joint);
        }

        /// 
        /// 矩阵转换
        /// 
        /// 
        /// 
        private static double[] Matrix2Trsf(double[,] Matrix)
        {
            if (Matrix == null || Matrix.Length != 16 || Matrix[3, 0] != 0 || Matrix[3, 1] != 0 || Matrix[3, 2] != 0 || Matrix[3, 3] != 1)
            {

                return null;
            }

            try
            {
                double[] Res = new double[6];
                Res[0] = Matrix[0, 3] * 1000;
                Res[1] = Matrix[1, 3] * 1000;
                Res[2] = Matrix[2, 3] * 1000;



                //matrix[0,2]必须小于1大于-1
                //n 0 a
                if (Matrix[0, 2] < (-1.0 + Math.Pow(1, -5) * Math.Pow(1, -5) / 2.0))
                {
                    Res[3] = 0;
                    Res[4] = -Math.PI / 2.0;
                    Res[5] = Math.Atan2(Matrix[0, 0], Matrix[0, 1]);


                }
                else if (Matrix[0, 2] > (1.0 - Math.Pow(1, -5) * Math.Pow(1, -5) / 2.0))
                {
                    Res[3] = 0;
                    Res[4] = Math.PI / 2.0;
                    Res[5] = Math.Atan2(Matrix[0, 0], -1 * Matrix[0, 1]);

                }
                else
                {
                    double l_sign = 1.0;
                    Res[4] = Math.Asin(Matrix[0, 2]);
                    if ((Matrix[2, 2] < 0.0) && (Matrix[0, 0] < 0.0) && ((Math.Abs(Matrix[0, 2]) > Math.Pow(1, -10)) || (Math.Abs(Matrix[2, 1]) > Math.Pow(1, -10))))
                    {
                        if (Res[4] >= 0.0)
                        {
                            Res[4] = Math.PI - Res[4];
                        }
                        else
                        {
                            Res[4] = -Math.PI - Res[4];
                        }
                        l_sign = -1.0;
                    }
                    Res[3] = Math.Atan2(-l_sign * Matrix[1, 2], l_sign * Matrix[2, 2]);
                    Res[5] = Math.Atan2(-l_sign * Matrix[0, 1], l_sign * Matrix[0, 0]);



                }

                Res[3] = Res[3] * 180.0 / Math.PI;
                Res[4] = Res[4] * 180.0 / Math.PI;
                Res[5] = Res[5] * 180.0 / Math.PI;

                return Res;

            }

            catch
            {

                return null;

            }

        }

        /// 
        /// 转换求逆
        /// 
        /// 
        /// 
        public static double[] InverseTrsf(double[] Trsf)
        {
            if (!IsTrsf(Trsf))
            {
                return null;
            }

            double angle = 0;
            double[,] data = new double[4, 4];
            data[0, 0] = 1;
            data[0, 1] = 0;
            data[0, 2] = 0;
            data[0, 3] = Trsf[0] / 1000.0 * (-1);

            data[1, 0] = 0;
            data[1, 1] = 1;
            data[1, 2] = 0;
            data[1, 3] = Trsf[1] / 1000.0 * (-1);

            data[2, 0] = 0;
            data[2, 1] = 0;
            data[2, 2] = 1;
            data[2, 3] = Trsf[2] / 1000.0 * (-1);

            data[3, 0] = 0;
            data[3, 1] = 0;
            data[3, 2] = 0;
            data[3, 3] = 1;


            angle = Trsf[3] * Math.PI / 180.0;
            double[,] rx = new double[4, 4];
            rx[0, 0] = 1;
            rx[0, 1] = 0;
            rx[0, 2] = 0;
            rx[0, 3] = 0;

            rx[1, 0] = 0;
            rx[1, 1] = Math.Cos(angle);
            rx[1, 2] = Math.Sin(angle);
            rx[1, 3] = 0;

            rx[2, 0] = 0;
            rx[2, 1] = Math.Sin(angle) * (-1);
            rx[2, 2] = Math.Cos(angle);
            rx[2, 3] = 0;

            rx[3, 0] = 0;
            rx[3, 1] = 0;
            rx[3, 2] = 0;
            rx[3, 3] = 1;



            angle = Trsf[4] * Math.PI / 180;
            double[,] ry = new double[4, 4];
            ry[0, 0] = Math.Cos(angle);
            ry[0, 1] = 0;
            ry[0, 2] = Math.Sin(angle) * (-1);
            ry[0, 3] = 0;

            ry[1, 0] = 0;
            ry[1, 1] = 1;
            ry[1, 2] = 0;
            ry[1, 3] = 0;

            ry[2, 0] = Math.Sin(angle) * (1);
            ry[2, 1] = 0;
            ry[2, 2] = Math.Cos(angle);
            ry[2, 3] = 0;

            ry[3, 0] = 0;
            ry[3, 1] = 0;
            ry[3, 2] = 0;
            ry[3, 3] = 1;

            angle = Trsf[5] * Math.PI / 180;
            double[,] rz = new double[4, 4];

            rz[0, 0] = Math.Cos(angle);
            rz[0, 1] = Math.Sin(angle) * (1);
            rz[0, 2] = 0;
            rz[0, 3] = 0;

            rz[1, 0] = Math.Sin(angle) * (-1);
            rz[1, 1] = Math.Cos(angle);
            rz[1, 2] = 0;
            rz[1, 3] = 0;

            rz[2, 0] = 0;
            rz[2, 1] = 0;
            rz[2, 2] = 1;
            rz[2, 3] = 0;

            rz[3, 0] = 0;
            rz[3, 1] = 0;
            rz[3, 2] = 0;
            rz[3, 3] = 1;

            return Matrix2Trsf(MatrixMatrix(MatrixMatrix(MatrixMatrix(rz, ry), rx), data));

        }
        /// 
        /// 
        /// 
        /// 
        /// 
        private static double[] UnitVer(double[] Normal)
        {

            if (Normal == null || Normal.Length < 3)
            {
                return null;
            }
            double R = Math.Sqrt(Normal[0] * Normal[0] + Normal[1] * Normal[1] + Normal[2] * Normal[2]);
            if (R == 0)
            {
                return null;
            }
            double[] point = new double[3];
            point[0] = Normal[0] / R;
            point[1] = Normal[1] / R;
            point[2] = Normal[2] / R;
            return point;

        }

        /// 
        /// 三点求外接圆心
        /// 
        /// 
        /// 
        /// 
        /// 
        public static double[] GetCircleCenter(double[] Trsf1, double[] Trsf2, double[] Trsf3)
        {

            if (GetFrame(Trsf1, Trsf2, Trsf3) == null)
                return null;





            return null;


        }

        /// 
        /// 三点求外接圆的半径
        /// 
        /// 
        /// 
        /// 
        /// 
        public static double GetCircleRadius(double[] Trsf1, double[] Trsf2, double[] Trsf3)
        {
            double[] Center = GetCircleCenter(Trsf1, Trsf2, Trsf3);

            if (Center == null)
            {
                return 0;
            }

            double len = (Center[0] - Trsf1[0]) * (Center[0] - Trsf1[0]) + (Center[1] - Trsf1[1]) * (Center[1] - Trsf1[1]) + (Center[2] - Trsf1[2]) * (Center[2] - Trsf1[2]);
            return Math.Sqrt(len);

        }

        /// 
        /// 四点求球心
        /// 
        /// 
        /// 
        /// 
        /// 
        /// 
        public static double[] GetBallCenter(double[] Trsf1, double[] Trsf2, double[] Trsf3, double[] Trsf4)
        {

            if (GetFrame(Trsf1, Trsf2, Trsf3) == null)
                return null;





            return null;


        }

        /// 
        /// 四点求球的半径
        /// 
        /// 
        /// 
        /// 
        /// 
        /// 
        public static double GetBallRadius(double[] Trsf1, double[] Trsf2, double[] Trsf3, double[] Trsf4)
        {
            double[] Center = GetBallCenter(Trsf1, Trsf2, Trsf3, Trsf4);

            if (Center == null)
            {
                return 0;
            }

            double len = (Center[0] - Trsf1[0]) * (Center[0] - Trsf1[0]) + (Center[1] - Trsf1[1]) * (Center[1] - Trsf1[1]) + (Center[2] - Trsf1[2]) * (Center[2] - Trsf1[2]);
            return Math.Sqrt(len);

        }


        /// 
        /// 技术来源http://zh.wikipedia.org/zh-cn/%E8%B2%9D%E8%8C%B2%E6%9B%B2%E7%B7%9A
        ///         
        /// 
        /// 
        /// 
        /// 
        /// 
        /// 
        /// 
        /// 
        public static double[,] GetBlendPoints(double[] Trsf1, double[] Trsf2, double[] Trsf3, int Count)
        {
            if (!IsTrsf(Trsf1) || !IsTrsf(Trsf2) || !IsTrsf(Trsf3) || Count < 0)
            {
                return null;

            }
            double[,] Res = new double[Count, 6];
            double t = 1.0 / Count;
            for (int i = 0; i < Count; i++)
            {
                t = 1.0 / Count * i;
                for (int j = 0; j < 6; j++)
                {
                    Res[i, j] = (1 - t) * (1 - t) * Trsf1[j] + 2 * t * Trsf2[j] + t * t * Trsf3[j];

                }
            }
            return Res;


        }

        private static double[] GetStartBlend(double[] Trsf1, double[] Trsf2, double reach)
        {
            if (!IsTrsf(Trsf1) || !IsTrsf(Trsf2) || reach < 0)
            {
                return null;

            }
            double len = Distance(Trsf1, Trsf2);
            double[] Res = new double[6];
            for (int i = 0; i < 6; i++)
            {

                Res[i] = reach / len * (Trsf1[i] - Trsf2[i]) + Trsf2[i];
            }

            return Res;

        }
        private static double[] GetEndBlend(double[] Trsf1, double[] Trsf2, double leave)
        {
            if (!IsTrsf(Trsf1) || !IsTrsf(Trsf2) || leave < 0)
            {
                return null;

            }
            double len = Distance(Trsf1, Trsf2);
            double[] Res = new double[6];
            for (int i = 0; i < 6; i++)
            {

                Res[i] = leave / len * (Trsf2[i] - Trsf1[i]) + Trsf1[i];
            }

            return Res;

        }
        public static double[,] CartBlend(double[] Start, double[] Center, double[] end, double reach, double leave, int Count)
        {


            return GetBlendPoints(GetStartBlend(Start, Center, reach), Center, GetEndBlend(Start, Center, leave), Count);
        }








        public static bool[] GetPose(double[] Joint,double[,] DH)
        {
            if (!IsJoint(Joint))
            {
                return null;
            }
            bool[] Res = new bool[3];
            double[] d = new double[3];
            d[0] = DH[2, 3]+DH[2,5]+DH[0,1];
            d[1]=DH[2,3];
            d[2]=DH[0,0];


            Res[0]=((d[0]*Math.Sin(Joint[1]+d[1]*Math.Sin(Joint[1]+Joint[2]+d[2]))<0));

            Res[1] = (Joint[2] >= 0);

            Res[2] = (Joint[4] >= 0);
            return Res;
        
        }
        #endregion

    }


 

你可能感兴趣的:(C#)