修改代码，删除多余文件。

SourceCode/OnGoingTasks/TrajectoryPlanning/Demo/IScanTrajectoryPlanning.cs

@@ -1,33 +0,0 @@
-using KinematicsLib;
-using RobotModule;
-using RUSCommon;
-using System;
-using System.Collections.Generic;
-using TrajectoryPlanningTest;
-
-namespace RoboticUsScanSystem.ScanTrajectoryPlanning
-{
-    public interface IScanTrajectoryPlanning
-    {
-        void TrajectoryPlanning(float[] poseList, int pt3DListLen, ref float[] trajFloats, ref int trajLen, ref float sLength, ref int _trajDim,out IntPtr _trajPlanHandle)
-        ;
-
-        void ConvertCartInCamToBase(Matrix flage2Tool, float[] trajFloats, Matrix flage2Base, int trajLen, int trajDim, List<RobotWayPointData> wayPtsList, ref List<RobotWayPointData> firstWayPt, ref RobotWayPointData currentWayPt, ref IKinematics Kinematics, ConversionType conversionType);
-
-        /// <summary>
-        /// 加载手眼标定偏置
-        /// </summary>
-        /// <param name="cam2Target"></param>
-        void LoadOffset(string filePath, ref Matrix cam2Target, ConversionType conversionType);
-
-        /// <summary>
-        /// 转换相机-基座的位姿
-        /// </summary>
-        /// <param name="trajInCamIn"></param>
-        /// <param name="cam2TargetIn"></param>
-        /// <param name="flange2BaseIn"></param>
-        /// <param name="trajInBaseOut"></param>
-        /// <param name="conversionType"></param>
-        void CalculateTrajInBase(Matrix trajInCamIn, Matrix cam2TargetIn, Matrix flange2BaseIn, out Matrix trajInBaseOut, ConversionType conversionType);
-    }
-}

SourceCode/OnGoingTasks/TrajectoryPlanning/Demo/MainWindow.xaml.cs

@@ -238,13 +238,6 @@ namespace TrajectoryPlanningTest
         private volatile bool _isOfflineRunning = false;
 
 
-        private bool _enableUpdate = true;
-
-        // 创建静态的手动重置事件对象
-        private static ManualResetEvent _waitUpdateEvent = new ManualResetEvent(false);
-
-        private Matrix _flage2BaseInThisTraj = Matrix.Identity(4);
-
         #endregion private
 
         #region 用户界面响应
@@ -758,7 +751,6 @@ namespace TrajectoryPlanningTest
         /// <param name="e"></param>
         private async void OnCameraLocalPoseClick(object sender, RoutedEventArgs e)
         {
-<<<<<<< HEAD
             Matrix flangePoseInBase = new Matrix(_poseFlangeLocal2Base);
             flangePoseInBase.SetTopRightCorner(flangePoseInBase.GetTopRightCorner(3, 1) * 1e-3);
 
@@ -787,19 +779,6 @@ namespace TrajectoryPlanningTest
             {
                 MessageBox.Show("无法计算对应局部位姿，请重新规划路径进行计算");
             }
-=======
-            // 走到局部拍照位姿
-            List<RobotWayPointData> camWayPt = new List<RobotWayPointData>();
-            RobotWayPointData wayPt = new RobotWayPointData();
-            wayPt.JointPos.JointAngle = _qCamLocal;
-            camWayPt.Add(wayPt);
-            _robotModule.RunOfflineWayPoints(EnumRobotMotionType.MoveJ, camWayPt, _speed);
-            // 等待信号被设置
-            await Task.Run(() => _waitEvent.WaitOne());
-            _waitEvent.Reset();
-            // 等待1毫秒
-            await Task.Delay(1);
->>>>>>> upstream/master
 
             //清除原始图像上面的路径点
             MyImageCanvas.Clear();
@@ -1027,13 +1006,7 @@ namespace TrajectoryPlanningTest
                 return;
             }
 
