Merge remote-tracking branch 'upstream/master'

# Conflicts:
#	SourceCode/OnGoingTasks/TrajectoryPlanning/Demo/IScanTrajectoryPlanning.cs
#	SourceCode/OnGoingTasks/TrajectoryPlanning/Demo/MainWindow.xaml.cs
#	SourceCode/OnGoingTasks/TrajectoryPlanning/Demo/ScanTrajectoryPlanning.cs

SourceCode/OnGoingTasks/TrajectoryPlanning/Demo/IScanTrajectoryPlanning.cs

@@ -0,0 +1,33 @@
+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,6 +238,13 @@ 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 用户界面响应
@@ -751,6 +758,7 @@ 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);
 
@@ -779,6 +787,19 @@ 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();
@@ -1006,7 +1027,13 @@ 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);
@@ -1014,11 +1041,89 @@ 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>
@@ -1058,7 +1163,6 @@ namespace TrajectoryPlanningTest
                 _robotModule.DisConnect();
                 _robotModule.Dispose();
                 _robotModule.NotifyRobotStateUpdate -= OnRobotStateUpdate;
-                _robotModule.NotifyIsRealTimeControlUpdate -= OnIsRealTimeControlUpdate;
                 _robotModule.NotifyCommandSuccessStateUpdate -= OnCommandSuccessState;
                 _robotModule.NotifyRealTimeControlStepUpdate -= OnRealTimeControlStepUpdate;
                 _robotModule.NotifyMotionFinish -= OnUpdateMotionExecutionStatus;
@@ -1097,7 +1201,6 @@ namespace TrajectoryPlanningTest
                 _robotModule.NotifyRobotStateUpdate += OnRobotStateUpdate;
                 _robotModule.NotifyCommandSuccessStateUpdate += OnCommandSuccessState;
                 _robotModule.NotifyRealTimeControlStepUpdate += OnRealTimeControlStepUpdate;
-                _robotModule.NotifyIsRealTimeControlUpdate += OnIsRealTimeControlUpdate;
                 _robotModule.NotifyMotionFinish += OnUpdateMotionExecutionStatus;
                 _robotModule.NotifyLogWrite += OnLogWrite;
                 _robotModule.NotifyError += OnErrorOccur;
@@ -1433,7 +1536,6 @@ namespace TrajectoryPlanningTest
             //file.Close();
 
             //运动已结束
-            _isOfflineRunning = false;
             _enableSeg = true;
         }
 
@@ -1489,7 +1591,10 @@ namespace TrajectoryPlanningTest
             _waitEvent.Reset();
             // 等待1毫秒
             await Task.Delay(1);
+<<<<<<< HEAD
             // 离线运动走完了
+=======
+>>>>>>> upstream/master
         }
 
         #endregion 用户界面响应
@@ -1656,6 +1761,79 @@ 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
@@ -1684,6 +1862,27 @@ 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)
             {
@@ -2101,6 +2300,50 @@ 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>
@@ -2132,6 +2375,7 @@ namespace TrajectoryPlanningTest
             //const int delta = 5;
 
             //规划刹车轨迹
+<<<<<<< HEAD
             //IntPtr brakeTraj = IntPtr.Zero;
             //int brakeTrajLen = 0;
             //int brakeTrajDim = 0;
@@ -2151,12 +2395,32 @@ namespace TrajectoryPlanningTest
 
 
             _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
 
             // 用brake轨迹替换原有轨迹t + delta之后的数据
             if (_updateCount < brakeT + delta)
@@ -2165,10 +2429,17 @@ 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);
+                // 轨迹改变后的运行时间
+                _trajLen = _wayPtsList.Count;
+                //Console.WriteLine("__new trajLen:"+ _trajLen);
+>>>>>>> upstream/master
             }
             else
             {

SourceCode/OnGoingTasks/TrajectoryPlanning/Demo/ScanTrajectoryPlanning.cs

@@ -0,0 +1,213 @@
+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;
+            }
+        }
+    }
+}

SourceCode/RUSTasks/TaskRobotModule/RCIWrapper/Source/RCIDefInfo.h

