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

仿真器操作演示

程序框架

源码笔记：

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

#region KINEMATICS  位置正逆解/// <summary>///正运动学机器人库DLL函数调用/// </summary>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<double>());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;}/// <summary>/// 逆运动学机器人库DLL函数调用/// </summary>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<double>());//输出//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 AnglesRobotJoints[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 TransformationsTransform3DGroup 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();//单位矩阵4X4R = 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=RF2 = 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*RF3 = 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*RF5 = 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*RF6 = 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 Transformationsforeach (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 Pointdouble flangePointOffsetX = RobotJoints[5].joint.rotPointX;double flangePointOffsetY = 0;double flangePointOffsetZ = RobotJoints[5].joint.rotPointZ;F8 = new Transform3DGroup(); // 法兰点的变换矩阵Transform Group for Flange PointT = new TranslateTransform3D(flangePointOffsetX, flangePointOffsetY, flangePointOffsetZ);//法兰原点偏置位姿F8.Children.Add(T);F8.Children.Add(F7);//F8=F7*TrobotFlangePoint.ModelMatrix = F8.ToMatrix();//设置场景中机器人法兰点的模型矩阵//相机边界框 Camera Bounding Boxdouble 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