-            // 需要的是法兰坐标系在基坐标系下的位姿
-            Matrix tool2BaseInThisTraj = new Matrix(_currentCartMat);
-            _flage2BaseInThisTraj = tool2BaseInThisTraj * _flage2Tool;
-            _flage2BaseInThisTraj.SetTopRightCorner(_flage2BaseInThisTraj.GetTopRightCorner(3, 1) * 1e3);
-
             _wayPtsList.Clear();
-<<<<<<< HEAD
 
             // 获取法兰坐标系在基坐标系下的位姿
             Matrix tool2Base = new Matrix(4, 4, _currentWayPt.CartPos.Position);
@@ -1041,89 +1014,11 @@ namespace TrajectoryPlanningTest
             _flage2BaseInThisTraj.SetTopRightCorner(_flage2BaseInThisTraj.GetTopRightCorner(3, 1) * 1e3);
 
             _tranjPlanScan.ConvertCartInCamToBase(_flage2Tool, _trajectory, _flage2BaseInThisTraj, ref _wayPtsList, ref _firstWayPt, ref _currentWayPt, ref _Kinematics, _conType);
-=======
-            ScanTrajectoryPlanning _tranjPlan = new ScanTrajectoryPlanning();
-            _tranjPlan.ConvertCartInCamToBase(_flage2Tool, _trajFloats, _flage2BaseInThisTraj, _trajLen, _trajDim, _wayPtsList, ref _firstWayPt, ref _currentWayPt, ref _Kinematics, conType);
->>>>>>> upstream/master
 
             MessageBox.Show("完成！");
         }
 
         /// <summary>
-<<<<<<< HEAD
-=======
-        /// 转换相机-基座的位姿
-        /// </summary>
-        /// <param name="trajFloats"></param>
-        /// <param name="trajLen"></param>
-        /// <param name="trajDim"></param>
-        /// <param name="wayPtsList"></param>
-        /// <param name="firstWayPt"></param>
-        private void ConvertCartInCamToBase(float[] trajFloats, int trajLen, Matrix flage2Base, int trajDim, List<RobotWayPointData> wayPtsList, out List<RobotWayPointData> firstWayPt, ConversionType conversionType)
-        {
-            //// 需要的是法兰坐标系在基坐标系下的位姿
-            //Matrix tool2Base = new Matrix(4, 4, _currentWayPt.CartPos.Position);
-            //Matrix flage2Base = tool2Base * _flage2Tool;
-            //flage2Base.SetTopRightCorner(flage2Base.GetTopRightCorner(3, 1) * 1e3);
-
-            firstWayPt = new List<RobotWayPointData>();
-            Matrix cam2Target = new Matrix(4);
-
-            //相机坐标系下的点云转换到机器人基坐标系下
-            ScanTrajectoryPlanning _trajPlanScan = new ScanTrajectoryPlanning();
-
-            switch (conversionType)
-            {
-                case ConversionType.EyeInHand:
-                    _trajPlanScan.LoadOffset(@"..\Data\cam2Flange.xml", ref cam2Target, ConversionType.EyeInHand);
-                    break;
-
-                case ConversionType.EyeToHand:
-                    _trajPlanScan.LoadOffset(@"..\Data\cam2Base.xml", ref cam2Target, ConversionType.EyeToHand);
-                    break;
-            }
-
-            RobotWayPointData wayPt = new RobotWayPointData(new CartesianPos(), new JointPos(), new double[7]);
-            CartesianPos cartPos = new CartesianPos();
-            for (int i = 0; i < trajLen; i++)
-            {
-                Matrix toolTrajInCam = Matrix.Identity(4);
-                //四元数转旋转矩阵
-                //trajFloats 前三位是笛卡尔空间位置 后四位是四元数姿态
-                toolTrajInCam[0, 3] = Convert.ToDouble(trajFloats[i * trajDim + 0]);
-                toolTrajInCam[1, 3] = Convert.ToDouble(trajFloats[i * trajDim + 1]);
-                toolTrajInCam[2, 3] = Convert.ToDouble(trajFloats[i * trajDim + 2]);
-
-                Quaternion toolInCamQuat = new Quaternion(trajFloats[i * trajDim + 3], trajFloats[i * trajDim + 4], trajFloats[i * trajDim + 5], trajFloats[i * trajDim + 6]);
-                Matrix toolInCamPose = Quaternion.ConvertToRotationMatrix(toolInCamQuat);
-
-                // 以person坐标系为探头坐标系的标准位姿
-                // 如果探头要朝人体的左则再绕z轴转90度
-                // 如果探头要朝人体的右则再绕z轴转-90度
-                // 计算旋转后person坐标系在Cam坐标系中的姿态
-                //Matrix probePose = Matrix.RotateAroundAxisZ(0.0 * Math.PI / 180.0);
-                //Matrix tool2Cam = AlignUSProbeWithBody(probePose, person2Cam.GetTopLeftCorner(3, 3), toolInCamPose);
-
-                toolTrajInCam.SetTopLeftCorner(toolInCamPose);
-                ScanTrajectoryPlanning _tranjPlanScan = new ScanTrajectoryPlanning();
-                _tranjPlanScan.CalculateTrajInBase(toolTrajInCam, cam2Target, flage2Base, out Matrix toolTrajInBase, conversionType);
-
-                toolTrajInBase.SetTopRightCorner(toolTrajInBase.GetTopRightCorner(3, 1) * 1e-3);
-
-                cartPos.Position = toolTrajInBase.GetData();
-                wayPt.CartPos = cartPos;
-                Matrix flageTrajInBase = toolTrajInBase * _flage2Tool;
-                _Kinematics.GetJointPose(flageTrajInBase.GetData(), 0, _currentWayPt.JointPos.JointAngle, ref wayPt.JointPos.JointAngle);
-                if (i == 0)
-                {
-                    firstWayPt.Add(wayPt);
-                }
-                wayPtsList.Add(wayPt);
-            }
-        }
-
-        /// <summary>
->>>>>>> upstream/master
         /// 选择机器人IP
         /// </summary>
         /// <param name="sender"></param>
