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

@@ -0,0 +1,200 @@
#!/usr/bin/env python3
"""读取 FANUC J519 允许速度/加速度/jerk 上限表。
本脚本只做 UDP packet type=3 查询,不启动运行时服务,也不写入机器人参数。
响应中的 limit 值保留为控制器原始单位:速度 deg/s、加速度 deg/s^2、jerk deg/s^3。
"""
from __future__ import annotations
import argparse
import csv
import socket
import struct
import sys
from dataclasses import dataclass
from pathlib import Path
LIMIT_TYPES = {
0: ("velocity", "deg/s"),
1: ("acceleration", "deg/s^2"),
2: ("jerk", "deg/s^3"),
}
@dataclass(frozen=True)
class LimitTable:
"""保存单个轴、单种限制类型的 20 档 no-payload / max-payload 上限表。"""
axis: int
limit_type: int
vmax_mm_s: int
intermediate_check_s: int
no_payload: tuple[float, ...]
max_payload: tuple[float, ...]
def pack_request(axis: int, limit_type: int) -> bytes:
"""按手册 Table 5 封装允许上限表查询请求。"""
return struct.pack(">IIII", 3, 1, axis, limit_type)
def parse_response(packet: bytes, expected_axis: int, expected_type: int) -> LimitTable:
"""按手册 Table 6 解析允许上限表响应。"""
if len(packet) != 184:
raise ValueError(f"响应长度应为 184 字节,实际为 {len(packet)} 字节。")
packet_type, version, axis, limit_type, vmax, check_time = struct.unpack_from(">IIIIII", packet, 0)
if packet_type != 3 or version != 1:
raise ValueError(f"响应头不正确packet_type={packet_type}, version={version}")
if axis != expected_axis or limit_type != expected_type:
raise ValueError(
f"响应与请求不匹配:期望 axis={expected_axis}, type={expected_type}"
f"实际 axis={axis}, type={limit_type}"
)
values = struct.unpack_from(">40f", packet, 24)
return LimitTable(
axis=axis,
limit_type=limit_type,
vmax_mm_s=vmax,
intermediate_check_s=check_time,
no_payload=tuple(values[:20]),
max_payload=tuple(values[20:]),
)
def query_table(sock: socket.socket, robot_ip: str, port: int, axis: int, limit_type: int) -> LimitTable:
"""发送单个查询请求并等待对应响应。"""
sock.sendto(pack_request(axis, limit_type), (robot_ip, port))
packet, address = sock.recvfrom(2048)
if address[0] != robot_ip:
raise ValueError(f"收到非目标机器人响应:{address[0]}:{address[1]}")
return parse_response(packet, axis, limit_type)
def iter_requested_tables(
robot_ip: str,
port: int,
timeout_s: float,
axes: range,
limit_types: tuple[int, ...],
) -> list[LimitTable]:
"""按轴和限制类型顺序读取所有上限表。"""
tables: list[LimitTable] = []
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock:
sock.settimeout(timeout_s)
for axis in axes:
for limit_type in limit_types:
tables.append(query_table(sock, robot_ip, port, axis, limit_type))
return tables
def write_csv(path: Path, tables: list[LimitTable]) -> None:
"""把 20 档速度区间和两套 payload 表写成扁平 CSV。"""
with path.open("w", newline="", encoding="utf-8") as file:
writer = csv.writer(file)
writer.writerow(
[
"axis",
"limit_type",
"unit",
"vmax_mm_s",
"intermediate_check_s",
"speed_bin_index",
"speed_bin_upper_mm_s",
"no_payload_limit",
"max_payload_limit",
]
)
for table in tables:
limit_name, unit = LIMIT_TYPES[table.limit_type]
for index, (no_payload, max_payload) in enumerate(zip(table.no_payload, table.max_payload), start=1):
writer.writerow(
[
table.axis,
limit_name,
unit,
table.vmax_mm_s,
table.intermediate_check_s,
index,
table.vmax_mm_s * index / 20.0,
f"{no_payload:.9g}",
f"{max_payload:.9g}",
]
)
def print_summary(tables: list[LimitTable]) -> None:
"""打印每张表的首末档,便于现场快速确认返回值是否正常。"""
for table in tables:
limit_name, unit = LIMIT_TYPES[table.limit_type]
print(
f"axis={table.axis} type={limit_name:<12} unit={unit:<8} "
f"vmax={table.vmax_mm_s}mm/s check={table.intermediate_check_s}s "
f"no_payload[1]={table.no_payload[0]:.6g} no_payload[20]={table.no_payload[-1]:.6g} "
f"max_payload[1]={table.max_payload[0]:.6g} max_payload[20]={table.max_payload[-1]:.6g}"
)
def parse_args() -> argparse.Namespace:
"""解析命令行参数。"""
parser = argparse.ArgumentParser(
description="Read FANUC J519 packet type=3 allowable upper limit tables.",
)
parser.add_argument("robot_ip", help="机器人控制柜 IP例如 192.168.10.11。")
parser.add_argument("--port", type=int, default=60015, help="J519 UDP 端口,默认 60015。")
parser.add_argument("--timeout", type=float, default=2.0, help="单次响应超时秒数,默认 2.0。")
parser.add_argument("--axis-start", type=int, default=1, help="起始轴号,默认 1。")
parser.add_argument("--axis-end", type=int, default=9, help="结束轴号,默认 9。")
parser.add_argument(
"--types",
default="0,1,2",
help="限制类型列表0=velocity, 1=acceleration, 2=jerk默认 0,1,2。",
)
parser.add_argument(
"--csv",
type=Path,
default=Path("analysis/fanuc_allowable_limit_tables.csv"),
help="CSV 输出路径。",
)
return parser.parse_args()
def main() -> int:
"""脚本入口。"""
args = parse_args()
limit_types = tuple(int(item.strip()) for item in args.types.split(",") if item.strip())
unknown_types = [item for item in limit_types if item not in LIMIT_TYPES]
if unknown_types:
print(f"未知限制类型:{unknown_types}", file=sys.stderr)
return 2
if not 1 <= args.axis_start <= args.axis_end <= 9:
print("轴范围必须满足 1 <= axis-start <= axis-end <= 9。", file=sys.stderr)
return 2
tables = iter_requested_tables(
args.robot_ip,
args.port,
args.timeout,
range(args.axis_start, args.axis_end + 1),
limit_types,
)
args.csv.parent.mkdir(parents=True, exist_ok=True)
write_csv(args.csv, tables)
print_summary(tables)
print(f"\nCSV 已写入:{args.csv}")
return 0
if __name__ == "__main__":
raise SystemExit(main())

BIN
docs/0.7x.pcap Normal file

Binary file not shown.

View File

