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>

View File

@@ -163,6 +163,28 @@ public sealed class FanucJ519ClientTests : IDisposable
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证直角坐标命令会把 Data format 写为 0并按通用目标槽位写入 X/Y/Z/W/P/R。
/// </summary>
[Fact]
public void PackCommandPacket_WritesCartesianDataFormatAndTargetValues()
{
var command = new FanucJ519Command(
sequence: 7,
targetValues: [100.0, 200.0, 300.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0],
dataStyle: 0);
var packet = FanucJ519Protocol.PackCommandPacket(command);
Assert.Equal(0, packet[0x12]);
Assert.Equal(100.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
Assert.Equal(200.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x20, 4)));
Assert.Equal(300.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x24, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x28, 4)));
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x2c, 4)));
Assert.Equal(3.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
}
/// <summary>
/// 验证配置 J519 buffer size 后,实际回发命令序号会在状态包序号基础上增加该缓冲深度。
/// </summary>

View File

@@ -956,6 +956,111 @@ public sealed class RuntimeOrchestrationTests
}
}
/// <summary>
/// 验证飞拍执行阻塞在运行时时,状态页元数据快照仍能通过短锁快速返回。
/// </summary>
[Fact]
public async Task ControllerClientCompatService_GetStatusSnapshotMetadata_DoesNotWaitForRunningFlyshot()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
var executing = Task.Run(() => service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2)));
var metadataTask = Task.Run(() => service.GetStatusSnapshotMetadata());
var completed = await Task.WhenAny(metadataTask, Task.Delay(TimeSpan.FromMilliseconds(150)));
runtime.ReleaseExecution();
await executing;
Assert.Same(metadataTask, completed);
var metadata = await metadataTask;
Assert.True(metadata.IsSetup);
Assert.Equal("FANUC_LR_Mate_200iD", metadata.RobotName);
Assert.Equal(["demo-flyshot"], metadata.UploadedTrajectories);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证两个飞拍执行命令必须串行进入 runtime避免 J519 队列被并发执行覆盖。
/// </summary>
[Fact]
public async Task ControllerClientCompatService_ExecuteTrajectoryByName_SerializesConcurrentExecutionCommands()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
var first = Task.Run(() => service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2)));
var second = Task.Run(() => service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
await Task.Delay(TimeSpan.FromMilliseconds(100));
Assert.Equal(1, runtime.ExecuteCallCount);
runtime.ReleaseExecution();
await first;
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2), expectedCallCount: 2));
runtime.ReleaseExecution();
await second;
Assert.Equal(2, runtime.ExecuteCallCount);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证飞拍链路在进入运行时前就会准备最终发送队列,而不是把 speedRatio 重采样留给运行时临场处理。
/// </summary>
@@ -2005,6 +2110,12 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
{
LastExecutedResult = result;
}
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
LastExecutedResult = result;
}
}
/// <summary>
@@ -2166,6 +2277,182 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
_jointPositions = finalJointPositions.ToArray();
}
}
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
}
}
/// <summary>
/// 模拟 runtime 执行入口长期占用的测试运行时,用于验证兼容层锁边界。
/// </summary>
internal sealed class BlockingExecutionControllerRuntime : IControllerRuntime
{
private readonly object _lock = new();
private readonly ManualResetEventSlim _executionStarted = new(false);
private readonly ManualResetEventSlim _releaseExecution = new(false);
private readonly double[] _jointPositions;
private bool _isEnabled;
private bool _isInMotion;
private int _executeCallCount;
/// <summary>
/// 初始化一份会阻塞 ExecuteTrajectory 的测试运行时。
/// </summary>
/// <param name="initialJointPositions">初始关节反馈。</param>
public BlockingExecutionControllerRuntime(IReadOnlyList<double> initialJointPositions)
{
_jointPositions = initialJointPositions.ToArray();
}
/// <summary>
/// 获取 runtime 执行入口被调用的次数。
/// </summary>
public int ExecuteCallCount
{
get
{
lock (_lock)
{
return _executeCallCount;
}
}
}
/// <summary>
/// 等待指定序号的执行调用进入 runtime。
/// </summary>
/// <param name="timeout">最长等待时间。</param>
/// <param name="expectedCallCount">期望已经进入的执行次数。</param>
/// <returns>是否在超时前等到。</returns>
public bool WaitForExecutionStarted(TimeSpan timeout, int expectedCallCount = 1)
{
var deadline = DateTimeOffset.UtcNow.Add(timeout);
while (DateTimeOffset.UtcNow < deadline)
{
lock (_lock)
{
if (_executeCallCount >= expectedCallCount)
{
return true;
}
}
_executionStarted.Wait(TimeSpan.FromMilliseconds(10));
}
return false;
}
/// <summary>
/// 释放当前阻塞的执行调用。
/// </summary>
public void ReleaseExecution()
{
_releaseExecution.Set();
}
public void ResetRobot(RobotProfile robot, string robotName)
{
}
public void SetActiveController(bool sim)
{
}
public void Connect(string robotIp)
{
}
public void Disconnect()
{
}
public void EnableRobot(int bufferSize)
{
_isEnabled = true;
}
public void DisableRobot()
{
_isEnabled = false;
}
public void StopMove()
{
lock (_lock)
{
_isInMotion = false;
}
ReleaseExecution();
}
public double GetSpeedRatio() => 1.0;
public void SetSpeedRatio(double ratio)
{
}
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
public void SetTcp(double x, double y, double z)
{
}
public bool GetIo(int port, string ioType) => false;
public void SetIo(int port, bool value, string ioType)
{
}
public IReadOnlyList<double> GetJointPositions()
{
return _jointPositions.ToArray();
}
public IReadOnlyList<double> GetPose() => [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
public ControllerStateSnapshot GetSnapshot()
{
lock (_lock)
{
return new ControllerStateSnapshot(
capturedAt: DateTimeOffset.UtcNow,
connectionState: "Connected",
isEnabled: _isEnabled,
isInMotion: _isInMotion,
speedRatio: 1.0,
jointPositions: _jointPositions.ToArray(),
cartesianPose: Array.Empty<double>(),
activeAlarms: Array.Empty<RuntimeAlarm>());
}
}
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
{
lock (_lock)
{
_executeCallCount++;
_isInMotion = true;
_executionStarted.Set();
_releaseExecution.Reset();
}
_releaseExecution.Wait();
lock (_lock)
{
_isInMotion = false;
_executionStarted.Reset();
}
}
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
ExecuteTrajectory(result, _jointPositions);
}
}
/// <summary>
@@ -2292,4 +2579,8 @@ internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime
_jointPositions = finalJointPositions.ToArray();
}
}
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
}
}

