【WPF-HelixToolkit】史陶比尔RX160L 机器人仿真器源码学习

仿真器采用WPF开发，利用HelixToolkit显示场景。 源码中正逆运动学算法基于RoboticsLibrary(c++)开源机器人库封装成dll，通过C#调用计算位置正逆解。从源码得知机械臂挂载相机，可以FK模式控制关节位置，可以IK模式控制TCP位姿。可以设定相机锁定目标点，此时IK模式的tcp旋转不可用。程序还有很多功能没有实现。

仿真器操作演示

程序框架

源码笔记：

正逆运动学计算：调用(C++)dll

#region KINEMATICS  位置正逆解


        /// 

        ///正运动学机器人库DLL函数调用
        /// 
        public double[] ForwardKinematics(double j1, double j2, double j3, double j4, double j5, double j6)
        {
            string path = new FileInfo(Assembly.GetEntryAssembly().Location).Directory.ToString();
            string filePath = path + "\\helix3d_under_cam.xml";
            //if (!bCamType)
            //    filePath = path + "\\helix3d_front_cam.xml";


            double[] retArr = new double[6];//正解结果
            IntPtr outPtr = Marshal.AllocHGlobal(6 * Marshal.SizeOf());
            rlForwardKinematics(filePath, CameraCenterZOffset, j1, j2, j3, j4, j5, j6, outPtr);
            Marshal.Copy(outPtr, retArr, 0, 6);
            Marshal.FreeHGlobal(outPtr);
            //上一笛卡尔tcp位姿
            double[] prevCartesians = new double[] { 
                CurCartPos.X, 
                CurCartPos.Y, 
                CurCartPos.Z, 
                CurCartPos.RX, 
                CurCartPos.RY,
                CurCartPos.RZ
            };
            //修正正解结果
            if (Math.Abs(retArr[3] - prevCartesians[3]) == 180)
            {
                retArr[3] = prevCartesians[3];
            }
            if (Math.Abs(retArr[4] - prevCartesians[4]) == 180)
            {
                retArr[4] = prevCartesians[4];
            }
            if (Math.Abs(retArr[5] - prevCartesians[5]) == 180)
            {
                retArr[5] = prevCartesians[5];
            }
            if (Math.Abs(retArr[3] - prevCartesians[3]) > 90)
            {
                int signn = 1;
                if (retArr[3] < 0) signn = -1;
                retArr[3] = -signn * (180 - Math.Abs(retArr[3]));
            }
            if (Math.Abs(retArr[4] - prevCartesians[4]) > 90)
            {
                int signn = 1;
                if (retArr[4] < 0) signn = -1;
                retArr[4] = signn * (180 - Math.Abs(retArr[4]));
                if (Math.Abs(retArr[4] - prevCartesians[4]) > 90)
                {
                    signn = 1;
                    if (retArr[4] < 0) signn = -1;
                    retArr[4] = signn * (180 - Math.Abs(retArr[4]));
                }
            }
            if (Math.Abs(retArr[5] - prevCartesians[5]) > 90 && Math.Abs(retArr[5] - prevCartesians[5]) < 270)
            {
                int signn1 = 1;
                if (retArr[5] < 0) signn1 = -1;
                retArr[5] = -signn1 * (180 - Math.Abs(retArr[5]));
            }


          //更新当前笛卡尔位姿为新的正解结果
            CurCartPos.X = retArr[0];
            CurCartPos.Y = retArr[1];
            CurCartPos.Z = retArr[2];
            CurCartPos.RX = retArr[3];
            CurCartPos.RY = retArr[4];
            CurCartPos.RZ = retArr[5];


            return retArr;
        }


        /// 

        /// 逆运动学机器人库DLL函数调用
        /// 
        public double[] InverseKinematics(double x, double y, double z, double rx, double ry, double rz)
        {
            //double[] jointAngles = new double[6] { mainJoints[0].angle, mainJoints[1].angle, mainJoints[2].angle, mainJoints[3].angle, mainJoints[4].angle, mainJoints[5].angle };
           //记录当前关节角度
            double[] jointAngles = new double[6] { 
                CurJointPos.J1, 
                CurJointPos.J2, 
                CurJointPos.J3, 
                CurJointPos.J4, 
                CurJointPos.J5, 
                CurJointPos.J6 
            };


            int size = Marshal.SizeOf(jointAngles[0]) * jointAngles.Length;//分配空间  字节 6*double（16）
            IntPtr prevJoints = Marshal.AllocHGlobal(size);//
            Marshal.Copy(jointAngles, 0, prevJoints, jointAngles.Length);//关节角度数组 拷贝到指针prevJoints地址


            string path = new FileInfo(Assembly.GetEntryAssembly().Location).Directory.ToString();//获取路径
            string filePath = path + "\\helix3d_under_cam.xml";//史陶比尔运动学模型  xml文件
            //if (!bCamType)
            //    filePath = path + "\\helix3d_front_cam.xml";


            double[] retArr = new double[6];//逆解结果
            IntPtr outPtr = Marshal.AllocHGlobal(6 * Marshal.SizeOf());//输出
            //Stopwatch sw = new Stopwatch();
            //sw.Start();
            rlInverseKinematics(filePath, CameraCenterZOffset, prevJoints, x, y, z, rx, ry, rz, outPtr);//逆解计算
            //sw.Stop();
            //parentWindow.Log($"IK Elapsed={sw.Elapsed}");
            Marshal.Copy(outPtr, retArr, 0, 6);//逆解输出拷贝到double数组
            Marshal.FreeHGlobal(outPtr);//释放分配的资源
            Marshal.FreeHGlobal(prevJoints);
            //更新当前关节位置
            CurJointPos.J1 = retArr[0];
            CurJointPos.J2 = retArr[1];
            CurJointPos.J3 = retArr[2];
            CurJointPos.J4 = retArr[3];
            CurJointPos.J5 = retArr[4];
            CurJointPos.J6 = retArr[5];


            return retArr;
        }