@@ -12,6 +12,7 @@ namespace Flyshot.ControllerClientCompat;
public sealed class ControllerClientCompatService : IControllerClientCompatService public sealed class ControllerClientCompatService : IControllerClientCompatService
{ {
private readonly object _stateLock = new(); private readonly object _stateLock = new();
private readonly object _motionLock = new();
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _uploadedTrajectories = new(StringComparer.Ordinal); private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _uploadedTrajectories = new(StringComparer.Ordinal);
private readonly ControllerClientCompatOptions _options; private readonly ControllerClientCompatOptions _options;
private readonly ControllerClientCompatRobotCatalog _robotCatalog; private readonly ControllerClientCompatRobotCatalog _robotCatalog;
@@ -197,8 +198,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
_runtime.SetActiveController(sim);
} }
_runtime.SetActiveController(sim);
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -214,9 +216,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
_runtime.Connect(robotIp);
} }
_runtime.Connect(robotIp);
_logger?.LogInformation("Connect 完成: robotIp={RobotIp}", robotIp); _logger?.LogInformation("Connect 完成: robotIp={RobotIp}", robotIp);
} }
@@ -226,8 +229,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
_runtime.Disconnect();
} }
_runtime.Disconnect();
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -237,8 +241,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
_runtime.EnableRobot(bufferSize);
} }
_runtime.EnableRobot(bufferSize);
_logger?.LogInformation("EnableRobot 完成"); _logger?.LogInformation("EnableRobot 完成");
} }
@@ -249,8 +253,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
_runtime.DisableRobot();
} }
// DisableRobot 是中断/控制命令,不能被长时间运动串行锁挡住。
_runtime.DisableRobot();
_logger?.LogInformation("DisableRobot 完成"); _logger?.LogInformation("DisableRobot 完成");
} }
@@ -261,8 +267,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
_runtime.StopMove();
} }
// StopMove 必须能在运动执行期间直接进入 runtime 取消发送任务。
_runtime.StopMove();
_logger?.LogInformation("StopMove 完成"); _logger?.LogInformation("StopMove 完成");
} }
@@ -272,14 +280,31 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
return _runtime.GetSnapshot(); return _runtime.GetSnapshot();
} }
/// <inheritdoc />
public ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata()
{
lock (_stateLock)
{
var isSetup = _activeRobotProfile is not null;
return new ControllerClientStatusSnapshotMetadata(
isSetup,
isSetup ? _configuredRobotName : null,
isSetup ? _activeRobotProfile!.DegreesOfFreedom : 0,
isSetup ? _uploadedTrajectories.Keys.ToArray() : Array.Empty<string>(),
GetServerVersion(),
GetClientVersion());
}
}
/// <inheritdoc /> /// <inheritdoc />
public double GetSpeedRatio() public double GetSpeedRatio()
{ {
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
return _runtime.GetSpeedRatio();
} }
return _runtime.GetSpeedRatio();
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -288,8 +313,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
_runtime.SetSpeedRatio(ratio);
} }
_runtime.SetSpeedRatio(ratio);
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -298,8 +324,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
_runtime.SetIo(port, value, ioType);
} }
_runtime.SetIo(port, value, ioType);
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -308,8 +335,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
return _runtime.GetIo(port, ioType);
} }
return _runtime.GetIo(port, ioType);
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -341,8 +369,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
_runtime.SetTcp(x, y, z);
} }
_runtime.SetTcp(x, y, z);
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -351,8 +380,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
return _runtime.GetTcp();
} }
return _runtime.GetTcp();
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -361,8 +391,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
return _runtime.GetJointPositions();
} }
return _runtime.GetJointPositions();
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -373,16 +404,44 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("MoveJoint 开始: 目标关节数={JointCount}", jointPositions.Count); _logger?.LogInformation("MoveJoint 开始: 目标关节数={JointCount}", jointPositions.Count);
_logger?.LogDebug("MoveJoint 目标关节: {Joints}", string.Join(", ", jointPositions.Select(j => j.ToString("F4")))); _logger?.LogDebug("MoveJoint 目标关节: {Joints}", string.Join(", ", jointPositions.Select(j => j.ToString("F4"))));
RobotProfile robot;
lock (_stateLock) lock (_stateLock)
{ {
var robot = RequireActiveRobot(); robot = RequireActiveRobot();
EnsureRuntimeEnabled(); EnsureRuntimeEnabled();
ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint"); }
lock (_motionLock)
{
ExecuteMoveJointAndWait(robot, jointPositions, "MoveJoint");
} }
_logger?.LogInformation("MoveJoint 完成"); _logger?.LogInformation("MoveJoint 完成");
} }
/// <inheritdoc />
public void MovePose(IReadOnlyList<double> pose)
{
ArgumentNullException.ThrowIfNull(pose);
_logger?.LogInformation("MovePose 开始: 目标位姿维度={PoseCount}", pose.Count);
_logger?.LogDebug("MovePose 目标位姿: {Pose}", string.Join(", ", pose.Select(v => v.ToString("F4"))));
RobotProfile robot;
lock (_stateLock)
{
robot = RequireActiveRobot();
EnsureRuntimeEnabled();
}
lock (_motionLock)
{
ExecuteMovePoseAndWait(robot, pose, "MovePose");
}
_logger?.LogInformation("MovePose 完成");
}
/// <inheritdoc /> /// <inheritdoc />
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null) public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
{ {
@@ -398,13 +457,19 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogDebug("ExecuteTrajectory 路点详情: {Waypoints}", _logger?.LogDebug("ExecuteTrajectory 路点详情: {Waypoints}",
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Select(j => j.ToString("F4")))}]"))); string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Select(j => j.ToString("F4")))}]")));
RobotProfile robot;
CompatibilityRobotSettings settings;
lock (_stateLock) lock (_stateLock)
{ {
var robot = RequireActiveRobot(); robot = RequireActiveRobot();
EnsureRuntimeEnabled(); EnsureRuntimeEnabled();
settings = RequireRobotSettings();
}
lock (_motionLock)
{
// 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。 // 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。
var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale; var planningSpeedScale = settings.PlanningSpeedScale;
var speedRatio = _runtime.GetSnapshot().SpeedRatio; var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale, speedRatio); var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale, speedRatio);
_logger?.LogInformation( _logger?.LogInformation(
@@ -428,8 +493,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock) lock (_stateLock)
{ {
EnsureRobotSetup(); EnsureRobotSetup();
return _runtime.GetPose();
} }
return _runtime.GetPose();
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -443,16 +509,20 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
trajectory.Waypoints.Count, trajectory.Waypoints.Count,
trajectory.ShotFlags.Count(static f => f)); trajectory.ShotFlags.Count(static f => f));
string robotName;
CompatibilityRobotSettings settings;
lock (_stateLock) lock (_stateLock)
{ {
EnsureRuntimeEnabled(); EnsureRuntimeEnabled();
_uploadedTrajectories[trajectory.Name] = trajectory; _uploadedTrajectories[trajectory.Name] = trajectory;
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
var settings = _robotSettings ?? CreateDefaultRobotSettings(); settings = _robotSettings ?? CreateDefaultRobotSettings();
_trajectoryStore.Save(robotName, settings, trajectory);
} }
// RobotConfig.json 持久化是文件 I/O放在状态锁外避免状态页轮询被磁盘写入拖住。
_trajectoryStore.Save(robotName, settings, trajectory);
_logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name); _logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name);
} }
@@ -478,25 +548,34 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}", "ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}",
name, options.Method, options.MoveToStart, options.UseCache, options.Wait); name, options.Method, options.MoveToStart, options.UseCache, options.Wait);
RobotProfile robot;
CompatibilityRobotSettings settings;
ControllerClientCompatUploadedTrajectory trajectory;
lock (_stateLock) lock (_stateLock)
{ {
var robot = RequireActiveRobot(); robot = RequireActiveRobot();
EnsureRuntimeEnabled(); EnsureRuntimeEnabled();
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory)) if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
{ {
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name); _logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory does not exist."); throw new InvalidOperationException("FlyShot trajectory does not exist.");
} }
// 飞拍执行只拿上传轨迹的瞬时副本,后续规划/导出都不再依赖字典锁。
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
if (trajectory.Waypoints.Count == 0) if (trajectory.Waypoints.Count == 0)
{ {
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name); _logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory contains no waypoints."); throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
} }
settings = RequireRobotSettings();
}
lock (_motionLock)
{
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。 // 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
var settings = RequireRobotSettings();
var speedRatio = _runtime.GetSnapshot().SpeedRatio; var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale, speedRatio); var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale, speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio); bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
@@ -523,7 +602,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
if (options.MoveToStart) if (options.MoveToStart)
{ {
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点"); _logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start"); ExecuteMoveJointAndWait(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
EnsureFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name); EnsureFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name);
} }
else else
@@ -555,7 +634,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
/// <param name="robot">当前机器人模型。</param> /// <param name="robot">当前机器人模型。</param>
/// <param name="targetJointPositions">目标关节位置,单位为弧度。</param> /// <param name="targetJointPositions">目标关节位置,单位为弧度。</param>
/// <param name="operationName">用于日志和超时异常的操作名。</param> /// <param name="operationName">用于日志和超时异常的操作名。</param>
private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName) private void ExecuteMoveJointAndWait(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
{ {
var currentJointPositions = _runtime.GetJointPositions(); var currentJointPositions = _runtime.GetJointPositions();
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions)); EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
@@ -574,6 +653,30 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
WaitForRuntimeMotionComplete(operationName, moveResult.Duration); WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
} }
/// <summary>
/// 从当前 TCP 位姿生成临时直角 PTP 稠密轨迹并阻塞等待运行时完成。
/// </summary>
/// <param name="robot">当前机器人模型,用于取得 J519 伺服周期。</param>
/// <param name="targetPose">目标位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
/// <param name="operationName">用于日志和超时异常的操作名。</param>
private void ExecuteMovePoseAndWait(RobotProfile robot, IReadOnlyList<double> targetPose, string operationName)
{
var currentPose = NormalizeRuntimePose(_runtime.GetPose());
EnsurePoseVector(targetPose, nameof(targetPose));
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var moveResult = MovePoseTrajectoryGenerator.CreateResult(currentPose, targetPose, robot.ServoPeriod, speedRatio, _logger);
_logger?.LogInformation(
"{OperationName} 直角PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
operationName,
speedRatio,
moveResult.Duration.TotalSeconds,
moveResult.DenseCartesianTrajectory?.Count ?? 0);
_runtime.ExecuteCartesianTrajectory(moveResult, targetPose);
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
}
/// <summary> /// <summary>
/// 校验当前反馈是否接近飞拍起点;不接近时直接抛出兼容错误。 /// 校验当前反馈是否接近飞拍起点;不接近时直接抛出兼容错误。
/// </summary> /// </summary>
@@ -649,43 +752,45 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("SaveTrajectoryInfo 开始: name={Name}, method={Method}", name, method); _logger?.LogInformation("SaveTrajectoryInfo 开始: name={Name}, method={Method}", name, method);
RobotProfile robot;
CompatibilityRobotSettings planningSettings;
ControllerClientCompatUploadedTrajectory trajectory;
lock (_stateLock) lock (_stateLock)
{ {
var robot = RequireActiveRobot(); robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory)) if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
{ {
_logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name); _logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory does not exist."); throw new InvalidOperationException("FlyShot trajectory does not exist.");
} }
// 先通过规划校验避免静默接受非法参数,同时把轨迹信息强制刷写到本地 JSON。 // 先通过规划校验避免静默接受非法参数,同时把轨迹信息强制刷写到本地 JSON。
var planningSettings = RequireRobotSettings(); // 保存轨迹信息会执行规划和文件导出,先复制上传数据再释放状态锁。
var speedRatio = _runtime.GetSnapshot().SpeedRatio; trajectory = CloneUploadedTrajectory(uploadedTrajectory);
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot( planningSettings = RequireRobotSettings();
robot,
trajectory,
new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory: true,
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
// var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
// var settings = _robotSettings ?? CreateDefaultRobotSettings();
// _trajectoryStore.Save(robotName, settings, trajectory);
} }
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory: true,
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name); _logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
} }
@@ -699,42 +804,48 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("IsFlyshotTrajectoryValid 开始: name={Name}, method={Method}", name, method); _logger?.LogInformation("IsFlyshotTrajectoryValid 开始: name={Name}, method={Method}", name, method);
RobotProfile robot;
CompatibilityRobotSettings planningSettings;
ControllerClientCompatUploadedTrajectory trajectory;
lock (_stateLock) lock (_stateLock)
{ {
var robot = RequireActiveRobot(); robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory)) if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
{ {
_logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name); _logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory does not exist."); throw new InvalidOperationException("FlyShot trajectory does not exist.");
} }
var planningSettings = RequireRobotSettings(); // 有效性检查只消费当前快照,不要求和后续上传/删除形成长事务。
var speedRatio = _runtime.GetSnapshot().SpeedRatio; trajectory = CloneUploadedTrajectory(uploadedTrajectory);
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot( planningSettings = RequireRobotSettings();
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory,
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
duration = bundle.Result.Duration;
_logger?.LogInformation(
"IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
name, bundle.Result.IsValid, duration.TotalSeconds);
return bundle.Result.IsValid;
} }
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory,
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
duration = bundle.Result.Duration;
_logger?.LogInformation(
"IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
name, bundle.Result.IsValid, duration.TotalSeconds);
return bundle.Result.IsValid;
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -747,6 +858,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name); _logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name);
string robotName;
lock (_stateLock) lock (_stateLock)
{ {
if (!_uploadedTrajectories.Remove(name)) if (!_uploadedTrajectories.Remove(name))
@@ -755,10 +867,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
throw new InvalidOperationException("DeleteFlyShotTraj failed"); throw new InvalidOperationException("DeleteFlyShotTraj failed");
} }
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
_trajectoryStore.Delete(robotName, name);
} }
// 删除持久化文件不占用状态锁,状态页只需要看到内存字典的即时快照。
_trajectoryStore.Delete(robotName, name);
_logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name); _logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name);
} }
@@ -798,6 +912,21 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
return _robotSettings ?? CreateDefaultRobotSettings(); return _robotSettings ?? CreateDefaultRobotSettings();
} }
/// <summary>
/// 复制一份上传轨迹快照,避免锁外规划期间观察到可变集合引用。
/// </summary>
/// <param name="trajectory">待复制的上传轨迹。</param>
/// <returns>上传轨迹副本。</returns>
private static ControllerClientCompatUploadedTrajectory CloneUploadedTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
{
return new ControllerClientCompatUploadedTrajectory(
trajectory.Name,
trajectory.Waypoints,
trajectory.ShotFlags,
trajectory.OffsetValues,
trajectory.AddressGroups);
}
/// <summary> /// <summary>
/// 校验机器人已经完成初始化。 /// 校验机器人已经完成初始化。
/// </summary> /// </summary>
@@ -841,6 +970,45 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
} }
} }
/// <summary>
/// 校验直角位姿为 [x,y,z,w,p,r] 六维有限数。
/// </summary>
/// <param name="pose">待校验位姿,单位为 mm/deg。</param>
/// <param name="paramName">调用方参数名。</param>
private static void EnsurePoseVector(IReadOnlyList<double> pose, string paramName)
{
if (pose.Count != 6)
{
throw new ArgumentException("位姿必须为 [x,y,z,w,p,r] 六维数组。", paramName);
}
for (var index = 0; index < pose.Count; index++)
{
var value = pose[index];
if (double.IsNaN(value) || double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个位姿值必须是有限数值。");
}
}
}
/// <summary>
/// 将运行时位姿快照归一化为 J519 直角命令需要的 [x,y,z,w,p,r] 六维。
/// </summary>
/// <param name="pose">运行时返回的位姿数组。</param>
/// <returns>六维直角位姿。</returns>
private static IReadOnlyList<double> NormalizeRuntimePose(IReadOnlyList<double> pose)
{
if (pose.Count < 6)
{
throw new InvalidOperationException("Runtime pose must contain at least [x,y,z,w,p,r].");
}
var normalized = pose.Take(6).ToArray();
EnsurePoseVector(normalized, nameof(pose));
return normalized;
}
/// <summary> /// <summary>
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。 /// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
/// </summary> /// </summary>

