使用C# 处理机器人运动学

using NumSharp;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Security.Cryptography.X509Certificates;
using System.Text;
using System.Threading.Tasks;

namespace Con_Num
{
    public class MyCover
    {
        /// <summary>
        /// 传入参数说明
        /// </summary>
        /// <param name="px">图纸上该点的x坐标</param>
        /// <param name="py">图纸上该点的y坐标</param>
        /// <param name="pz">图纸上该点的z坐标</param>
        /// <param name="x">在X轴上位移了x长度</param>
        /// <param name="y">在Y轴上位移了y长度</param>
        /// <param name="z">在Z轴上位移了z长度</param>
        /// <param name="a">工件绕Z轴旋转的角度</param>
        /// <param name="b">工件绕Y轴旋转的角度</param>
        /// <param name="r">工件绕X轴旋转的角度</param>
        public MyCover(float px, float py, float pz, float x, float y, float z, float a, float b, float r)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.a = a;
            this.b = b;
            this.r = r;
            this.rot = new float[4, 4] 
            { 
                {
                    this.Cos(this.a)*this.Cos(this.b),
                    this.Cos(this.a)*this.Sin(this.b)*this.Sin(this.r)-this.Sin(this.a)*this.Cos(this.r),
                    this.Cos(this.a)*this.Sin(this.b)*this.Cos(this.r)+this.Sin(this.a)*this.Sin(this.r),
                    this.x
                },
                {
                    this.Sin(this.a)*this.Cos(this.b),
                    this.Sin(this.a)*this.Sin(this.b)*this.Sin(this.r)+this.Cos(this.a)*this.Cos(this.r),
                    this.Sin(this.a)*this.Sin(this.b)*this.Cos(this.r)-this.Cos(this.a)*this.Sin(this.r),
                    this.y
                },
                {
                    -this.Sin(this.b),
                    this.Cos(this.b)*this.Sin(this.r),
                    this.Cos(this.b)*this.Cos(this.r),
                    this.z
                },
                {
                    0,
                    0,
                    0,
                    1
                },
            };
            this.line = new float[4, 1] { { px}, { py }, { pz }, { 1 } };
        }
        #region 字段和属性

        
        private float[,] rot;

        public float[,] Rot
        {
            get { return rot; }
            set { rot = value; }
        }

        private float[,] line;

        public float[,] Line
        {
            get { return line; }
            set { line = value; }
        }

        private float x;

        public float X
        {
            get { return x; }
            set { x = value; }
        }

        private float y;

        public float Y
        {
            get { return y; }
            set { y = value; }
        }

        private float z;

        public float Z
        {
            get { return z; }
            set { z = value; }
        }

        private float px;

        public float Px
        {
            get { return px; }
            set { px = value; }
        }

        private float py;

        public float Py
        {
            get { return py; }
            set { py = value; }
        }

        private float pz;

        public float Pz
        {
            get { return pz; }
            set { pz = value; }
        }

        private float a;

        public float A
        {
            get { return a; }
            set { a = value; }
        }

        private float b;

        public float B
        {
            get { return b; }
            set { b = value; }
        }

        private float r;

        public float R
        {
            get { return r; }
            set { r = value; }
        }

        #endregion

        public float Cos(float angle)
        {
            return (float)Math.Round(Math.Cos(angle * 1.00 * (Math.PI) / 180), 5);
        }

        public float Sin(float angle)
        {
            return (float)Math.Round(Math.Sin(angle * 1.00 * (Math.PI) / 180), 5);
        }

        public NDArray TestNdArray()
        {
            //float[,] rotMatrix, float[,] pointMatrix
            float[,] rotMatrix = this.rot;
            float[,] pointMatrix = line;
            NDArray ndRotMatrix = np.array(rotMatrix);
            NDArray ndPointMatrix = np.array(pointMatrix);

            return np.matmul(ndRotMatrix, ndPointMatrix);
        }
    }
}

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using NumSharp;

namespace Con_Num
{
    internal class Program
    {

        static void Main(string[] args)
        {
            //int[,] Rot = new int[4, 4];
            //int[,] Lin = new int[4, 1];
            //NDArray ndarr = np.array(Rot);

            //Console.WriteLine(Math.Round(Math.Sin(90 * 1.00 * (Math.PI) / 180), 5));
            //Console.ReadKey();

            MyCover myCover = new MyCover(0, 0, 1, 0, 0, 0, 0, 0, 30);

            NDArray arr1 = myCover.TestNdArray();
            NDArray arrRot = np.array(myCover.Rot);
            Console.WriteLine(arr1.ToString());
            //[[0],
            //[-0.5],
            //[0.86603],
            //[1]]

            Console.WriteLine("=================");
            Console.WriteLine(arrRot.ToString());
            Console.WriteLine("======****===========");
            Console.WriteLine(arrRot["1,1:"].ToString());
            Console.WriteLine("=================");
            Console.WriteLine(arr1["0, 0"].ToString()); //0
            Console.WriteLine(arr1["1, 0"].ToString()); //-0.5
            Console.WriteLine(arr1["2, 0"].ToString()); //0.86603
            Console.ReadKey();
        }


    }
}