feat(fanuc): 添加直角坐标点动功能与相关接口

* 新增 `MovePose` 方法,支持以直角坐标执行点到点移动。
* 引入 `LegacyCartesianPoseRequest` 类,处理直角位姿请求体的解析与验证。
* 更新 `LegacyHttpApiController`,实现 `/move_pose/` 路由以支持新功能。
* 增强状态快照元数据,提供机器人初始化状态与已上传轨迹信息。
* 更新前端状态页面,增加直角坐标点动控制面板与步长设置选项。
* 相关文档与测试用例同步更新,确保新功能的完整性与稳定性。
This commit is contained in:
2026-05-14 17:46:42 +08:00
parent d120ef2a39
commit 2cd42f04e5
22 changed files with 2062 additions and 104 deletions

View File

@@ -555,6 +555,30 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.5));
}
/// <summary>
/// 验证 MovePose 低速倍率仍保持固定伺服周期,并通过拉长时长降低直角运动速度。
/// </summary>
[Fact]
public void MovePoseTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
{
var servoPeriod = TimeSpan.FromMilliseconds(8);
var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 };
var targetPose = new[] { 140.0, 260.0, 330.0, 8.0, 10.0, 12.0 };
var fullSpeed = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 1.0);
var speed07 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.7);
var speed05 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.5);
Assert.True(speed07.DenseCartesianTrajectory!.Count > fullSpeed.DenseCartesianTrajectory!.Count);
Assert.True(speed05.DenseCartesianTrajectory!.Count > speed07.DenseCartesianTrajectory!.Count);
AssertDenseRowsUseServoPeriod(fullSpeed.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed07.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed05.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
AssertPoseEqual(startPose, fullSpeed.DenseCartesianTrajectory[0].Skip(1).ToArray());
AssertPoseEqual(targetPose, fullSpeed.DenseCartesianTrajectory[^1].Skip(1).ToArray());
Assert.True(MovePoseTrajectoryGenerator.SatisfiesDefaultCartesianLimits(speed05.DenseCartesianTrajectory, speedRatio: 0.5));
}
[Fact]
public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming()
{
@@ -578,6 +602,41 @@ public sealed class FanucControllerRuntimeDenseTests
AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints);
}
/// <summary>
/// 验证 MovePose 会生成直角坐标 J519 队列,并使用 Data format=0 下发 X/Y/Z/W/P/R。
/// </summary>
[Fact]
public void MovePose_RealMode_GeneratesCartesianJ519Queue()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
using var j519Client = new FanucJ519Client();
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var service = CreateCompatService(runtime);
var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 };
var targetPose = new[] { 110.0, 220.0, 315.0, 4.0, 5.0, 6.0 };
service.SetUpRobot("FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 1.0);
SetPrivateField(runtime, "_pose", startPose);
service.MovePose(targetPose);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.NotEmpty(commands);
Assert.All(commands, static command => Assert.Equal(0, command.DataStyle));
AssertPoseEqual(startPose, commands[0].TargetValues.Take(6).ToArray());
AssertPoseEqual(targetPose, commands[^1].TargetValues.Take(6).ToArray());
Assert.All(commands, static command =>
{
Assert.Equal(0.0, command.TargetValues[6], precision: 6);
Assert.Equal(0.0, command.TargetValues[7], precision: 6);
Assert.Equal(0.0, command.TargetValues[8], precision: 6);
});
}
/// <summary>
/// 验证运行时稠密发送不再依赖当前 speed_ratio倍率合法性应在上游规划/生成阶段处理。
/// </summary>
@@ -1103,6 +1162,15 @@ public sealed class FanucControllerRuntimeDenseTests
}
}
private static void AssertPoseEqual(IReadOnlyList<double> expected, IReadOnlyList<double> actual)
{
Assert.Equal(expected.Count, actual.Count);
for (var index = 0; index < expected.Count; index++)
{
Assert.Equal(expected[index], actual[index], precision: 6);
}
}
/// <summary>
/// 创建用于就绪状态测试的最小 J519 响应。
/// </summary>