@@ -1,78 +0,0 @@
-
-#ifndef __RUSDEFINFO_H__
-#define __RUSDEFINFO_H__
-
-#include <cstdint>
-
-// 珞石机器人型号
-enum class RokaXmateType {
-    XMATE3_PRO,  // xmate 7轴 3kg
-    XMATE7_PRO,  // xmate 7轴 7kg
-    XMATE3,      // xmate 6轴 3kg
-    XMATE7       // xmate 6轴 7kg
-};
-
-// 珞石SDK返回的运动控制状态
-enum class RokaRobotRunningState
-{
-    UNCONNECTED,
-    CONNECTED,
-    MOVING,
-    DRAGING,
-    ERROR_STATE
-};
-
-//// 错误码
-//enum ROBOT_EXECUTION_STATUS
-//{
-//    RUS_SUCCESSED = 0,
-//    RUS_FAILED,
-//    RUS_FAILED_SET_POWER,
-//    RUS_FAILED_GET_JOINT_ANGLE,
-//    RUS_FAILED_MOVE_JOINT,
-//    RUS_FAILED_GET_ROBOT_STATE,
-//    RUS_FAILED_SET_INFO,
-//    RCI_LOAD_FILE_ERROR,
-//    RCI_INVALID_INPUT_ARGS,
-//    RUS_FAILED_UNKNOWN
-//};
-
-/**
- * 坐标系枚举
- */
-enum class RokaeSegmentFrame { kJoint1, kJoint2, kJoint3, kJoint4, kJoint5, kJoint6, kJoint7, kFlange, kEndEffector, kStiffness };
-
-enum class RokaeControllerMode : unsigned char { kJointPosition, kCartesianPosition, kJointImpedance, kCartesianImpedance, kTorque, kOther };
-
-enum class RokaeMotionGeneratorMode : unsigned char { kIdle, kJointPosition, kCartesianPosition };
-
-enum class EnumRobotMotionType : unsigned char { MoveJ, MoveL, MoveC, Drag, CallBackJointPosition, CallBackCartPosition, CallBackCartDrag, CallBackCartScan };
-
-//enum DragType { TRANSLATION_ONLY, ROTATION_ONLY, FREELY };
-//enum DragSpace { JOINT_DRAG, CARTESIAN_DRAG };
-enum class DragType : uint8_t { TRANSLATION_ONLY, ROTATION_ONLY, FREELY };
-enum class DragSpace : uint8_t { JOINT_DRAG, CARTESIAN_DRAG };
-
-// 路点(wayPoint_S)中包含了末端笛卡尔位姿
-// 机器人当前路点包含末端位姿、速度和关节角
-struct CartesianPos
-{
-    double position[16];    // 机器人位姿
-    double positionVector[6];    // 机器人末端速度
-};
-
-struct JointPos
-{
-    double jointAngle[7];   // 机械臂关节角信息
-    double psi;  //臂角
-    bool psiValid;
-};
-
-typedef struct _WayPoint
-{
-    CartesianPos cartPos; // 机械臂的位置信息 X,Y,Z
-    JointPos jointPos;    // 关节角度等信息
-    double dq[7];         // 关节速度
-}WayPoint;
-
-#endif

SourceCode/RUSTasks/TaskTrajectoryPlanning/TrajectoryPlanning.sln

@@ -5,22 +5,24 @@ VisualStudioVersion = 17.5.33502.453
 MinimumVisualStudioVersion = 10.0.40219.1
 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "TrajectoryPlanning", "TrajectoryPlanning\TrajectoryPlanning.vcxproj", "{5E5F28D6-274E-4A66-8782-6FD27258503D}"
 EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "TrajPlanModuleLib", "TrajPlanModuleLib\TrajPlanModuleLib.csproj", "{6F42C698-6185-4BF8-9CCB-FC2ED61470FA}"
+	ProjectSection(ProjectDependencies) = postProject
+		{5E5F28D6-274E-4A66-8782-6FD27258503D} = {5E5F28D6-274E-4A66-8782-6FD27258503D}
+	EndProjectSection
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|x64 = Debug|x64
-		Debug|x86 = Debug|x86
 		Release|x64 = Release|x64
-		Release|x86 = Release|x86
 	EndGlobalSection
 	GlobalSection(ProjectConfigurationPlatforms) = postSolution
 		{5E5F28D6-274E-4A66-8782-6FD27258503D}.Debug|x64.ActiveCfg = Debug|x64
 		{5E5F28D6-274E-4A66-8782-6FD27258503D}.Debug|x64.Build.0 = Debug|x64
-		{5E5F28D6-274E-4A66-8782-6FD27258503D}.Debug|x86.ActiveCfg = Debug|Win32
-		{5E5F28D6-274E-4A66-8782-6FD27258503D}.Debug|x86.Build.0 = Debug|Win32
 		{5E5F28D6-274E-4A66-8782-6FD27258503D}.Release|x64.ActiveCfg = Release|x64
 		{5E5F28D6-274E-4A66-8782-6FD27258503D}.Release|x64.Build.0 = Release|x64
-		{5E5F28D6-274E-4A66-8782-6FD27258503D}.Release|x86.ActiveCfg = Release|Win32
-		{5E5F28D6-274E-4A66-8782-6FD27258503D}.Release|x86.Build.0 = Release|Win32
+		{6F42C698-6185-4BF8-9CCB-FC2ED61470FA}.Debug|x64.ActiveCfg = Debug|x64
+		{6F42C698-6185-4BF8-9CCB-FC2ED61470FA}.Release|x64.ActiveCfg = Release|x64
+		{6F42C698-6185-4BF8-9CCB-FC2ED61470FA}.Release|x64.Build.0 = Release|x64
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE