用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系统

 

    /// <summary>
    /// 根据给的点获得数据
    /// </summary>

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

        /// <summary>
        /// 路径FRAME
        /// </summary>
        public static double[] TrajFrame = new double[6];
        /// <summary>
        /// 路径偏移
        /// </summary>
        public static double[] TrajOffset = new double[6];
        /// <summary>
        /// 圆的直径
        /// </summary>
        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)