✨ feat(fanuc): 添加直角坐标点动功能与相关接口
* 新增 `MovePose` 方法,支持以直角坐标执行点到点移动。 * 引入 `LegacyCartesianPoseRequest` 类,处理直角位姿请求体的解析与验证。 * 更新 `LegacyHttpApiController`,实现 `/move_pose/` 路由以支持新功能。 * 增强状态快照元数据,提供机器人初始化状态与已上传轨迹信息。 * 更新前端状态页面,增加直角坐标点动控制面板与步长设置选项。 * 相关文档与测试用例同步更新,确保新功能的完整性与稳定性。
This commit is contained in:
@@ -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>
|
||||
|
||||
Reference in New Issue
Block a user