View File

@@ -0,0 +1,62 @@
namespace Flyshot.ControllerClientCompat;
/// <summary>
/// 保存状态页需要的 ControllerClient 兼容层元数据快照,避免状态页连续读取多个受保护字段。
/// </summary>
public sealed record ControllerClientStatusSnapshotMetadata
{
/// <summary>
/// 初始化一份兼容层状态元数据快照。
/// </summary>
/// <param name="isSetup">当前是否已经完成机器人初始化。</param>
/// <param name="robotName">当前机器人名称;未初始化时为空。</param>
/// <param name="degreesOfFreedom">当前机器人自由度;未初始化时为 0。</param>
/// <param name="uploadedTrajectories">当前已上传轨迹名称快照。</param>
/// <param name="serverVersion">兼容服务端版本。</param>
/// <param name="clientVersion">兼容客户端版本。</param>
public ControllerClientStatusSnapshotMetadata(
bool isSetup,
string? robotName,
int degreesOfFreedom,
IReadOnlyList<string> uploadedTrajectories,
string serverVersion,
string clientVersion)
{
IsSetup = isSetup;
RobotName = robotName;
DegreesOfFreedom = degreesOfFreedom;
UploadedTrajectories = uploadedTrajectories.ToArray();
ServerVersion = serverVersion;
ClientVersion = clientVersion;
}
/// <summary>
/// 获取当前是否已经完成机器人初始化。
/// </summary>
public bool IsSetup { get; }
/// <summary>
/// 获取当前机器人名称;未初始化时为空。
/// </summary>
public string? RobotName { get; }
/// <summary>
/// 获取当前机器人自由度;未初始化时为 0。
/// </summary>
public int DegreesOfFreedom { get; }
/// <summary>
/// 获取已上传轨迹名称数组快照。
/// </summary>
public IReadOnlyList<string> UploadedTrajectories { get; }
/// <summary>
/// 获取兼容服务端版本。
/// </summary>
public string ServerVersion { get; }
/// <summary>
/// 获取兼容客户端版本。
/// </summary>
public string ClientVersion { get; }
}

View File

@@ -95,6 +95,12 @@ public interface IControllerClientCompatService
/// <returns>控制器运行时状态快照。</returns> /// <returns>控制器运行时状态快照。</returns>
ControllerStateSnapshot GetControllerSnapshot(); ControllerStateSnapshot GetControllerSnapshot();
/// <summary>
/// 读取状态页所需的兼容层元数据快照。
/// </summary>
/// <returns>兼容层元数据快照。</returns>
ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata();
/// <summary> /// <summary>
/// 获取当前速度倍率。 /// 获取当前速度倍率。
/// </summary> /// </summary>
@@ -157,6 +163,12 @@ public interface IControllerClientCompatService
/// <param name="jointPositions">目标关节位置。</param> /// <param name="jointPositions">目标关节位置。</param>
void MoveJoint(IReadOnlyList<double> jointPositions); void MoveJoint(IReadOnlyList<double> jointPositions);
/// <summary>
/// 按直角坐标移动到目标 TCP 位姿。
/// </summary>
/// <param name="pose">目标位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
void MovePose(IReadOnlyList<double> pose);
/// <summary> /// <summary>
/// 执行普通轨迹。 /// 执行普通轨迹。
/// </summary> /// </summary>

View File