View File

@@ -44,6 +44,28 @@ public sealed class DebugConsoleEndpointTests(FlyshotServerFactory factory) : IC
Assert.Contains("/api/debug/config", script, StringComparison.Ordinal);
}
/// <summary>
/// 调试页应当为 MovePose 提供可直接发送的六字段请求体模板。
/// </summary>
[Fact]
public async Task GetDebugScript_ContainsMovePoseRequestSample()
{
using var configuredFactory = CreateFactoryWithSwaggerEnabled(true);
using var client = configuredFactory.CreateClient();
using var scriptResponse = await client.GetAsync("/assets/debug.js");
Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode);
var script = await scriptResponse.Content.ReadAsStringAsync();
Assert.Contains("/move_pose/", script, StringComparison.Ordinal);
Assert.Contains("x: 100.0", script, StringComparison.Ordinal);
Assert.Contains("y: 200.0", script, StringComparison.Ordinal);
Assert.Contains("z: 300.0", script, StringComparison.Ordinal);
Assert.Contains("w: 0.0", script, StringComparison.Ordinal);
Assert.Contains("p: 45.0", script, StringComparison.Ordinal);
Assert.Contains("r: 0.0", script, StringComparison.Ordinal);
}
/// <summary>
/// 当 Swagger 启用时,调试配置 API 应当返回实际 Swagger JSON 地址。
/// </summary>