@@ -1163,6 +1058,7 @@ namespace TrajectoryPlanningTest
                 _robotModule.DisConnect();
                 _robotModule.Dispose();
                 _robotModule.NotifyRobotStateUpdate -= OnRobotStateUpdate;
+                _robotModule.NotifyIsRealTimeControlUpdate -= OnIsRealTimeControlUpdate;
                 _robotModule.NotifyCommandSuccessStateUpdate -= OnCommandSuccessState;
                 _robotModule.NotifyRealTimeControlStepUpdate -= OnRealTimeControlStepUpdate;
                 _robotModule.NotifyMotionFinish -= OnUpdateMotionExecutionStatus;
@@ -1201,6 +1097,7 @@ namespace TrajectoryPlanningTest
                 _robotModule.NotifyRobotStateUpdate += OnRobotStateUpdate;
                 _robotModule.NotifyCommandSuccessStateUpdate += OnCommandSuccessState;
                 _robotModule.NotifyRealTimeControlStepUpdate += OnRealTimeControlStepUpdate;
+                _robotModule.NotifyIsRealTimeControlUpdate += OnIsRealTimeControlUpdate;
                 _robotModule.NotifyMotionFinish += OnUpdateMotionExecutionStatus;
                 _robotModule.NotifyLogWrite += OnLogWrite;
                 _robotModule.NotifyError += OnErrorOccur;
@@ -1536,6 +1433,7 @@ namespace TrajectoryPlanningTest
             //file.Close();
 
             //运动已结束
+            _isOfflineRunning = false;
             _enableSeg = true;
         }
 
@@ -1591,10 +1489,7 @@ namespace TrajectoryPlanningTest
             _waitEvent.Reset();
             // 等待1毫秒
             await Task.Delay(1);
-<<<<<<< HEAD
             // 离线运动走完了
-=======
->>>>>>> upstream/master
         }
 
         #endregion 用户界面响应