@@ -0,0 +1,353 @@
using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;
namespace Flyshot.ControllerClientCompat;
/// <summary>
/// MovePose 直角坐标轨迹生成器。
/// 将起始 TCP 位姿到目标 TCP 位姿的单段运动,按保守直角速度、加速度和 jerk 限制生成 8ms 稠密点。
/// </summary>
internal static class MovePoseTrajectoryGenerator
{
/// <summary>
/// 直角位置轴最大速度,单位为 mm/s。
/// </summary>
private const double LinearVelocityLimit = 50.0;
/// <summary>
/// 直角位置轴最大加速度,单位为 mm/s^2。
/// </summary>
private const double LinearAccelerationLimit = 250.0;
/// <summary>
/// 直角位置轴最大 jerk单位为 mm/s^3。
/// </summary>
private const double LinearJerkLimit = 1250.0;
/// <summary>
/// 姿态轴最大速度,单位为 deg/s。
/// </summary>
private const double AngularVelocityLimit = 10.0;
/// <summary>
/// 姿态轴最大加速度,单位为 deg/s^2。
/// </summary>
private const double AngularAccelerationLimit = 50.0;
/// <summary>
/// 姿态轴最大 jerk单位为 deg/s^3。
/// </summary>
private const double AngularJerkLimit = 250.0;
/// <summary>
/// 7 阶平滑点到点时间律的一阶导数最大值。
/// </summary>
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
/// <summary>
/// 7 阶平滑点到点时间律的二阶导数最大值。
/// </summary>
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
/// <summary>
/// 7 阶平滑点到点时间律的三阶导数最大值。
/// </summary>
private const double SmoothPtpJerkShapeCoefficient = 52.5;
/// <summary>
/// 单次 MovePose 最大采样点数上限,避免极低速度倍率生成过大的队列。
/// </summary>
private const int MaxMovePoseSampleCount = 1_000_000;
/// <summary>
/// 离散限位校验允许的浮点容差。
/// </summary>
private const double DiscreteLimitTolerance = 1.000001;
/// <summary>
/// 离散限位校验失败时最多拉长的采样周期次数。
/// </summary>
private const int MaxDiscreteLimitStretchIterations = 10_000;
/// <summary>
/// 计算 MovePose 轨迹的完整结果。
/// </summary>
/// <param name="startPose">起始位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
/// <param name="targetPose">目标位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
/// <param name="servoPeriod">J519 伺服发送周期。</param>
/// <param name="speedRatio">速度倍率,必须大于 0。</param>
/// <param name="logger">可选诊断日志。</param>
/// <returns>包含稠密直角轨迹的规划结果。</returns>
public static TrajectoryResult CreateResult(
IReadOnlyList<double> startPose,
IReadOnlyList<double> targetPose,
TimeSpan servoPeriod,
double speedRatio,
ILogger? logger = null)
{
ArgumentNullException.ThrowIfNull(startPose);
ArgumentNullException.ThrowIfNull(targetPose);
EnsurePoseVector(startPose, nameof(startPose));
EnsurePoseVector(targetPose, nameof(targetPose));
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose execution.");
}
var samplePeriodSeconds = servoPeriod.TotalSeconds;
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
{
throw new InvalidOperationException("MovePose servo period must be a finite positive duration.");
}
var requestedDurationSeconds = ResolveDurationSeconds(startPose, targetPose, speedRatio);
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
var denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
var stretchCount = 0;
while (!SatisfiesDefaultCartesianLimits(denseCartesianTrajectory, speedRatio))
{
stretchCount++;
if (stretchCount > MaxDiscreteLimitStretchIterations)
{
throw new InvalidOperationException("MovePose duration cannot be stretched enough to satisfy Cartesian limits.");
}
// 离散差分以真实下发点为准;不满足时逐周期拉长后重采样。
durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
}
logger?.LogDebug(
"MovePoseTrajectoryGenerator: requestedDuration={RequestedDuration:F4}s, duration={Duration:F4}s, speedRatio={SpeedRatio}, samplePeriod={SamplePeriod:F6}s, sampleCount={SampleCount}, stretchCount={StretchCount}",
requestedDurationSeconds,
durationSeconds,
speedRatio,
samplePeriodSeconds,
denseCartesianTrajectory.Count,
stretchCount);
return new TrajectoryResult(
programName: "move-pose",
method: PlanningMethod.Doubles,
isValid: true,
duration: TimeSpan.FromSeconds(durationSeconds),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: denseCartesianTrajectory.Count,
denseCartesianTrajectory: denseCartesianTrajectory);
}
/// <summary>
/// 检查稠密直角轨迹是否满足默认六维速度、加速度和 jerk 限制。
/// </summary>
/// <param name="rows">稠密轨迹行,每行为 [time,x,y,z,w,p,r]。</param>
/// <param name="speedRatio">生成该轨迹时使用的速度倍率。</param>
/// <returns>满足限制时返回 true否则返回 false。</returns>
public static bool SatisfiesDefaultCartesianLimits(IReadOnlyList<IReadOnlyList<double>> rows, double speedRatio)
{
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose limit validation.");
}
double? previousTime = null;
double[]? previousPositions = null;
double[]? previousVelocities = null;
double[]? previousAccelerations = null;
foreach (var row in rows)
{
if (row.Count != 7)
{
throw new InvalidOperationException("MovePose dense trajectory rows must contain time plus six pose values.");
}
var currentTime = row[0];
var currentPositions = row.Skip(1).Take(6).ToArray();
if (previousTime is not null && previousPositions is not null)
{
var dt = currentTime - previousTime.Value;
if (dt <= 0.0)
{
throw new InvalidOperationException("MovePose dense trajectory timestamps must be strictly increasing.");
}
var velocities = new double[6];
var accelerations = new double[6];
for (var index = 0; index < 6; index++)
{
var limits = GetAxisLimits(index, speedRatio);
velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
if (Math.Abs(velocities[index]) > limits.Velocity * DiscreteLimitTolerance)
{
return false;
}
accelerations[index] = previousVelocities is null
? 0.0
: (velocities[index] - previousVelocities[index]) / dt;
if (Math.Abs(accelerations[index]) > limits.Acceleration * DiscreteLimitTolerance)
{
return false;
}
if (previousAccelerations is not null)
{
var jerk = (accelerations[index] - previousAccelerations[index]) / dt;
if (Math.Abs(jerk) > limits.Jerk * DiscreteLimitTolerance)
{
return false;
}
}
}
previousVelocities = velocities;
previousAccelerations = accelerations;
}
previousTime = currentTime;
previousPositions = currentPositions;
}
return true;
}
/// <summary>
/// 根据 7 阶平滑点到点时间律和直角六维限制计算理论最短时长。
/// </summary>
private static double ResolveDurationSeconds(IReadOnlyList<double> startPose, IReadOnlyList<double> targetPose, double speedRatio)
{
var duration = 0.0;
for (var index = 0; index < 6; index++)
{
var distance = Math.Abs(targetPose[index] - startPose[index]);
if (distance <= 0.0)
{
continue;
}
var limits = GetAxisLimits(index, speedRatio);
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limits.Velocity;
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limits.Acceleration);
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limits.Jerk);
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
}
return duration;
}
/// <summary>
/// 生成从起始位姿到目标位姿的稠密等时间隔直角轨迹点序列。
/// </summary>
private static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
IReadOnlyList<double> startPose,
IReadOnlyList<double> targetPose,
double durationSeconds,
double samplePeriodSeconds)
{
var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1;
var rows = new List<IReadOnlyList<double>>(checked((int)sampleCount));
for (var index = 0L; index < sampleCount; index++)
{
var time = Math.Min(index * samplePeriodSeconds, durationSeconds);
rows.Add(CreateRow(time, durationSeconds, startPose, targetPose));
}
return rows;
}
/// <summary>
/// 将请求时长向上对齐到整数个伺服周期。
/// </summary>
private static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
{
return ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) * samplePeriodSeconds;
}
/// <summary>
/// 计算时长对应的采样间隔数,轨迹至少包含起点和终点两帧。
/// </summary>
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
{
var rawIntervals = durationSeconds / samplePeriodSeconds;
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
{
throw new InvalidOperationException("MovePose sample count is not representable.");
}
var intervals = (long)Math.Ceiling(Math.Max(0.0, rawIntervals) - 1e-9);
intervals = Math.Max(1, intervals);
if (intervals + 1 > MaxMovePoseSampleCount)
{
throw new InvalidOperationException($"MovePose sample count must be between 2 and {MaxMovePoseSampleCount}.");
}
return intervals;
}
/// <summary>
/// 构造单个轨迹行:[time_seconds,x,y,z,w,p,r]。
/// </summary>
private static IReadOnlyList<double> CreateRow(
double timeSeconds,
double durationSeconds,
IReadOnlyList<double> startPose,
IReadOnlyList<double> targetPose)
{
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
var scale = MoveJointTrajectoryGenerator.EvaluateSmoothPtpPositionScale(u);
var row = new double[7];
row[0] = Math.Round(timeSeconds, 9);
for (var index = 0; index < 6; index++)
{
row[index + 1] = startPose[index] + ((targetPose[index] - startPose[index]) * scale);
}
return row;
}
/// <summary>
/// 获取指定直角轴在当前速度倍率下的有效限制。
/// </summary>
private static CartesianAxisLimit GetAxisLimits(int index, double speedRatio)
{
var linearAxis = index < 3;
var velocity = linearAxis ? LinearVelocityLimit : AngularVelocityLimit;
var acceleration = linearAxis ? LinearAccelerationLimit : AngularAccelerationLimit;
var jerk = linearAxis ? LinearJerkLimit : AngularJerkLimit;
return new CartesianAxisLimit(
velocity * speedRatio,
acceleration * speedRatio * speedRatio,
jerk * speedRatio * speedRatio * speedRatio);
}
/// <summary>
/// 校验位姿向量必须为六维有限数。
/// </summary>
private static void EnsurePoseVector(IReadOnlyList<double> pose, string parameterName)
{
if (pose.Count != 6)
{
throw new ArgumentException("MovePose expects pose [x,y,z,w,p,r].", parameterName);
}
if (pose.Any(static value => double.IsNaN(value) || double.IsInfinity(value)))
{
throw new ArgumentException("MovePose pose values must be finite.", parameterName);
}
}
/// <summary>
/// 表示单个直角轴的有效速度、加速度和 jerk 限制。
/// </summary>
private readonly record struct CartesianAxisLimit(double Velocity, double Acceleration, double Jerk);
}

View File