View File

@@ -1,5 +1,6 @@
using System.Net;
using System.Net.Http.Json;
using System.Text;
using System.Text.Json;
namespace Flyshot.Server.IntegrationTests;
@@ -131,6 +132,13 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
Assert.Equal("robot moved", moveJointJson.RootElement.GetProperty("status").GetString());
}
using (var movePoseResponse = await client.PostAsJsonAsync("/move_pose/", new { x = 100.0, y = 200.0, z = 300.0, w = 1.0, p = 2.0, r = 3.0 }))
{
Assert.Equal(HttpStatusCode.OK, movePoseResponse.StatusCode);
using var movePoseJson = await ReadJsonAsync(movePoseResponse);
Assert.Equal("robot moved", movePoseJson.RootElement.GetProperty("status").GetString());
}
using (var getJointPositionResponse = await client.GetAsync("/get_joint_position/"))
{
Assert.Equal(HttpStatusCode.OK, getJointPositionResponse.StatusCode);
@@ -165,6 +173,32 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
}
}
/// <summary>
/// 验证 MovePose 请求必须显式提供六个有限直角坐标字段,避免缺字段被模型绑定静默补 0。
/// </summary>
[Theory]
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":2.0}""")]
[InlineData("null")]
[InlineData("""{"x":1e999,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":1000.1,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":-1000.1,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":200.0,"z":-0.1,"w":1.0,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":-180.1,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":90.1,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":180.1}""")]
public async Task MovePose_InvalidPayload_ReturnsLegacyBadRequest(string payload)
{
using var client = factory.CreateClient();
await InitializeRobotAsync(client);
using var content = new StringContent(payload, Encoding.UTF8, "application/json");
using var response = await client.PostAsync("/move_pose/", content);
Assert.Equal(HttpStatusCode.BadRequest, response.StatusCode);
using var json = await ReadJsonAsync(response);
Assert.Equal("MovePose failed", json.RootElement.GetProperty("detail").GetString());
}
/// <summary>
/// 验证飞拍 HTTP 接口可以按旧 API 层的路径和字段完成上传、列出、执行与删除。
/// </summary>

View File

@@ -34,6 +34,39 @@ public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFi
Assert.Contains("/api/status/snapshot", script, StringComparison.Ordinal);
}
/// <summary>
/// 状态页应当提供直角坐标点动按钮,并复用现有 MovePose HTTP 接口。
/// </summary>
[Fact]
public async Task GetStatusPageAssets_ExposeCartesianJogControls()
{
using var client = factory.CreateClient();
using var htmlResponse = await client.GetAsync("/status.html");
Assert.Equal(HttpStatusCode.OK, htmlResponse.StatusCode);
var html = await htmlResponse.Content.ReadAsStringAsync();
Assert.Contains("直角坐标点动", html, StringComparison.Ordinal);
Assert.Contains("id=\"linear-step\"", html, StringComparison.Ordinal);
Assert.Contains("id=\"angular-step\"", html, StringComparison.Ordinal);
foreach (var axis in new[] { "x", "y", "z", "w", "p", "r" })
{
Assert.Contains($"data-axis=\"{axis}\"", html, StringComparison.Ordinal);
Assert.Contains($"data-axis=\"{axis}\" data-direction=\"1\"", html, StringComparison.Ordinal);
Assert.Contains($"data-axis=\"{axis}\" data-direction=\"-1\"", html, StringComparison.Ordinal);
}
using var scriptResponse = await client.GetAsync("/assets/status.js");
Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode);
var script = await scriptResponse.Content.ReadAsStringAsync();
Assert.Contains("/move_pose/", script, StringComparison.Ordinal);
Assert.Contains("cartesianPose", script, StringComparison.Ordinal);
Assert.Contains("pointerdown", script, StringComparison.Ordinal);
Assert.Contains("pointerup", script, StringComparison.Ordinal);
}
/// <summary>
/// 验证状态快照 API 会返回运行时连接、使能、速度和机器人元数据。
/// </summary>