修改逆解_Kinematics.GetJointPose的输入参数

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

@@ -726,16 +726,18 @@ namespace TrajectoryPlanningTest
             flangePoseInBase.SetTopRightCorner(flangePoseInBase.GetTopRightCorner(3, 1) * 1e-3);
 
             RobotWayPointData wayPtGlobal = new RobotWayPointData(new CartesianPos(), new JointPos(), new double[7]);
-            wayPtGlobal.JointPos.JointAngle = _qZero;
-            _Kinematics.GetJointPose(flangePoseInBase.GetData(), 0, _qZero, ref wayPtGlobal.JointPos.JointAngle);
+            RobotWayPointData wayPtLocal = new RobotWayPointData(new CartesianPos(), new JointPos(), new double[7]);
+            wayPtLocal.JointPos.JointAngle = _qZero;
+            wayPtGlobal.JointPos.JointAngle = _currentWayPt.JointPos.JointAngle;
+            _Kinematics.GetJointPose(flangePoseInBase.GetData(), 0, wayPtGlobal.JointPos.JointAngle, ref wayPtLocal.JointPos.JointAngle);
 
-            if (wayPtGlobal.JointPos.JointAngle.All(angle => !double.IsNaN(angle)))
+            if (wayPtLocal.JointPos.JointAngle.All(angle => !double.IsNaN(angle)))
             {
-                List<RobotWayPointData> wayPtGlobalList = new List<RobotWayPointData>();
-                wayPtGlobalList.Add(wayPtGlobal);
+                List<RobotWayPointData> wayPtLocalList = new List<RobotWayPointData>();
+                wayPtLocalList.Add(wayPtLocal);
                 // 走到规划路径的第一个点
                 _isOfflineRunning = true;
-                _robotModule.RunOfflineWayPoints(EnumRobotMotionType.MoveJ, wayPtGlobalList, _speed);
+                _robotModule.RunOfflineWayPoints(EnumRobotMotionType.MoveJ, wayPtLocalList, _speed);
                 // 等待信号被设置
                 await Task.Run(() => _waitEvent.WaitOne());
                 _waitEvent.Reset();