@@ -24,7 +24,8 @@ public sealed class TrajectoryResult
int plannedWaypointCount, int plannedWaypointCount,
int triggerSampleIndexOffsetCycles = 0, int triggerSampleIndexOffsetCycles = 0,
IEnumerable<IReadOnlyList<double>>? denseJointTrajectory = null, IEnumerable<IReadOnlyList<double>>? denseJointTrajectory = null,
FlyshotPreparedExecution? preparedFlyshotExecution = null) FlyshotPreparedExecution? preparedFlyshotExecution = null,
IEnumerable<IReadOnlyList<double>>? denseCartesianTrajectory = null)
{ {
if (string.IsNullOrWhiteSpace(programName)) if (string.IsNullOrWhiteSpace(programName))
{ {
@@ -60,6 +61,7 @@ public sealed class TrajectoryResult
var copiedTriggerTimeline = triggerTimeline.ToArray(); var copiedTriggerTimeline = triggerTimeline.ToArray();
var copiedArtifacts = artifacts.ToArray(); var copiedArtifacts = artifacts.ToArray();
var copiedDenseJointTrajectory = denseJointTrajectory?.Select(static row => row.ToArray()).ToArray(); var copiedDenseJointTrajectory = denseJointTrajectory?.Select(static row => row.ToArray()).ToArray();
var copiedDenseCartesianTrajectory = denseCartesianTrajectory?.Select(static row => row.ToArray()).ToArray();
ProgramName = programName; ProgramName = programName;
Method = method; Method = method;
@@ -74,6 +76,7 @@ public sealed class TrajectoryResult
PlannedWaypointCount = plannedWaypointCount; PlannedWaypointCount = plannedWaypointCount;
TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles; TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles;
DenseJointTrajectory = copiedDenseJointTrajectory; DenseJointTrajectory = copiedDenseJointTrajectory;
DenseCartesianTrajectory = copiedDenseCartesianTrajectory;
PreparedFlyshotExecution = preparedFlyshotExecution; PreparedFlyshotExecution = preparedFlyshotExecution;
} }
@@ -156,6 +159,13 @@ public sealed class TrajectoryResult
[JsonPropertyName("denseJointTrajectory")] [JsonPropertyName("denseJointTrajectory")]
public IReadOnlyList<IReadOnlyList<double>>? DenseJointTrajectory { get; } public IReadOnlyList<IReadOnlyList<double>>? DenseJointTrajectory { get; }
/// <summary>
/// 获取稠密直角坐标轨迹采样点,每行格式为 [time, x, y, z, w, p, r]。
/// Null 表示本结果不是直角流式运动。
/// </summary>
[JsonPropertyName("denseCartesianTrajectory")]
public IReadOnlyList<IReadOnlyList<double>>? DenseCartesianTrajectory { get; }
/// <summary> /// <summary>
/// Gets the prepared flyshot execution queue when the flyshot chain has already built the final 8ms send sequence. /// Gets the prepared flyshot execution queue when the flyshot chain has already built the final 8ms send sequence.
/// </summary> /// </summary>

View File

@@ -113,4 +113,11 @@ public interface IControllerRuntime
/// <param name="result">规划结果。</param> /// <param name="result">规划结果。</param>
/// <param name="finalJointPositions">轨迹执行结束后的关节位置。</param> /// <param name="finalJointPositions">轨迹执行结束后的关节位置。</param>
void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions); void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions);
/// <summary>
/// 执行一条已经完成规划的直角坐标轨迹,并更新最终 TCP 位姿。
/// </summary>
/// <param name="result">规划结果,必须包含稠密直角坐标轨迹。</param>
/// <param name="finalPose">轨迹执行结束后的 [x,y,z,w,p,r] 位姿。</param>
void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose);
} }

View File

@@ -538,6 +538,78 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
} }
} }
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
ArgumentNullException.ThrowIfNull(result);
ArgumentNullException.ThrowIfNull(finalPose);
CancellationToken denseSendCancellationToken = default;
CancellationTokenSource? denseSendCancellationSource = null;
var shouldRunDenseTrajectory = false;
_logger?.LogInformation(
"ExecuteCartesianTrajectory 开始: program={ProgramName}, 时长={Duration}s, 稠密采样={HasDense}, speedRatio={SpeedRatio}",
result.ProgramName, result.Duration.TotalSeconds, result.DenseCartesianTrajectory is not null, _speedRatio);
lock (_stateLock)
{
EnsureEnabled();
EnsureValidTrajectory(result);
EnsurePoseCount(finalPose.Count);
CancelSendTaskLocked();
if (!IsSimulationMode && result.DenseCartesianTrajectory is not null)
{
EnsureJ519ReadyForDenseExecution();
// 真机直角模式同样预装完整队列,由机器人状态包节拍驱动出队。
_isInMotion = true;
_sendCts = new CancellationTokenSource();
denseSendCancellationSource = _sendCts;
denseSendCancellationToken = _sendCts.Token;
shouldRunDenseTrajectory = true;
_logger?.LogInformation("ExecuteCartesianTrajectory 开始同步直角稠密发送任务");
}
else
{
if (!IsSimulationMode)
{
var command = new FanucJ519Command(
sequence: 0,
targetValues: BuildCartesianTargetValues(finalPose),
dataStyle: 0);
_j519Client.UpdateCommand(command);
}
_isInMotion = true;
_pose = MergeFinalCartesianPose(finalPose);
_isInMotion = false;
_logger?.LogInformation("ExecuteCartesianTrajectory 完成(单点模式)");
}
}
if (shouldRunDenseTrajectory)
{
try
{
SendDenseCartesianTrajectory(result, finalPose, denseSendCancellationToken);
_logger?.LogInformation("ExecuteCartesianTrajectory 完成(稠密模式)");
}
finally
{
lock (_stateLock)
{
if (ReferenceEquals(_sendCts, denseSendCancellationSource))
{
denseSendCancellationSource?.Dispose();
_sendCts = null;
}
}
}
}
}
/// <summary> /// <summary>
/// 释放运行时持有的所有 Socket 客户端。 /// 释放运行时持有的所有 Socket 客户端。
/// </summary> /// </summary>
@@ -759,6 +831,71 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
} }
} }
/// <summary>
/// 直角稠密轨迹发送任务:预生成 Data format=0 的 J519 命令队列,并等待状态包驱动执行完成。
/// </summary>
private void SendDenseCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose, CancellationToken cancellationToken)
{
var rows = result.DenseCartesianTrajectory ?? throw new InvalidOperationException("Cartesian trajectory requires dense Cartesian samples.");
var commands = new List<FanucJ519Command>(rows.Count);
var sentPoseRows = new List<IReadOnlyList<double>>(rows.Count);
var outputDir = CreateDenseSendOutputDirectory(result.ProgramName);
var stopwatch = Stopwatch.StartNew();
try
{
foreach (var row in rows)
{
cancellationToken.ThrowIfCancellationRequested();
if (row.Count != 7)
{
throw new InvalidOperationException("Cartesian dense trajectory rows must contain time plus six pose values.");
}
var pose = row.Skip(1).Take(6).ToArray();
var command = new FanucJ519Command(
sequence: 0,
targetValues: BuildCartesianTargetValues(pose),
dataStyle: 0,
writeIoType: 2,
writeIoIndex: 1,
writeIoMask: 0,
writeIoValue: 0);
commands.Add(command);
sentPoseRows.Add(BuildDenseSendPoseRow(row[0], pose));
}
TryWriteDenseCartesianSendArtifacts(outputDir, sentPoseRows);
_j519Client.LoadCommandQueue(commands);
if (_j519Client.IsConnected)
{
_j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult();
}
_logger?.LogInformation(
"SendDenseCartesianTrajectory 正常完成: 采样数={SampleCount}, 队列装载耗时={ElapsedMs}ms",
commands.Count,
stopwatch.ElapsedMilliseconds);
}
catch (OperationCanceledException)
{
_logger?.LogWarning(
"SendDenseCartesianTrajectory 被取消: 已生成 {Current}/{Total} 条命令",
commands.Count,
rows.Count);
}
finally
{
lock (_stateLock)
{
_isInMotion = false;
_pose = MergeFinalCartesianPose(finalPose);
}
}
}
/// <summary> /// <summary>
/// 若已有 J519 响应,则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。 /// 若已有 J519 响应,则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
/// </summary> /// </summary>
@@ -879,6 +1016,41 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
return row; return row;
} }
/// <summary>
/// 构造实际发送直角位姿文本行,格式为 send_time + X/Y/Z/W/P/R。
/// </summary>
private static IReadOnlyList<double> BuildDenseSendPoseRow(double sendTime, IReadOnlyList<double> pose)
{
var row = new double[pose.Count + 1];
row[0] = Math.Round(sendTime, 6);
for (var index = 0; index < pose.Count; index++)
{
row[index + 1] = Math.Round(pose[index], 6);
}
return row;
}
/// <summary>
/// 构造 J519 直角命令的 9 个目标槽位,当前现场无扩展轴时 E1/E2/E3 补零。
/// </summary>
private static double[] BuildCartesianTargetValues(IReadOnlyList<double> pose)
{
EnsurePoseCount(pose.Count);
return [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], 0.0, 0.0, 0.0];
}
/// <summary>
/// 将六维 WPR 直角目标合并回运行时缓存,保留旧 GetPose 仿真路径的 7 维外形。
/// </summary>
private double[] MergeFinalCartesianPose(IReadOnlyList<double> pose)
{
EnsurePoseCount(pose.Count);
return _pose.Length >= 7
? [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], _pose[6]]
: pose.ToArray();
}
/// <summary> /// <summary>
/// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件;若落盘失败,只记录日志,不影响运动主流程。 /// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件;若落盘失败,只记录日志,不影响运动主流程。
/// </summary> /// </summary>
@@ -910,6 +1082,25 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
} }
} }
/// <summary>
/// 尝试把直角实际发送点位写入文本文件;若落盘失败,只记录日志,不影响运动主流程。
/// </summary>
private void TryWriteDenseCartesianSendArtifacts(string outputDir, IReadOnlyList<IReadOnlyList<double>> sentPoseRows)
{
try
{
WriteDenseRows(Path.Combine(outputDir, "ActualSendPoseTraj.txt"), sentPoseRows);
_logger?.LogInformation(
"SendDenseCartesianTrajectory 已写出实际发送记录: outputDir={OutputDir}, poseRows={PoseRows}",
outputDir,
sentPoseRows.Count);
}
catch (Exception exception)
{
_logger?.LogWarning(exception, "SendDenseCartesianTrajectory 写出实际发送记录失败: outputDir={OutputDir}", outputDir);
}
}
/// <summary> /// <summary>
/// 以旧轨迹文本兼容的空格分隔格式写出数值行。 /// 以旧轨迹文本兼容的空格分隔格式写出数值行。
/// </summary> /// </summary>
@@ -1089,6 +1280,17 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
} }
} }
/// <summary>
/// 校验直角位姿固定为 [x,y,z,w,p,r] 六维。
/// </summary>
private static void EnsurePoseCount(int poseCount)
{
if (poseCount != 6)
{
throw new InvalidOperationException($"Expected 6 Cartesian pose values but received {poseCount}.");
}
}
/// <summary> /// <summary>
/// 校验机器人已经完成初始化。 /// 校验机器人已经完成初始化。
/// </summary> /// </summary>