#endregion

贝塞尔曲线插值

//获取贝塞尔曲线插值点：控制点 p0  p1   p2 p3  https://en.wikipedia.org/wiki/B%C3%A9zier_curve  
        public static Point3D GetPoint(Point3D p0, Point3D p1, Point3D p2, Point3D p3, float t)
        {
            if (t < 0)
                t = 0;
            else if (t > 1)
                t = 1;
            float oneMinusT = 1f - t;
            Point3D ret = new Point3D();
            ret.X = oneMinusT* oneMinusT *oneMinusT * p0.X + 3f * oneMinusT * oneMinusT * t * p1.X + 
                3f * oneMinusT * t * t * p2.X + t * t * t * p3.X;
            ret.Y = oneMinusT * oneMinusT * oneMinusT * p0.Y + 3f * oneMinusT * oneMinusT * t * p1.Y +
                3f * oneMinusT * t * t * p2.Y + t * t * t * p3.Y;
            ret.Z = oneMinusT * oneMinusT * oneMinusT * p0.Z + 3f * oneMinusT * oneMinusT * t * p1.Z +
                3f * oneMinusT * t * t * p2.Z + t * t * t * p3.Z;
            return ret;
        }

更新场景中机械臂位姿：

//更新场景中机械臂状态
        private void UpdateRoboticArm(double J1, double J2, double J3, double J4, double J5, double J6, 
            double camOffsetX = 0, double camOffsetY = 0, double camOffsetZ = 0, 
            double camRotAxisX = 0, double camRotAxisY = 0, double camRotAxisZ = 0)
        {
            // TODO: Check new joint angles within range limits


            //更新场景中关节角度 Update Joints Angles
            RobotJoints[0].joint.angle = J1;
            RobotJoints[1].joint.angle = J2;
            RobotJoints[2].joint.angle = J3;
            RobotJoints[3].joint.angle = J4;
            RobotJoints[4].joint.angle = J5;
            RobotJoints[5].joint.angle = J6;


            //更新机器人运动学关节角度(以前的关节) Update Robot Kinematics Joint Angles (Previous Joints)
            RobotKinematics.CurJointPos.J1 = J1;
            RobotKinematics.CurJointPos.J2 = J2;
            RobotKinematics.CurJointPos.J3 = J3;
            RobotKinematics.CurJointPos.J4 = J4;
            RobotKinematics.CurJointPos.J5 = J5;
            RobotKinematics.CurJointPos.J6 = J6;


            // 设置变换矩阵  Setup Transformations
            Transform3DGroup F1;//J1位姿
            Transform3DGroup F2;
            Transform3DGroup F3;
            Transform3DGroup F4;
            Transform3DGroup F5;//J5位姿
            Transform3DGroup F6;//J6 和 相机挂载位姿 
            Transform3DGroup F7;//相机位姿
            Transform3DGroup F8;//机器人法兰点位姿
            Transform3DGroup F9;//相机边界框位姿
            RotateTransform3D R;
            TranslateTransform3D T;
            F1 = new Transform3DGroup();//单位矩阵4X4
            R = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D((double)RobotJoints[0].joint.rotAxisX, (double)RobotJoints[0].joint.rotAxisY, (double)RobotJoints[0].joint.rotAxisZ), J1), new Point3D((double)RobotJoints[0].joint.rotPointX, (double)RobotJoints[0].joint.rotPointY, (double)RobotJoints[0].joint.rotPointZ));
            F1.Children.Add(R);//F1=R
            F2 = new Transform3DGroup();
            T = new TranslateTransform3D(0.0, 0.0, 0.0);
            R = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D((double)RobotJoints[1].joint.rotAxisX, (double)RobotJoints[1].joint.rotAxisY, (double)RobotJoints[1].joint.rotAxisZ), J2), new Point3D((double)RobotJoints[1].joint.rotPointX, (double)RobotJoints[1].joint.rotPointY, (double)RobotJoints[1].joint.rotPointZ));
            F2.Children.Add(T);
            F2.Children.Add(R);
            F2.Children.Add(F1);//左乘    F2= F1*R
            F3 = new Transform3DGroup();
            T = new TranslateTransform3D(0.0, 0.0, 0.0);
            //旋转变换：轴角法+平一点
            R = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D((double)RobotJoints[2].joint.rotAxisX, (double)RobotJoints[2].joint.rotAxisY, (double)RobotJoints[2].joint.rotAxisZ), J3), new Point3D((double)RobotJoints[2].joint.rotPointX, (double)RobotJoints[2].joint.rotPointY, (double)RobotJoints[2].joint.rotPointZ));
            F3.Children.Add(T);
            F3.Children.Add(R);
            F3.Children.Add(F2);//位姿  F3= F2*R  相对自身动坐标系变换
            F4 = new Transform3DGroup();
            T = new TranslateTransform3D(0.0, 0.0, 0.0);
            R = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D((double)RobotJoints[3].joint.rotAxisX, (double)RobotJoints[3].joint.rotAxisY, (double)RobotJoints[3].joint.rotAxisZ), J4), new Point3D((double)RobotJoints[3].joint.rotPointX, (double)RobotJoints[3].joint.rotPointY, (double)RobotJoints[3].joint.rotPointZ));
            F4.Children.Add(T);
            F4.Children.Add(R);
            F4.Children.Add(F3);//F4=F3*R
            F5 = new Transform3DGroup();
            T = new TranslateTransform3D(0.0, 0.0, 0.0);
            R = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D((double)RobotJoints[4].joint.rotAxisX, (double)RobotJoints[4].joint.rotAxisY, (double)RobotJoints[4].joint.rotAxisZ), J5), new Point3D((double)RobotJoints[4].joint.rotPointX, (double)RobotJoints[4].joint.rotPointY, (double)RobotJoints[4].joint.rotPointZ));
            F5.Children.Add(T);
            F5.Children.Add(R);
            F5.Children.Add(F4);//F5=F4*R
            F6 = new Transform3DGroup();
            T = new TranslateTransform3D(0.0, 0.0, 0.0);
            R = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D((double)RobotJoints[5].joint.rotAxisX, (double)RobotJoints[5].joint.rotAxisY, (double)RobotJoints[5].joint.rotAxisZ), J6), new Point3D((double)RobotJoints[5].joint.rotPointX, (double)RobotJoints[5].joint.rotPointY, (double)RobotJoints[5].joint.rotPointZ));
            F6.Children.Add(T);
            F6.Children.Add(R);
            F6.Children.Add(F5);//F6=F5*R


            //F7 = new Transform3DGroup();
            //T = new TranslateTransform3D(camOffsetX, camOffsetY, camOffsetZ);
            //R = new RotateTransform3D(
            //    new AxisAngleRotation3D(
            //        new Vector3D((double)RobotJoints[5].joint.rotAxisX, (double)RobotJoints[5].joint.rotAxisY, (double)RobotJoints[5].joint.rotAxisZ),
            //        camRotAxisZ),
            //    new Point3D((double)RobotJoints[5].joint.rotPointX, (double)RobotJoints[5].joint.rotPointY, (double)RobotJoints[5].joint.rotPointZ));
            //F7.Children.Add(R);
            //R = new RotateTransform3D(
            //    new AxisAngleRotation3D(
            //        new Vector3D(1, 0, 0),
            //        camRotAxisX),
            //    new Point3D((double)0, (double)0, (double)RobotJoints[5].joint.rotPointZ + 180));
            //F7.Children.Add(R);
            //R = new RotateTransform3D(
            //    new AxisAngleRotation3D(
            //        new Vector3D(0, 1, 0), 
            //        camRotAxisY),
            //    new Point3D((double)130, (double)0, (double)RobotJoints[5].joint.rotPointZ + 180));
            //F7.Children.Add(R);
            //F7.Children.Add(T);
            //F7.Children.Add(F6);


            /// Test ///
            F7 = new Transform3DGroup();
            T = new TranslateTransform3D(camOffsetX, camOffsetY, camOffsetZ);//相机偏置
            R = new RotateTransform3D(
                new AxisAngleRotation3D(
                    new Vector3D(0, 0, 1), camRotAxisZ),//相机绕自身ZZ轴旋转 旋转点J6轴线上
                new Point3D((double)RobotJoints[5].joint.rotPointX, (double)RobotJoints[5].joint.rotPointY, (double)RobotJoints[5].joint.rotPointZ + cameraBoundingBox.BoundingBox.SizeZ / 2 + FLANGE_HEIGHT));
            F7.Children.Add(R);
            R = new RotateTransform3D(
                new AxisAngleRotation3D(
                    new Vector3D(1, 0, 0), camRotAxisX),
                new Point3D((double)RobotJoints[5].joint.rotPointX, (double)RobotJoints[5].joint.rotPointY, (double)RobotJoints[5].joint.rotPointZ + cameraBoundingBox.BoundingBox.SizeZ / 2 + FLANGE_HEIGHT));
            F7.Children.Add(R);
            R = new RotateTransform3D(
                new AxisAngleRotation3D(
                    new Vector3D(0, 1, 0), camRotAxisY),
                new Point3D((double)RobotJoints[5].joint.rotPointX, (double)RobotJoints[5].joint.rotPointY, (double)RobotJoints[5].joint.rotPointZ + cameraBoundingBox.BoundingBox.SizeZ / 2 + FLANGE_HEIGHT));
            F7.Children.Add(R);
            F7.Children.Add(T);
            F7.Children.Add(F6);//F7=F6*T*RY*RX*RZ


            /// 结束测试End of Test ///


            // 应用变换Apply Transformations
            foreach (var node in RobotJoints[0].model.Traverse()) // Joint 1
            {
                if (node is MeshNode m)
                {
                    m.ModelMatrix = F1.ToMatrix();//设置场景节点LINK1的模型矩阵
                }
            }
            foreach (var node in RobotJoints[1].model.Traverse()) // Joint 2
            {
                if (node is MeshNode m)
                {
                    m.ModelMatrix = F2.ToMatrix();//设置场景节点LINK2的模型矩阵
                }
            }
            foreach (var node in RobotJoints[2].model.Traverse()) // Joint 3
            {
                if (node is MeshNode m)
                {
                    m.ModelMatrix = F3.ToMatrix();//设置场景节点LINK3的模型矩阵
                }
            }
            foreach (var node in RobotJoints[3].model.Traverse()) // Joint 4
            {
                if (node is MeshNode m)
                {
                    m.ModelMatrix = F4.ToMatrix();//设置场景节点LINK4的模型矩阵
                }
            }
            foreach (var node in RobotJoints[4].model.Traverse()) // Joint 5
            {
                if (node is MeshNode m)
                {
                    m.ModelMatrix = F5.ToMatrix();//设置场景节点LINK5的模型矩阵
                }
            }
            foreach (var node in RobotJoints[5].model.Traverse()) // Joint 6
            {
                if (node is MeshNode m)
                {
                    m.ModelMatrix = F6.ToMatrix();//设置场景节点LINK6的模型矩阵
                }
            }
            foreach (var node in RobotJoints[7].model.Traverse()) //相机挂载 Camera Mount
            {
                if (node is MeshNode m)
                {
                    m.ModelMatrix = F6.ToMatrix();//设置场景节点相机挂载的模型矩阵
                }
            }
            foreach (var node in RobotJoints[8].model.Traverse()) //相机 Camera
            {
                if (node is MeshNode m)
                {                    
                    m.ModelMatrix = F7.ToMatrix();//设置场景节点相机的模型矩阵
                }
            }


            // 法兰点Flange Point
            double flangePointOffsetX = RobotJoints[5].joint.rotPointX;
            double flangePointOffsetY = 0;
            double flangePointOffsetZ = RobotJoints[5].joint.rotPointZ;
            F8 = new Transform3DGroup(); // 法兰点的变换矩阵Transform Group for Flange Point
            T = new TranslateTransform3D(flangePointOffsetX, flangePointOffsetY, flangePointOffsetZ);//法兰原点偏置位姿
            F8.Children.Add(T);
            F8.Children.Add(F7);//F8=F7*T
            robotFlangePoint.ModelMatrix = F8.ToMatrix();//设置场景中机器人法兰点的模型矩阵


            //相机边界框 Camera Bounding Box
            double boxOffsetX = 0;
            double boxOffsetY = 0;
            double boxOffsetZ = RobotJoints[5].joint.rotPointZ + (cameraBoundingBox.BoundingBox.SizeZ / 2);
            if (CameraMount == ToolPlacement.BOTTOM)
            {
                boxOffsetX = cameraBoundingBox.BoundingBox.SizeX / 2;
            }
            else if (CameraMount == ToolPlacement.FRONT)
            {
                boxOffsetX = cameraBoundingBox.BoundingBox.SizeX - 20;
            }
            else { }
            F9 = new Transform3DGroup(); //相机边框变换组
            T = new TranslateTransform3D(boxOffsetX, boxOffsetY, boxOffsetZ + FLANGE_HEIGHT);
            F9.Children.Add(T);
            F9.Children.Add(F7);
            cameraBoundingBox.SceneNode.ModelMatrix = F9.ToMatrix();//设置相机边界框的模型矩阵


            if (IsPathTracingEnabled)//启用路径跟踪
            {
                var toolPosition = new Vector3(
                    (float)slider_X.Value,
                    (float)slider_Y.Value,
                    (float)slider_Z.Value + RobotFlangeOffset.Z
                );//工具位置：根据滑动条值确定
                Tracer_AddPoint(toolPosition);//工具tcp点添加到路径跟踪器
            }
        }

参考：

https://github.com/helix-toolkit/helix-toolkit

https://www.roboticslibrary.org/

https://en.wikipedia.org/wiki/B%C3%A9zier_curve#Cubic_B%C3%A9zier_curves 贝塞尔曲线插值

https://www.cnblogs.com/hnfxs/p/3148743.html贝塞尔曲线插值

The End