@@ -1761,79 +1656,6 @@ namespace TrajectoryPlanningTest
 
                         if (pclData.ByteCount != 0)
                         {
-<<<<<<< HEAD
-=======
-                            Point2D[][] path2DPoints = new Point2D[1][];
-                            int[] path2DPointsLen = new int[1];
-                            if (_enableSeg)
-                            {
-                                // 加载分割
-                                IHumanOrganSegment _segResult;
-                                string netDir = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "Networks");
-                                _segResult = new HumanOrganSegment(1, netDir);
-                                DetectedObject _inferResult = new DetectedObject();
-
-                                RawImage newImage;
-
-                                int rectX = 0;
-                                int rectY = 0;
-                                int rectWidth = imgProcess.Width;
-                                int rectHeight = imgProcess.Height;
-                                Rect cropRect = new Rect(rectX, rectY, rectWidth, rectHeight);
-                                newImage = imgProcess.CropRect(cropRect);
-
-                                _inferResult = _segResult.SegmentOneHumanOrganImage(newImage);
-
-                                // 由2D分割轮廓生成一小段2D线段
-
-                                if (_inferResult.Contour != null)
-                                {
-                                    int len = _inferResult.Contour.Contours[0].Points.Length;
-
-                                    Point2D[] contoursTransform = new Point2D[len];
-                                    for (int cindx = 0; cindx < len; cindx++)
-                                    {
-                                        contoursTransform[cindx].X = _inferResult.Contour.Contours[0].Points[cindx].X + cropRect.Left;
-                                        contoursTransform[cindx].Y = _inferResult.Contour.Contours[0].Points[cindx].Y + cropRect.Top;
-                                    }
-
-                                    lock (_processedDataLocker)
-                                    {
-                                        MyImageCanvas.UpdateContours(contoursTransform);
-                                    }
-
-                                    Generate2DRawPathPoints(cropRect.Width, cropRect.Height, contoursTransform, out path2DPoints, out path2DPointsLen);
-                                    _enableSeg = false;
-                                    lock (_processedDataLocker)
-                                    {
-                                        //MyImageCanvas.UpdateContours(contours);
-                                        //MyImageCanvas.UpdateContours(path2DPoints[0]);
-                                    }
-                                }
-                                else
-                                {
-                                    MessageBox.Show("Failed in seg!");
-                                }
-                            }
-                            else
-                            {
-                                List<Point2D> pt2DContoursList = new List<Point2D>();
-                                Point2D prevPt = new Point2D();
-                                foreach (var p in _contourInOrigImg)
-                                {
-                                    Point2D currentPoint = new Point2D { X = p.X, Y = p.Y };
-                                    if (currentPoint.X != prevPt.X || currentPoint.Y != prevPt.Y)
-                                    {
-                                        pt2DContoursList.Add(currentPoint);
-                                        prevPt = currentPoint;
-                                    }
-                                }
-                                path2DPoints[0] = pt2DContoursList.ToArray();
-                                path2DPointsLen[0] = pt2DContoursList.Count();
-                            }
-
-                            // 将2D线段投影到3D，生成初始3D扫查路径
->>>>>>> upstream/master
                             GCHandle hObjectCloud = GCHandle.Alloc(pclData.DataBuffer, GCHandleType.Pinned);
                             IntPtr pDataCloud = hObjectCloud.AddrOfPinnedObject();
                             StructPCLDataInfo cloudGlobalStruct = new StructPCLDataInfo
@@ -1862,27 +1684,6 @@ namespace TrajectoryPlanningTest
                     }
                 }
                 _enableGetPath = false;
-<<<<<<< HEAD
-=======
-                if (_enablePlanTraj)
-                {
-                    ScanTrajectoryPlanning _trajPlanScan = new ScanTrajectoryPlanning();
-                    float sLength = 0;
-                    _trajPlanScan.TrajectoryPlanning(_ptnLocal3DList, _pt3DListLen, ref _trajFloats, ref _trajLen, ref sLength, ref _trajDim, out _trajPlanHandle);
-
-                    MessageBox.Show("3D点个数：" + _trajLen + ". 长度:" + sLength);
-
-                    _enablePlanTraj = false;
-                    _planTrajSucceed = true;
-
-                    if (_isShowTrajectory)
-                    {
-                        // 可视化点云
-                        IScanShowCloudGenerator _showTraj = new ScanShowCloudGenerator();
-                        _showTraj.GenerateShowTrajectory(_trajFloats, _trajLen, _trajDim);
-                    }
-                }
->>>>>>> upstream/master
             }
             catch (Exception excep)
             {
@@ -2300,50 +2101,6 @@ namespace TrajectoryPlanningTest
             MessageBox.Show("完成");
         }
 