View File

@@ -7,7 +7,7 @@ namespace Flyshot.Runtime.Fanuc.Protocol;
/// </summary> /// </summary>
public sealed class FanucJ519Command public sealed class FanucJ519Command
{ {
private readonly double[] _targetJoints; private readonly double[] _targetValues;
/// <summary> /// <summary>
/// 初始化 J519 命令数据。 /// 初始化 J519 命令数据。
@@ -35,11 +35,53 @@ public sealed class FanucJ519Command
ushort writeIoIndex = 1, ushort writeIoIndex = 1,
ushort writeIoMask = 0, ushort writeIoMask = 0,
ushort writeIoValue = 0) ushort writeIoValue = 0)
: this(
sequence,
targetValues: targetJoints,
lastData,
readIoType,
readIoIndex,
readIoMask,
dataStyle,
writeIoType,
writeIoIndex,
writeIoMask,
writeIoValue)
{ {
ArgumentNullException.ThrowIfNull(targetJoints); }
if (targetJoints.Count is <= 0 or > 9)
/// <summary>
/// 初始化 J519 命令数据。
/// </summary>
/// <param name="sequence">命令序号。</param>
/// <param name="targetValues">J519 目标槽位,直角模式为 X/Y/Z/W/P/R/E1/E2/E3关节模式为 J1..J9。</param>
/// <param name="lastData">是否为最后一帧数据。</param>
/// <param name="readIoType">读取 IO 类型。</param>
/// <param name="readIoIndex">读取 IO 起始索引。</param>
/// <param name="readIoMask">读取 IO 掩码。</param>
/// <param name="dataStyle">目标数据类型0 为直角坐标1 为关节坐标。</param>
/// <param name="writeIoType">写入 IO 类型。</param>
/// <param name="writeIoIndex">写入 IO 起始索引。</param>
/// <param name="writeIoMask">写入 IO 掩码。</param>
/// <param name="writeIoValue">写入 IO 数值。</param>
public FanucJ519Command(
uint sequence,
IEnumerable<double> targetValues,
byte lastData = 0,
byte readIoType = 2,
ushort readIoIndex = 1,
ushort readIoMask = 255,
byte dataStyle = 1,
byte writeIoType = 2,
ushort writeIoIndex = 1,
ushort writeIoMask = 0,
ushort writeIoValue = 0)
{
ArgumentNullException.ThrowIfNull(targetValues);
var copiedTargetValues = targetValues.ToArray();
if (copiedTargetValues.Length is <= 0 or > 9)
{ {
throw new ArgumentOutOfRangeException(nameof(targetJoints), "J519 目标数据必须包含 1 到 9 个槽位。"); throw new ArgumentOutOfRangeException(nameof(targetValues), "J519 目标数据必须包含 1 到 9 个槽位。");
} }
Sequence = sequence; Sequence = sequence;
@@ -52,7 +94,7 @@ public sealed class FanucJ519Command
WriteIoIndex = writeIoIndex; WriteIoIndex = writeIoIndex;
WriteIoMask = writeIoMask; WriteIoMask = writeIoMask;
WriteIoValue = writeIoValue; WriteIoValue = writeIoValue;
_targetJoints = targetJoints.ToArray(); _targetValues = copiedTargetValues;
} }
/// <summary> /// <summary>
@@ -108,7 +150,12 @@ public sealed class FanucJ519Command
/// <summary> /// <summary>
/// 获取目标关节或扩展轴数据。 /// 获取目标关节或扩展轴数据。
/// </summary> /// </summary>
public IReadOnlyList<double> TargetJoints => _targetJoints; public IReadOnlyList<double> TargetJoints => _targetValues;
/// <summary>
/// 获取 J519 目标槽位,直角模式为 X/Y/Z/W/P/R/E1/E2/E3关节模式为 J1..J9。
/// </summary>
public IReadOnlyList<double> TargetValues => _targetValues;
} }
/// <summary> /// <summary>

View File

@@ -431,6 +431,32 @@ public sealed class LegacyHttpApiController : ControllerBase
} }
} }
/// <summary>
/// 以直角坐标 `[x,y,z,w,p,r]` 执行点到点移动。
/// </summary>
/// <param name="pose_data">直角位姿请求体。</param>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpPost("/move_pose/")]
public IActionResult MovePose([FromBody] JsonElement pose_data)
{
try
{
var poseRequest = LegacyCartesianPoseRequest.FromJson(pose_data);
var pose = poseRequest.ToPoseArray();
_logger.LogInformation("MovePose 调用: x={X}, y={Y}, z={Z}, w={W}, p={P}, r={R}",
poseRequest.x, poseRequest.y, poseRequest.z, poseRequest.w, poseRequest.p, poseRequest.r);
_compatService.MovePose(pose);
_logger.LogInformation("MovePose 成功");
return Ok(new { status = "robot moved" });
}
catch (Exception exception)
{
_logger.LogError(exception, "MovePose 失败");
return LegacyBadRequest("MovePose failed");
}
}
/// <summary> /// <summary>
/// 兼容旧 `GetNearestIK(pose, seed, ik)` 参数形状。 /// 兼容旧 `GetNearestIK(pose, seed, ik)` 参数形状。
/// </summary> /// </summary>
@@ -874,6 +900,136 @@ public sealed class LegacyJointPositionRequest
public List<double> joints { get; init; } = []; public List<double> joints { get; init; } = [];
} }
/// <summary>
/// 表示 `/move_pose/` 路由使用的直角位姿请求体。
/// </summary>
public sealed class LegacyCartesianPoseRequest
{
/// <summary>
/// MovePose 第一版入口硬限制TCP X/Y 最大绝对值,单位 mm。
/// </summary>
private const double MaxHorizontalMillimeters = 1000.0;
/// <summary>
/// MovePose 第一版入口硬限制TCP Z 最小值,单位 mm。
/// </summary>
private const double MinZMillimeters = 0.0;
/// <summary>
/// MovePose 第一版入口硬限制TCP Z 最大值,单位 mm。
/// </summary>
private const double MaxZMillimeters = 1200.0;
/// <summary>
/// MovePose 第一版入口硬限制W/R 姿态角最大绝对值,单位 deg。
/// </summary>
private const double MaxRollYawDegrees = 180.0;
/// <summary>
/// MovePose 第一版入口硬限制P 姿态角最大绝对值,单位 deg。
/// </summary>
private const double MaxPitchDegrees = 90.0;
/// <summary>
/// 获取或设置 TCP X单位为 mm。
/// </summary>
public double x { get; init; }
/// <summary>
/// 获取或设置 TCP Y单位为 mm。
/// </summary>
public double y { get; init; }
/// <summary>
/// 获取或设置 TCP Z单位为 mm。
/// </summary>
public double z { get; init; }
/// <summary>
/// 获取或设置姿态 W单位为 deg。
/// </summary>
public double w { get; init; }
/// <summary>
/// 获取或设置姿态 P单位为 deg。
/// </summary>
public double p { get; init; }
/// <summary>
/// 获取或设置姿态 R单位为 deg。
/// </summary>
public double r { get; init; }
/// <summary>
/// 从原始 JSON 请求体解析直角位姿并显式拒绝缺字段、null 和非有限数。
/// </summary>
/// <param name="json">原始 JSON 请求体。</param>
/// <returns>解析后的直角位姿请求。</returns>
public static LegacyCartesianPoseRequest FromJson(JsonElement json)
{
if (json.ValueKind != JsonValueKind.Object)
{
throw new ArgumentException("MovePose request body must be an object.");
}
// 旧接口要求请求体必须完整提供 x/y/z/w/p/r不能让模型绑定把缺字段静默补 0。
return new LegacyCartesianPoseRequest
{
x = ReadRequiredFiniteDouble(json, "x", -MaxHorizontalMillimeters, MaxHorizontalMillimeters),
y = ReadRequiredFiniteDouble(json, "y", -MaxHorizontalMillimeters, MaxHorizontalMillimeters),
z = ReadRequiredFiniteDouble(json, "z", MinZMillimeters, MaxZMillimeters),
w = ReadRequiredFiniteDouble(json, "w", -MaxRollYawDegrees, MaxRollYawDegrees),
p = ReadRequiredFiniteDouble(json, "p", -MaxPitchDegrees, MaxPitchDegrees),
r = ReadRequiredFiniteDouble(json, "r", -MaxRollYawDegrees, MaxRollYawDegrees)
};
}
/// <summary>
/// 转换为兼容层使用的六维位姿数组。
/// </summary>
/// <returns>[x,y,z,w,p,r] 数组。</returns>
public IReadOnlyList<double> ToPoseArray()
{
return [x, y, z, w, p, r];
}
/// <summary>
/// 读取必填有限数值字段。
/// </summary>
/// <param name="json">请求体 JSON 对象。</param>
/// <param name="propertyName">字段名。</param>
/// <param name="minInclusive">允许的最小值。</param>
/// <param name="maxInclusive">允许的最大值。</param>
/// <returns>字段对应的有限 double 数值。</returns>
private static double ReadRequiredFiniteDouble(
JsonElement json,
string propertyName,
double minInclusive,
double maxInclusive)
{
if (!json.TryGetProperty(propertyName, out var property) || property.ValueKind != JsonValueKind.Number)
{
throw new ArgumentException($"MovePose request field '{propertyName}' is required and must be a number.");
}
var value = property.GetDouble();
if (double.IsNaN(value) || double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(propertyName, "MovePose request values must be finite.");
}
if (value < minInclusive || value > maxInclusive)
{
throw new ArgumentOutOfRangeException(
propertyName,
value,
$"MovePose request field '{propertyName}' must be between {minInclusive} and {maxInclusive}.");
}
return value;
}
}
/// <summary> /// <summary>
/// 表示旧 `/upload_flyshot/` 路由使用的飞拍上传请求体。 /// 表示旧 `/upload_flyshot/` 路由使用的飞拍上传请求体。
/// </summary> /// </summary>

