1.修复眼在手上时机器人实时回调刹车出现前后轨迹跳变的问题。

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

@@ -117,6 +117,8 @@ namespace ForceControl
         private volatile bool _planTrajSucceed = false;
         private bool _isShowTrajectory = false; // 可视化轨迹
 
+        private Matrix _flage2BaseInThisTraj = Matrix.Identity(4);
+
         //机器人相关
         private bool _isRobotConnected = false;
 
@@ -664,9 +666,15 @@ namespace ForceControl
                 return;
             }
 
+
+            // 需要的是法兰坐标系在基坐标系下的位姿
+            Matrix tool2BaseInThisTraj = new Matrix(_currentCartMat);
+            _flage2BaseInThisTraj = tool2BaseInThisTraj * _flage2Tool;
+            _flage2BaseInThisTraj.SetTopRightCorner(_flage2BaseInThisTraj.GetTopRightCorner(3, 1) * 1e3);
+
             _wayPtsList.Clear();
             _firstWayPt.Clear();
-            ConvertCartInCamToBase(_trajectory, _conType, out _wayPtsList, out _firstWayPt);
+            ConvertCartInCamToBase(_trajectory, _conType, _flage2BaseInThisTraj, out _wayPtsList, out _firstWayPt);
             _trajLen = _wayPtsList.Count;
 
             MessageBox.Show("完成！");
@@ -676,7 +684,7 @@ namespace ForceControl
             {
                 while (true)
                 {
-                    if (_updateCount >= 2000)
+                    if (_updateCount >= 3000)
                     {
                         BrakeRobot();
                         break;
@@ -1478,7 +1486,7 @@ namespace ForceControl
             _trajPlanModule.TrajectoryBrake(brakeT + delta, ref brakeTraj);
 
             // 确认轨迹
-            ConvertCartInCamToBase(brakeTraj, _conType, out List<RobotWayPointData> brakeWayPts, out _);
+            ConvertCartInCamToBase(brakeTraj, _conType, _flage2BaseInThisTraj, out List<RobotWayPointData> brakeWayPts, out _);
 
             _trajLenOld = _wayPtsList.Count;
 
@@ -2103,7 +2111,7 @@ namespace ForceControl
         /// <param name="trajFloats"></param>
         /// <param name="wayPtsList"></param>
         /// <param name="firstWayPt"></param>
-        private void ConvertCartInCamToBase(List<float[]> traj, ConversionType conversionType, out List<RobotWayPointData> wayPtsList, out List<RobotWayPointData> firstWayPt)
+        private void ConvertCartInCamToBase(List<float[]> traj, ConversionType conversionType, Matrix flage2Base, out List<RobotWayPointData> wayPtsList, out List<RobotWayPointData> firstWayPt)
         {
             wayPtsList = new List<RobotWayPointData>();
             firstWayPt = new List<RobotWayPointData>();
@@ -2112,11 +2120,6 @@ namespace ForceControl
             Matrix cam2Target = new Matrix(4);
             LoadOffset(conversionType, ref cam2Target);
 
-            // 需要的是法兰坐标系在基坐标系下的位姿
-            Matrix tool2Base = new Matrix(_currentCartMat);
-            Matrix flage2Base = tool2Base * _flage2Tool;
-            flage2Base.SetTopRightCorner(flage2Base.GetTopRightCorner(3, 1) * 1e3);
-
             RobotWayPointData wayPt = new RobotWayPointData(new CartesianPos(), new JointPos(), new double[7]);
             CartesianPos cartPos = new CartesianPos();
             for (int i = 0; i < traj.Count; i++)
@@ -2513,11 +2516,6 @@ namespace ForceControl
 
                     if (_updateCount >= _trajLen)
                     {
-                        Dispatcher.InvokeAsync(() =>
-                        {
-                            // 更新界面上显示的数据
-                            TextRealTimeControlStep.Text = string.Empty;
-                        });
                         break;
                     }
                 }

SourceCode/RUSTasks/TaskTrajectoryPlanning/TrajectoryPlanning/ITrajectoryPlanning.cpp

@@ -51,7 +51,8 @@ bool __stdcall TrajectoryPlanningRun(void* hTrajectoryPlan, float*& trajectory,
     const auto trajectoryPlanning = static_cast<TrajPlanning::TrajectoryPlanning*>(hTrajectoryPlan);
     std::vector<float> result;
     bool status = trajectoryPlanning->TrajectoryPlanningRun(result, trajLen, trajDim);
-    trajectory = result.data();
+    trajectory = new float[result.size()];
+    std::memcpy(trajectory, result.data(), result.size() * sizeof(float));
     return status;
 }
 
@@ -66,6 +67,7 @@ bool __stdcall TrajectoryBrake(void* hTrajectoryPlan, int t, float*& trajectory,
     const auto trajectoryPlanning = static_cast<TrajPlanning::TrajectoryPlanning*>(hTrajectoryPlan);
     std::vector<float> result;
     bool status = trajectoryPlanning->TrajectoryBrake(t, result, trajLen, trajDim);
-    trajectory = result.data();
+    trajectory = new float[result.size()];
+    std::memcpy(trajectory, result.data(), result.size() * sizeof(float));
     return status;
 }

SourceCode/RUSTasks/TaskTrajectoryPlanning/TrajectoryPlanning/VelocityPlaningCore/VelocityPlanning.cpp

@@ -6,14 +6,12 @@ using namespace VelPlaning;
 
 void VelocityPlaningBasic::SetPara(VPlanningType vPlanningType, float stratV, float targetV, float vmax, float amax, float jmax, float period)
 {
-
     _stratV = stratV;
     _targetV = targetV;
     _vmax = vmax;
     _amax = amax;
     _jmax = jmax;
     _period = period;
-
 }
 
 bool VelocityPlaningBasic::VelocityPlanningRun(float startObj, float targetObj, std::vector<float>& siList)
@@ -71,4 +69,4 @@ bool VelocityPlaningBasic::Brake(int t, std::vector<float>& siList)
         siList.push_back(si);
     }
     return true;
-}
+}