-<<<<<<< HEAD
-=======
-        #endregion private
-
-        #region private funcs
-
-        /// <summary>
-        /// 实时更新轨迹路点
-        /// </summary>
-        private void RealTimeUpdateRobotWayPt()
-        {
-            _timer.Start();
-            while (true)
-            {
-                if (_timer.Duration >= 1000)
-                {
-                    //因为达到了1毫秒的时刻，所以要重新记录开始的时间点
-                    _timer.Stop();
-                    _timer.Start();
-
-                    RobotWayPointData wayPt = _wayPtsList[_updateCount];
-                    if (_enableUpdate)
-                    {
-                        _robotModule.EnableRunRealTimeControl = true;
-                        _robotModule.UpdateRealTimeWayPt(wayPt);
-                        _updateCount++;
-                    }
-                    ////记录系统时间
-                    //DateTime now = DateTime.Now;
-                    //_logList.Add(now.ToString("HH:mm:ss.fff") + " - MainWindow - RealTimeUpdateRobotWayPt : update " + _updateCount + "-th wayPt.");
-                    //Console.WriteLine("_updateCount:" + _updateCount);
-                    if (_updateCount >= _trajLen)
-                    {
-                        _robotModule.EnableRunRealTimeControl = false;
-                        break;
-                    }
-                }
-            }
-            //已经将list中所有的路点出队
-            _timer.Stop();
-            MessageBox.Show("完成");
-        }
-
->>>>>>> upstream/master
         /// <summary>
         /// 机器人中止决策，判断是终止还是中断
         /// </summary>
@@ -2375,7 +2132,6 @@ namespace TrajectoryPlanningTest
             //const int delta = 5;
 
             //规划刹车轨迹
-<<<<<<< HEAD
             //IntPtr brakeTraj = IntPtr.Zero;
             //int brakeTrajLen = 0;
             //int brakeTrajDim = 0;
@@ -2389,38 +2145,18 @@ namespace TrajectoryPlanningTest
             // 确认轨迹
             List<RobotWayPointData> brakeWayPts = new List<RobotWayPointData>();
 
-            _tranjPlanScan.ConvertCartInCamToBase(_flage2Tool, brakeTraj, _flage2BaseInThisTraj,ref brakeWayPts, ref _firstWayPt, ref _currentWayPt, ref _Kinematics, _conType);
+            _tranjPlanScan.ConvertCartInCamToBase(_flage2Tool, brakeTraj, _flage2BaseInThisTraj, ref brakeWayPts, ref _firstWayPt, ref _currentWayPt, ref _Kinematics, _conType);
 
             //_tranjPlanScan.ConvertCartInCamToBase(_flage2Tool, _trajectory, _flage2BaseInThisTraj, _wayPtsList, ref _firstWayPt, ref _currentWayPt, ref _Kinematics, _conType);
 
 
             _trajLenOld = _wayPtsList.Count;
-=======
-            IntPtr brakeTraj = IntPtr.Zero;
-            int brakeTrajLen = 0;
-            int brakeTrajDim = 0;
-
-            // ??
-            // 轨迹重新规划 需要知道原先的轨迹数据，中断时刻轨迹序号，预留的缓冲时长
-            TrajectoryBrake(trajHandle, brakeT + delta, ref brakeTraj, ref brakeTrajLen, ref brakeTrajDim);
-
-            float[] brakeTrajFloats = new float[brakeTrajLen * brakeTrajDim];
-            Marshal.Copy(brakeTraj, brakeTrajFloats, 0, brakeTrajFloats.Length);
-
-            // 确认轨迹
-            List<RobotWayPointData> brakeWayPts = new List<RobotWayPointData>();
-            ConvertCartInCamToBase(brakeTrajFloats, brakeTrajLen, _flage2BaseInThisTraj, brakeTrajDim, brakeWayPts, out _, conType);
->>>>>>> upstream/master
 
             // 原始轨迹长度
             _trajLenOld = _wayPtsList.Count;
 
             int offset = brakeT + delta;
-<<<<<<< HEAD
-            Console.WriteLine("_updateCount:"+ _updateCount + ".   brakeT + delta" + offset);
-=======
-            //Console.WriteLine("_updateCount:"+ _updateCount + ".   brakeT + delta" + offset);
->>>>>>> upstream/master
+            Console.WriteLine("_updateCount:" + _updateCount + ".   brakeT + delta" + offset);
 
             // 用brake轨迹替换原有轨迹t + delta之后的数据
             if (_updateCount < brakeT + delta)
@@ -2429,17 +2165,10 @@ namespace TrajectoryPlanningTest
                 _wayPtsList.RemoveRange(brakeT + delta, _wayPtsList.Count - (brakeT + delta));
                 // 加上brake数据
                 _wayPtsList.AddRange(brakeWayPts);
-<<<<<<< HEAD
-                Console.WriteLine("___old trajLen:"+ _trajLen);
-                // 轨迹改变后的运行时间
-                _trajLen = _wayPtsList.Count;
-                Console.WriteLine("__new trajLen:"+ _trajLen);
-=======
-                //Console.WriteLine("___old trajLen:"+ _trajLen);
+                Console.WriteLine("___old trajLen:" + _trajLen);
                 // 轨迹改变后的运行时间
                 _trajLen = _wayPtsList.Count;