View File

@@ -49,23 +49,18 @@ public sealed class StatusController : ControllerBase
public IActionResult GetSnapshot() public IActionResult GetSnapshot()
{ {
var snapshot = _compatService.GetControllerSnapshot(); var snapshot = _compatService.GetControllerSnapshot();
var isSetup = _compatService.IsSetUp; var metadata = _compatService.GetStatusSnapshotMetadata();
// 状态页需要在机器人未初始化时仍能打开,因此只有初始化后才读取机器人元数据。
var robotName = isSetup ? _compatService.GetRobotName() : null;
var degreesOfFreedom = isSetup ? _compatService.GetDegreesOfFreedom() : 0;
var uploadedTrajectories = isSetup ? _compatService.ListTrajectoryNames() : Array.Empty<string>();
return Ok(new return Ok(new
{ {
Status = "ok", Status = "ok",
Service = "flyshot-server-host", Service = "flyshot-server-host",
ServerVersion = _compatService.GetServerVersion(), ServerVersion = metadata.ServerVersion,
ClientVersion = _compatService.GetClientVersion(), ClientVersion = metadata.ClientVersion,
IsSetup = isSetup, IsSetup = metadata.IsSetup,
RobotName = robotName, RobotName = metadata.RobotName,
DegreesOfFreedom = degreesOfFreedom, DegreesOfFreedom = metadata.DegreesOfFreedom,
UploadedTrajectories = uploadedTrajectories, UploadedTrajectories = metadata.UploadedTrajectories,
Snapshot = snapshot Snapshot = snapshot
}); });
} }

View File

@@ -16,6 +16,17 @@ const state = {
history: [] history: []
}; };
const requestBodySamples = {
"POST /move_pose/": {
x: 100.0,
y: 200.0,
z: 300.0,
w: 0.0,
p: 45.0,
r: 0.0
}
};
/** 简单的 escape把任意字符串安全嵌入 textContent 之外的位置时使用。 */ /** 简单的 escape把任意字符串安全嵌入 textContent 之外的位置时使用。 */
function escapeHtml(value) { function escapeHtml(value) {
return String(value).replace(/[&<>"']/g, function (ch) { return String(value).replace(/[&<>"']/g, function (ch) {
@@ -137,6 +148,11 @@ function storageKey(op) {
return STORAGE_PREFIX + op.method + ":" + op.path; return STORAGE_PREFIX + op.method + ":" + op.path;
} }
/** 返回指定端点的手工调试样例,处理 JsonElement 等 OpenAPI 无法自动推断字段的接口。 */
function getRequestBodySample(op) {
return requestBodySamples[op.method + " " + op.path] || null;
}
/** 读取本端点最近一次输入;解析失败则当作空。 */ /** 读取本端点最近一次输入;解析失败则当作空。 */
function loadInputs(op) { function loadInputs(op) {
try { try {
@@ -281,7 +297,7 @@ function renderBodyEditor(container, op, savedBody) {
if (savedBody !== undefined && savedBody !== null) { if (savedBody !== undefined && savedBody !== null) {
initialText = typeof savedBody === "string" ? savedBody : JSON.stringify(savedBody, null, 2); initialText = typeof savedBody === "string" ? savedBody : JSON.stringify(savedBody, null, 2);
} else { } else {
const sample = buildSampleFromSchema(op.bodySchema, 0); const sample = getRequestBodySample(op) || buildSampleFromSchema(op.bodySchema, 0);
initialText = sample === null ? "" : JSON.stringify(sample, null, 2); initialText = sample === null ? "" : JSON.stringify(sample, null, 2);
} }
textarea.value = initialText; textarea.value = initialText;

View File

@@ -188,6 +188,68 @@ dd {
font-family: inherit; font-family: inherit;
} }
.jog-panel {
grid-column: 1 / -1;
}
.jog-content {
padding: 16px;
}
.jog-settings {
display: grid;
grid-template-columns: repeat(2, minmax(180px, 1fr));
gap: 12px;
margin-bottom: 14px;
}
.jog-settings label {
display: grid;
gap: 6px;
color: var(--muted);
font-size: 13px;
}
.jog-settings input {
min-height: 36px;
width: 100%;
padding: 0 10px;
border: 1px solid var(--line);
border-radius: 6px;
background: #ffffff;
color: var(--text);
font: inherit;
}
.jog-grid {
display: grid;
grid-template-columns: repeat(6, minmax(64px, 1fr));
gap: 8px;
}
.jog-button {
min-height: 44px;
padding: 0 8px;
font-weight: 650;
touch-action: none;
user-select: none;
}
.jog-button.active {
filter: brightness(0.88);
}
.jog-status {
min-height: 24px;
margin-top: 12px;
color: var(--muted);
font-family: Consolas, "Cascadia Mono", monospace;
}
.jog-status.error {
color: var(--bad);
}
@media (max-width: 820px) { @media (max-width: 820px) {
.topbar { .topbar {
align-items: flex-start; align-items: flex-start;
@@ -199,6 +261,11 @@ dd {
grid-template-columns: 1fr; grid-template-columns: 1fr;
} }
.jog-settings,
.jog-grid {
grid-template-columns: repeat(2, minmax(0, 1fr));
}
dl { dl {
grid-template-columns: 1fr; grid-template-columns: 1fr;
} }

View File

@@ -15,7 +15,28 @@ const fields = {
joints: document.getElementById("joints"), joints: document.getElementById("joints"),
pose: document.getElementById("pose"), pose: document.getElementById("pose"),
trajectories: document.getElementById("trajectories"), trajectories: document.getElementById("trajectories"),
refresh: document.getElementById("refresh") refresh: document.getElementById("refresh"),
linearStep: document.getElementById("linear-step"),
angularStep: document.getElementById("angular-step"),
jogStatus: document.getElementById("jog-status"),
jogButtons: Array.from(document.querySelectorAll(".jog-button"))
};
const axisIndexes = {
x: 0,
y: 1,
z: 2,
w: 3,
p: 4,
r: 5
};
// 点动状态集中保存,确保按住按钮时不会并发发送多条 MovePose 请求。
const jogState = {
timer: null,
activeButton: null,
inFlight: false,
lastSnapshot: null
}; };
function formatArray(values) { function formatArray(values) {
@@ -56,12 +77,136 @@ function setDot(connectionState) {
} }
} }
function clampStep(input, min, max) {
const value = Number(input.value);
if (!Number.isFinite(value)) {
input.value = String(min);
return min;
}
const clamped = Math.min(Math.max(value, min), max);
input.value = String(clamped);
return clamped;
}
function getStepForAxis(axis) {
// 平移和姿态使用不同单位,但共享 0.1 到 10 的现场可调范围。
return axis === "x" || axis === "y" || axis === "z"
? clampStep(fields.linearStep, 0.1, 10)
: clampStep(fields.angularStep, 0.1, 10);
}
function setJogStatus(message, isError) {
fields.jogStatus.textContent = message;
fields.jogStatus.classList.toggle("error", Boolean(isError));
}
async function loadSnapshotForJog() {
const response = await fetch("/api/status/snapshot", { cache: "no-store" });
if (!response.ok) {
throw new Error(`状态快照读取失败: HTTP ${response.status}`);
}
const payload = await response.json();
jogState.lastSnapshot = payload.snapshot;
return payload.snapshot;
}
function buildJogPose(snapshot, axis, direction) {
// 每次点动都从最新 TCP 位姿出发,只修改一个轴,避免连续按压时累积本地误差。
const sourcePose = Array.isArray(snapshot.cartesianPose) ? snapshot.cartesianPose : [];
if (sourcePose.length < 6) {
throw new Error("当前 TCP 位姿不足 6 维,无法点动。");
}
const pose = sourcePose.slice(0, 6).map(Number);
if (pose.some(value => !Number.isFinite(value))) {
throw new Error("当前 TCP 位姿包含非数值,无法点动。");
}
const axisIndex = axisIndexes[axis];
pose[axisIndex] = Number((pose[axisIndex] + direction * getStepForAxis(axis)).toFixed(6));
return {
x: pose[0],
y: pose[1],
z: pose[2],
w: pose[3],
p: pose[4],
r: pose[5]
};
}
async function sendJog(button) {
// MovePose 底层会生成完整直角轨迹;前端这里只负责构造增量目标位姿。
if (jogState.inFlight) {
return;
}
const axis = button.dataset.axis;
const direction = Number(button.dataset.direction);
if (!Object.prototype.hasOwnProperty.call(axisIndexes, axis) || !Number.isFinite(direction)) {
return;
}
jogState.inFlight = true;
try {
const snapshot = await loadSnapshotForJog();
const pose = buildJogPose(snapshot, axis, direction);
const response = await fetch("/move_pose/", {
method: "POST",
headers: { "Content-Type": "application/json" },
body: JSON.stringify(pose)
});
if (!response.ok) {
throw new Error(`MovePose 调用失败: HTTP ${response.status}`);
}
setJogStatus(`${axis.toUpperCase()}${direction > 0 ? "+" : "-"} 已发送`, false);
await refreshStatus();
} catch (error) {
setJogStatus(error instanceof Error ? error.message : "点动失败", true);
stopJog();
} finally {
jogState.inFlight = false;
}
}
function startJog(event) {
const button = event.currentTarget;
if (!button || button.disabled) {
return;
}
event.preventDefault();
stopJog();
jogState.activeButton = button;
button.classList.add("active");
sendJog(button);
jogState.timer = window.setInterval(function () {
sendJog(button);
}, 250);
}
function stopJog() {
if (jogState.timer !== null) {
window.clearInterval(jogState.timer);
jogState.timer = null;
}
if (jogState.activeButton) {
jogState.activeButton.classList.remove("active");
jogState.activeButton = null;
}
}
async function refreshStatus() { async function refreshStatus() {
fields.refresh.disabled = true; fields.refresh.disabled = true;
try { try {
const response = await fetch("/api/status/snapshot", { cache: "no-store" }); const response = await fetch("/api/status/snapshot", { cache: "no-store" });
const payload = await response.json(); const payload = await response.json();
const snapshot = payload.snapshot; const snapshot = payload.snapshot;
jogState.lastSnapshot = snapshot;
fields.connectionState.textContent = snapshot.connectionState; fields.connectionState.textContent = snapshot.connectionState;
fields.robotName.textContent = payload.robotName || "--"; fields.robotName.textContent = payload.robotName || "--";
@@ -88,5 +233,23 @@ async function refreshStatus() {
} }
fields.refresh.addEventListener("click", refreshStatus); fields.refresh.addEventListener("click", refreshStatus);
fields.linearStep.addEventListener("change", function () {
clampStep(fields.linearStep, 0.1, 10);
});
fields.angularStep.addEventListener("change", function () {
clampStep(fields.angularStep, 0.1, 10);
});
fields.jogButtons.forEach(function (button) {
button.addEventListener("pointerdown", startJog);
button.addEventListener("pointerup", stopJog);
button.addEventListener("pointercancel", stopJog);
button.addEventListener("pointerleave", stopJog);
});
window.addEventListener("blur", stopJog);
window.addEventListener("keyup", function (event) {
if (event.key === "Escape") {
stopJog();
}
});
refreshStatus(); refreshStatus();
window.setInterval(refreshStatus, 2000); window.setInterval(refreshStatus, 2000);

View File

@@ -57,6 +57,36 @@
<dt>已上传轨迹</dt><dd id="trajectories" class="empty">--</dd> <dt>已上传轨迹</dt><dd id="trajectories" class="empty">--</dd>
</dl> </dl>
</section> </section>
<section class="jog-panel">
<h2>直角坐标点动</h2>
<div class="jog-content">
<div class="jog-settings">
<label for="linear-step">
<span>平移步长 mm</span>
<input id="linear-step" type="number" min="0.1" max="10" step="0.1" value="1">
</label>
<label for="angular-step">
<span>姿态步长 deg</span>
<input id="angular-step" type="number" min="0.1" max="10" step="0.1" value="1">
</label>
</div>
<div class="jog-grid" aria-label="直角坐标点动按钮">
<button class="jog-button" type="button" data-axis="x" data-direction="-1">X-</button>
<button class="jog-button" type="button" data-axis="x" data-direction="1">X+</button>
<button class="jog-button" type="button" data-axis="y" data-direction="-1">Y-</button>
<button class="jog-button" type="button" data-axis="y" data-direction="1">Y+</button>
<button class="jog-button" type="button" data-axis="z" data-direction="-1">Z-</button>
<button class="jog-button" type="button" data-axis="z" data-direction="1">Z+</button>
<button class="jog-button" type="button" data-axis="w" data-direction="-1">W-</button>
<button class="jog-button" type="button" data-axis="w" data-direction="1">W+</button>
<button class="jog-button" type="button" data-axis="p" data-direction="-1">P-</button>
<button class="jog-button" type="button" data-axis="p" data-direction="1">P+</button>
<button class="jog-button" type="button" data-axis="r" data-direction="-1">R-</button>
<button class="jog-button" type="button" data-axis="r" data-direction="1">R+</button>
</div>
<div id="jog-status" class="jog-status">--</div>
</div>
</section>
</div> </div>
</main> </main>
<script src="/assets/status.js" defer></script> <script src="/assets/status.js" defer></script>

View File

@@ -555,6 +555,30 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.5)); 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] [Fact]
public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming() public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming()
{ {
@@ -578,6 +602,41 @@ public sealed class FanucControllerRuntimeDenseTests
AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints); 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> /// <summary>
/// 验证运行时稠密发送不再依赖当前 speed_ratio倍率合法性应在上游规划/生成阶段处理。 /// 验证运行时稠密发送不再依赖当前 speed_ratio倍率合法性应在上游规划/生成阶段处理。
/// </summary> /// </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> /// <summary>
/// 创建用于就绪状态测试的最小 J519 响应。 /// 创建用于就绪状态测试的最小 J519 响应。
/// </summary> /// </summary>

View File

@@ -163,6 +163,28 @@ public sealed class FanucJ519ClientTests : IDisposable
await client.StopMotionAsync(_cts.Token); 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> /// <summary>
/// 验证配置 J519 buffer size 后,实际回发命令序号会在状态包序号基础上增加该缓冲深度。 /// 验证配置 J519 buffer size 后,实际回发命令序号会在状态包序号基础上增加该缓冲深度。
/// </summary> /// </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> /// <summary>
/// 验证飞拍链路在进入运行时前就会准备最终发送队列,而不是把 speedRatio 重采样留给运行时临场处理。 /// 验证飞拍链路在进入运行时前就会准备最终发送队列,而不是把 speedRatio 重采样留给运行时临场处理。
/// </summary> /// </summary>
@@ -2005,6 +2110,12 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
{ {
LastExecutedResult = result; LastExecutedResult = result;
} }
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
LastExecutedResult = result;
}
} }
/// <summary> /// <summary>
@@ -2166,6 +2277,182 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
_jointPositions = finalJointPositions.ToArray(); _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> /// <summary>
@@ -2292,4 +2579,8 @@ internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime
_jointPositions = finalJointPositions.ToArray(); _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); 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> /// <summary>
/// 当 Swagger 启用时,调试配置 API 应当返回实际 Swagger JSON 地址。 /// 当 Swagger 启用时,调试配置 API 应当返回实际 Swagger JSON 地址。
/// </summary> /// </summary>

View File

@@ -1,5 +1,6 @@
using System.Net; using System.Net;
using System.Net.Http.Json; using System.Net.Http.Json;
using System.Text;
using System.Text.Json; using System.Text.Json;
namespace Flyshot.Server.IntegrationTests; namespace Flyshot.Server.IntegrationTests;
@@ -131,6 +132,13 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
Assert.Equal("robot moved", moveJointJson.RootElement.GetProperty("status").GetString()); 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/")) using (var getJointPositionResponse = await client.GetAsync("/get_joint_position/"))
{ {
Assert.Equal(HttpStatusCode.OK, getJointPositionResponse.StatusCode); 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> /// <summary>
/// 验证飞拍 HTTP 接口可以按旧 API 层的路径和字段完成上传、列出、执行与删除。 /// 验证飞拍 HTTP 接口可以按旧 API 层的路径和字段完成上传、列出、执行与删除。
/// </summary> /// </summary>

View File

@@ -34,6 +34,39 @@ public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFi
Assert.Contains("/api/status/snapshot", script, StringComparison.Ordinal); 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> /// <summary>
/// 验证状态快照 API 会返回运行时连接、使能、速度和机器人元数据。 /// 验证状态快照 API 会返回运行时连接、使能、速度和机器人元数据。
/// </summary> /// </summary>