-                //Console.WriteLine("__new trajLen:"+ _trajLen);
->>>>>>> upstream/master
+                Console.WriteLine("__new trajLen:" + _trajLen);
             }
             else
             {

SourceCode/OnGoingTasks/TrajectoryPlanning/Demo/ScanTrajectoryPlanning.cs

@@ -1,213 +0,0 @@
-using KinematicsLib;
-using RobotModule;
-using RUSCommon;
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using System.Runtime.InteropServices;
-using System.Windows;
-using System.Xml;
-using TrajectoryPlanningTest;
-
-namespace RoboticUsScanSystem.ScanTrajectoryPlanning
-{
-    // 轨迹规划类型
-    public enum EnumTrajPlanningType
-    {
-        // 笛卡尔空间
-        CartesianSpace,
-
-        // 关节空间
-        JointSpace
-    };
-
-    //速度曲线规划类型
-    public enum EnumVPlanningType
-    {
-        SCurveType,
-        TCurveType,
-        CosineType
-    };
-
-    public enum EnumCurveFittingType
-    {
-        LineFittingType, // 2点拟合
-        ArcFittingType, // 3个点
-        BSplineFittingType // 大于等于4个点
-    };
-
-    public class ScanTrajectoryPlanning : IScanTrajectoryPlanning
-    {
-        #region dll import
-
-        [DllImport("TrajectoryPlanning.dll", CallingConvention = CallingConvention.Cdecl)]
-        public static extern void CreateTrajectoryPlanning(out IntPtr hTrajPlan);
-
-        [DllImport("TrajectoryPlanning.dll", CallingConvention = CallingConvention.Cdecl)]
-        public static extern void ReleaseTrajectoryPlanning([In] IntPtr hTrajPlan);
-
-        [DllImport("TrajectoryPlanning.dll", CallingConvention = CallingConvention.Cdecl)]
-        public static extern unsafe bool LoadData([In] IntPtr hTrajPlan, EnumCurveFittingType curFitType, float[] poseList, int poseListLen, int dim, EnumTrajPlanningType trajPlanningType);
-
-        [DllImport("TrajectoryPlanning.dll", CallingConvention = CallingConvention.Cdecl)]
-        public static extern unsafe void SetPara([In] IntPtr hTrajPlan, EnumVPlanningType velPlanningType, float stratV, float targetV, float vmax, float amax, float jmax, float frequency);
-
-        [DllImport("TrajectoryPlanning.dll", CallingConvention = CallingConvention.Cdecl)]
-        public static extern unsafe void TrajectoryPlanningRun([In] IntPtr hTrajPlan, ref IntPtr trajectory, ref int trajLen, ref int trajDim);
-
-        [DllImport("TrajectoryPlanning.dll", CallingConvention = CallingConvention.Cdecl)]
-        public static extern unsafe void TrajectoryBrake([In] IntPtr hTrajPlan, [In] int t, ref IntPtr trajectory, ref int trajLen, ref int trajDim);
-
-        [DllImport("TrajectoryPlanning.dll", CallingConvention = CallingConvention.Cdecl)]
-        public static extern float GetTrajLength([In] IntPtr hTrajPlan);
-
-        #endregion dll import
-
-        #region private
-
-        //轨迹规划相关
-        //private static IntPtr _trajPlanHandle;
-
-        #endregion private
-
-        public void TrajectoryPlanning(float[] poseList, int pt3DListLen, ref float[] trajFloats, ref int trajLen, ref float sLength, ref int trajDim, out IntPtr _trajPlanHandle)
-        {
-            CreateTrajectoryPlanning(out _trajPlanHandle);
-
-            LoadData(_trajPlanHandle, EnumCurveFittingType.BSplineFittingType, poseList, pt3DListLen, 6, EnumTrajPlanningType.CartesianSpace);
-
-            SetPara(_trajPlanHandle, EnumVPlanningType.SCurveType, 0, 0, 20, 50, 500, 0.001f);
-
-            IntPtr trajectory = IntPtr.Zero;
-            TrajectoryPlanningRun(_trajPlanHandle, ref trajectory, ref trajLen, ref trajDim);
-            
-
-            trajFloats = new float[trajLen * trajDim];
-            Marshal.Copy(trajectory, trajFloats, 0, trajFloats.Length);
-
-            sLength = GetTrajLength(_trajPlanHandle);
-        }
-
-        public void ConvertCartInCamToBase(Matrix flage2Tool, float[] trajFloats, Matrix flage2Base, int trajLen , int trajDim, List<RobotWayPointData> wayPtsList, ref List<RobotWayPointData> firstWayPt, ref RobotWayPointData currentWayPt, ref IKinematics Kinematics, ConversionType conversionType)
-        {
-            //// 需要的是法兰坐标系在基坐标系下的位姿
-            //Matrix tool2Base = new Matrix(4, 4, currentWayPt.CartPos.Position);
-            //Matrix flage2Base = tool2Base * flage2Tool;
-            //flage2Base.SetTopRightCorner(flage2Base.GetTopRightCorner(3, 1) * 1e3);
-
-            firstWayPt = new List<RobotWayPointData>();
-            Matrix cam2Target = new Matrix(4);
-
-            //相机坐标系下的点云转换到机器人基坐标系下
-
-            switch (conversionType)
-            {
-                case ConversionType.EyeInHand:
-                    LoadOffset(@"..\Data\cam2Flange.xml", ref cam2Target, ConversionType.EyeInHand);
-                    break;
-
-                case ConversionType.EyeToHand:
-                    LoadOffset(@"..\Data\cam2Base.xml", ref cam2Target, ConversionType.EyeToHand);
-                    break;
-            }
-
-            RobotWayPointData wayPt = new RobotWayPointData(new CartesianPos(), new JointPos(), new double[7]);
-            CartesianPos cartPos = new CartesianPos();
-            for (int i = 0; i < trajLen; i++)
-            {
-                Matrix toolTrajInCam = Matrix.Identity(4);
-                //四元数转旋转矩阵
-                //trajFloats 前三位是笛卡尔空间位置 后四位是四元数姿态
-                toolTrajInCam[0, 3] = Convert.ToDouble(trajFloats[i * trajDim + 0]);
-                toolTrajInCam[1, 3] = Convert.ToDouble(trajFloats[i * trajDim + 1]);
-                toolTrajInCam[2, 3] = Convert.ToDouble(trajFloats[i * trajDim + 2]);
-
-                Quaternion toolInCamQuat = new Quaternion(trajFloats[i * trajDim + 3], trajFloats[i * trajDim + 4], trajFloats[i * trajDim + 5], trajFloats[i * trajDim + 6]);
-                Matrix toolInCamPose = Quaternion.ConvertToRotationMatrix(toolInCamQuat);
-
-                // 以person坐标系为探头坐标系的标准位姿
-                // 如果探头要朝人体的左则再绕z轴转90度
-                // 如果探头要朝人体的右则再绕z轴转-90度
-                // 计算旋转后person坐标系在Cam坐标系中的姿态
-                //Matrix probePose = Matrix.RotateAroundAxisZ(0.0 * Math.PI / 180.0);
-                //Matrix tool2Cam = AlignUSProbeWithBody(probePose, person2Cam.GetTopLeftCorner(3, 3), toolInCamPose);
-
-                toolTrajInCam.SetTopLeftCorner(toolInCamPose);
-                CalculateTrajInBase(toolTrajInCam, cam2Target, flage2Base, out Matrix toolTrajInBase, conversionType);
-                toolTrajInBase.SetTopRightCorner(toolTrajInBase.GetTopRightCorner(3, 1) * 1e-3);
-
-                cartPos.Position = toolTrajInBase.GetData();
-                wayPt.CartPos = cartPos;
-                Matrix flageTrajInBase = toolTrajInBase * flage2Tool;
-                Kinematics.GetJointPose(flageTrajInBase.GetData(), 0, currentWayPt.JointPos.JointAngle, ref wayPt.JointPos.JointAngle);
-                if (i == 0)
-                {
-                    firstWayPt.Add(wayPt);
-                }
-                wayPtsList.Add(wayPt);
-            }
-        }
-
-        /// <summary>
-        /// 加载手眼标定偏置
-        /// </summary>
-        /// <param name="cam2Flange"></param>
-        public void LoadOffset(string filePath, ref Matrix cam2Target, ConversionType conversionType)
-        {
-            XmlDocument doc = new XmlDocument();
-            doc.Load(filePath);
-
-            //得到data节点
-            XmlNode xn = null; // 默认将xn设置为null
-            switch (conversionType)
-            {
-                case ConversionType.EyeInHand:
-                    xn = doc.SelectSingleNode("opencv_storage").SelectSingleNode("cam2Flange").SelectSingleNode("data");
-                    break;
-
-                case ConversionType.EyeToHand:
-                    xn = doc.SelectSingleNode("opencv_storage").SelectSingleNode("cam2Base").SelectSingleNode("data");
-                    break;
-            }
-            if (xn != null)
-            {
-                string line = xn.InnerText;
-                //去除\r\n
-                line = line.Replace(System.Environment.NewLine, string.Empty);
-                string[] strArr = line.Split(' ').ToArray();
-
-                double[] offset = new double[16];
-                int k = 0;
-                foreach (string str in strArr)
-                {
-                    if (str != String.Empty)
-                    {
-                        offset[k] = Convert.ToDouble(str);
-                        k++;
-                    }
-                }
-
-                cam2Target.SetData(offset);
-            }
-            else
-            {
-                MessageBox.Show("手眼标定结果文件XML节点未找到或文件格式不正确！");
-            }
-        }
-
-        public void CalculateTrajInBase(Matrix trajInCamIn, Matrix cam2TargetIn, Matrix flange2BaseIn, out Matrix trajInBaseOut, ConversionType conversionType)
-        {
-            trajInBaseOut = Matrix.Identity(4);
-            switch (conversionType)
-            {
-                case ConversionType.EyeInHand:
-                    trajInBaseOut = flange2BaseIn * cam2TargetIn * trajInCamIn;
-                    break;
-
-                case ConversionType.EyeToHand:
-                    trajInBaseOut = cam2TargetIn * trajInCamIn;
-                    break;
-            }
-        }
-    }
-}