✨ feat(fanuc): 添加直角坐标点动功能与相关接口
* 新增 `MovePose` 方法,支持以直角坐标执行点到点移动。 * 引入 `LegacyCartesianPoseRequest` 类,处理直角位姿请求体的解析与验证。 * 更新 `LegacyHttpApiController`,实现 `/move_pose/` 路由以支持新功能。 * 增强状态快照元数据,提供机器人初始化状态与已上传轨迹信息。 * 更新前端状态页面,增加直角坐标点动控制面板与步长设置选项。 * 相关文档与测试用例同步更新,确保新功能的完整性与稳定性。
This commit is contained in:
200
analysis/read_fanuc_allowable_limits.py
Normal file
200
analysis/read_fanuc_allowable_limits.py
Normal 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
BIN
docs/0.7x.pcap
Normal file
Binary file not shown.
@@ -12,6 +12,7 @@ namespace Flyshot.ControllerClientCompat;
|
||||
public sealed class ControllerClientCompatService : IControllerClientCompatService
|
||||
{
|
||||
private readonly object _stateLock = new();
|
||||
private readonly object _motionLock = new();
|
||||
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _uploadedTrajectories = new(StringComparer.Ordinal);
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly ControllerClientCompatRobotCatalog _robotCatalog;
|
||||
@@ -197,8 +198,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetActiveController(sim);
|
||||
}
|
||||
|
||||
_runtime.SetActiveController(sim);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -214,9 +216,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.Connect(robotIp);
|
||||
}
|
||||
|
||||
_runtime.Connect(robotIp);
|
||||
|
||||
_logger?.LogInformation("Connect 完成: robotIp={RobotIp}", robotIp);
|
||||
}
|
||||
|
||||
@@ -226,8 +229,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.Disconnect();
|
||||
}
|
||||
|
||||
_runtime.Disconnect();
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -237,8 +241,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.EnableRobot(bufferSize);
|
||||
}
|
||||
_runtime.EnableRobot(bufferSize);
|
||||
_logger?.LogInformation("EnableRobot 完成");
|
||||
}
|
||||
|
||||
@@ -249,8 +253,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.DisableRobot();
|
||||
}
|
||||
|
||||
// DisableRobot 是中断/控制命令,不能被长时间运动串行锁挡住。
|
||||
_runtime.DisableRobot();
|
||||
_logger?.LogInformation("DisableRobot 完成");
|
||||
}
|
||||
|
||||
@@ -261,8 +267,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.StopMove();
|
||||
}
|
||||
|
||||
// StopMove 必须能在运动执行期间直接进入 runtime 取消发送任务。
|
||||
_runtime.StopMove();
|
||||
_logger?.LogInformation("StopMove 完成");
|
||||
}
|
||||
|
||||
@@ -272,14 +280,31 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
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 />
|
||||
public double GetSpeedRatio()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetSpeedRatio();
|
||||
}
|
||||
|
||||
return _runtime.GetSpeedRatio();
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -288,8 +313,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetSpeedRatio(ratio);
|
||||
}
|
||||
|
||||
_runtime.SetSpeedRatio(ratio);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -298,8 +324,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetIo(port, value, ioType);
|
||||
}
|
||||
|
||||
_runtime.SetIo(port, value, ioType);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -308,8 +335,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetIo(port, ioType);
|
||||
}
|
||||
|
||||
return _runtime.GetIo(port, ioType);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -341,8 +369,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetTcp(x, y, z);
|
||||
}
|
||||
|
||||
_runtime.SetTcp(x, y, z);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -351,8 +380,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetTcp();
|
||||
}
|
||||
|
||||
return _runtime.GetTcp();
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -361,8 +391,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetJointPositions();
|
||||
}
|
||||
|
||||
return _runtime.GetJointPositions();
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -373,16 +404,44 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
_logger?.LogInformation("MoveJoint 开始: 目标关节数={JointCount}", jointPositions.Count);
|
||||
_logger?.LogDebug("MoveJoint 目标关节: {Joints}", string.Join(", ", jointPositions.Select(j => j.ToString("F4"))));
|
||||
|
||||
RobotProfile robot;
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint");
|
||||
}
|
||||
|
||||
lock (_motionLock)
|
||||
{
|
||||
ExecuteMoveJointAndWait(robot, jointPositions, "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 />
|
||||
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
|
||||
{
|
||||
@@ -398,13 +457,19 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
_logger?.LogDebug("ExecuteTrajectory 路点详情: {Waypoints}",
|
||||
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Select(j => j.ToString("F4")))}]")));
|
||||
|
||||
RobotProfile robot;
|
||||
CompatibilityRobotSettings settings;
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
settings = RequireRobotSettings();
|
||||
}
|
||||
|
||||
lock (_motionLock)
|
||||
{
|
||||
// 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。
|
||||
var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
|
||||
var planningSpeedScale = settings.PlanningSpeedScale;
|
||||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||||
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale, speedRatio);
|
||||
_logger?.LogInformation(
|
||||
@@ -428,8 +493,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetPose();
|
||||
}
|
||||
|
||||
return _runtime.GetPose();
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
@@ -443,16 +509,20 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
trajectory.Waypoints.Count,
|
||||
trajectory.ShotFlags.Count(static f => f));
|
||||
|
||||
string robotName;
|
||||
CompatibilityRobotSettings settings;
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRuntimeEnabled();
|
||||
_uploadedTrajectories[trajectory.Name] = trajectory;
|
||||
|
||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||||
_trajectoryStore.Save(robotName, settings, trajectory);
|
||||
robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||||
}
|
||||
|
||||
// RobotConfig.json 持久化是文件 I/O,放在状态锁外,避免状态页轮询被磁盘写入拖住。
|
||||
_trajectoryStore.Save(robotName, settings, trajectory);
|
||||
|
||||
_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}",
|
||||
name, options.Method, options.MoveToStart, options.UseCache, options.Wait);
|
||||
|
||||
RobotProfile robot;
|
||||
CompatibilityRobotSettings settings;
|
||||
ControllerClientCompatUploadedTrajectory trajectory;
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
|
||||
{
|
||||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
// 飞拍执行只拿上传轨迹的瞬时副本,后续规划/导出都不再依赖字典锁。
|
||||
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
|
||||
if (trajectory.Waypoints.Count == 0)
|
||||
{
|
||||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
|
||||
}
|
||||
|
||||
settings = RequireRobotSettings();
|
||||
}
|
||||
|
||||
lock (_motionLock)
|
||||
{
|
||||
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
|
||||
var settings = RequireRobotSettings();
|
||||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale, speedRatio);
|
||||
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
|
||||
@@ -523,7 +602,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
if (options.MoveToStart)
|
||||
{
|
||||
_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);
|
||||
}
|
||||
else
|
||||
@@ -555,7 +634,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
/// <param name="robot">当前机器人模型。</param>
|
||||
/// <param name="targetJointPositions">目标关节位置,单位为弧度。</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();
|
||||
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
|
||||
@@ -574,6 +653,30 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
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>
|
||||
@@ -649,43 +752,45 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
|
||||
_logger?.LogInformation("SaveTrajectoryInfo 开始: name={Name}, method={Method}", name, method);
|
||||
|
||||
RobotProfile robot;
|
||||
CompatibilityRobotSettings planningSettings;
|
||||
ControllerClientCompatUploadedTrajectory trajectory;
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
|
||||
{
|
||||
_logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
// 先通过规划校验避免静默接受非法参数,同时把轨迹信息强制刷写到本地 JSON。
|
||||
var planningSettings = RequireRobotSettings();
|
||||
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);
|
||||
|
||||
// var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
// var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||||
// _trajectoryStore.Save(robotName, settings, trajectory);
|
||||
// 保存轨迹信息会执行规划和文件导出,先复制上传数据再释放状态锁。
|
||||
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
|
||||
planningSettings = RequireRobotSettings();
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
@@ -699,42 +804,48 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
|
||||
_logger?.LogInformation("IsFlyshotTrajectoryValid 开始: name={Name}, method={Method}", name, method);
|
||||
|
||||
RobotProfile robot;
|
||||
CompatibilityRobotSettings planningSettings;
|
||||
ControllerClientCompatUploadedTrajectory trajectory;
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
|
||||
{
|
||||
_logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
var planningSettings = RequireRobotSettings();
|
||||
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;
|
||||
// 有效性检查只消费当前快照,不要求和后续上传/删除形成长事务。
|
||||
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
|
||||
planningSettings = RequireRobotSettings();
|
||||
}
|
||||
|
||||
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 />
|
||||
@@ -747,6 +858,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
|
||||
_logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name);
|
||||
|
||||
string robotName;
|
||||
lock (_stateLock)
|
||||
{
|
||||
if (!_uploadedTrajectories.Remove(name))
|
||||
@@ -755,10 +867,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
throw new InvalidOperationException("DeleteFlyShotTraj failed");
|
||||
}
|
||||
|
||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
_trajectoryStore.Delete(robotName, name);
|
||||
robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
}
|
||||
|
||||
// 删除持久化文件不占用状态锁,状态页只需要看到内存字典的即时快照。
|
||||
_trajectoryStore.Delete(robotName, name);
|
||||
|
||||
_logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name);
|
||||
}
|
||||
|
||||
@@ -798,6 +912,21 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
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>
|
||||
@@ -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>
|
||||
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
|
||||
/// </summary>
|
||||
|
||||
@@ -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; }
|
||||
}
|
||||
@@ -95,6 +95,12 @@ public interface IControllerClientCompatService
|
||||
/// <returns>控制器运行时状态快照。</returns>
|
||||
ControllerStateSnapshot GetControllerSnapshot();
|
||||
|
||||
/// <summary>
|
||||
/// 读取状态页所需的兼容层元数据快照。
|
||||
/// </summary>
|
||||
/// <returns>兼容层元数据快照。</returns>
|
||||
ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度倍率。
|
||||
/// </summary>
|
||||
@@ -157,6 +163,12 @@ public interface IControllerClientCompatService
|
||||
/// <param name="jointPositions">目标关节位置。</param>
|
||||
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>
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -24,7 +24,8 @@ public sealed class TrajectoryResult
|
||||
int plannedWaypointCount,
|
||||
int triggerSampleIndexOffsetCycles = 0,
|
||||
IEnumerable<IReadOnlyList<double>>? denseJointTrajectory = null,
|
||||
FlyshotPreparedExecution? preparedFlyshotExecution = null)
|
||||
FlyshotPreparedExecution? preparedFlyshotExecution = null,
|
||||
IEnumerable<IReadOnlyList<double>>? denseCartesianTrajectory = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(programName))
|
||||
{
|
||||
@@ -60,6 +61,7 @@ public sealed class TrajectoryResult
|
||||
var copiedTriggerTimeline = triggerTimeline.ToArray();
|
||||
var copiedArtifacts = artifacts.ToArray();
|
||||
var copiedDenseJointTrajectory = denseJointTrajectory?.Select(static row => row.ToArray()).ToArray();
|
||||
var copiedDenseCartesianTrajectory = denseCartesianTrajectory?.Select(static row => row.ToArray()).ToArray();
|
||||
|
||||
ProgramName = programName;
|
||||
Method = method;
|
||||
@@ -74,6 +76,7 @@ public sealed class TrajectoryResult
|
||||
PlannedWaypointCount = plannedWaypointCount;
|
||||
TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles;
|
||||
DenseJointTrajectory = copiedDenseJointTrajectory;
|
||||
DenseCartesianTrajectory = copiedDenseCartesianTrajectory;
|
||||
PreparedFlyshotExecution = preparedFlyshotExecution;
|
||||
}
|
||||
|
||||
@@ -156,6 +159,13 @@ public sealed class TrajectoryResult
|
||||
[JsonPropertyName("denseJointTrajectory")]
|
||||
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>
|
||||
/// Gets the prepared flyshot execution queue when the flyshot chain has already built the final 8ms send sequence.
|
||||
/// </summary>
|
||||
|
||||
@@ -113,4 +113,11 @@ public interface IControllerRuntime
|
||||
/// <param name="result">规划结果。</param>
|
||||
/// <param name="finalJointPositions">轨迹执行结束后的关节位置。</param>
|
||||
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);
|
||||
}
|
||||
|
||||
@@ -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>
|
||||
/// 释放运行时持有的所有 Socket 客户端。
|
||||
/// </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>
|
||||
/// 若已有 J519 响应,则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
|
||||
/// </summary>
|
||||
@@ -879,6 +1016,41 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
|
||||
@@ -7,7 +7,7 @@ namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Command
|
||||
{
|
||||
private readonly double[] _targetJoints;
|
||||
private readonly double[] _targetValues;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 J519 命令数据。
|
||||
@@ -35,11 +35,53 @@ public sealed class FanucJ519Command
|
||||
ushort writeIoIndex = 1,
|
||||
ushort writeIoMask = 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;
|
||||
@@ -52,7 +94,7 @@ public sealed class FanucJ519Command
|
||||
WriteIoIndex = writeIoIndex;
|
||||
WriteIoMask = writeIoMask;
|
||||
WriteIoValue = writeIoValue;
|
||||
_targetJoints = targetJoints.ToArray();
|
||||
_targetValues = copiedTargetValues;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -108,7 +150,12 @@ public sealed class FanucJ519Command
|
||||
/// <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>
|
||||
|
||||
@@ -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>
|
||||
/// 兼容旧 `GetNearestIK(pose, seed, ik)` 参数形状。
|
||||
/// </summary>
|
||||
@@ -874,6 +900,136 @@ public sealed class LegacyJointPositionRequest
|
||||
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>
|
||||
/// 表示旧 `/upload_flyshot/` 路由使用的飞拍上传请求体。
|
||||
/// </summary>
|
||||
|
||||
@@ -49,23 +49,18 @@ public sealed class StatusController : ControllerBase
|
||||
public IActionResult GetSnapshot()
|
||||
{
|
||||
var snapshot = _compatService.GetControllerSnapshot();
|
||||
var isSetup = _compatService.IsSetUp;
|
||||
|
||||
// 状态页需要在机器人未初始化时仍能打开,因此只有初始化后才读取机器人元数据。
|
||||
var robotName = isSetup ? _compatService.GetRobotName() : null;
|
||||
var degreesOfFreedom = isSetup ? _compatService.GetDegreesOfFreedom() : 0;
|
||||
var uploadedTrajectories = isSetup ? _compatService.ListTrajectoryNames() : Array.Empty<string>();
|
||||
var metadata = _compatService.GetStatusSnapshotMetadata();
|
||||
|
||||
return Ok(new
|
||||
{
|
||||
Status = "ok",
|
||||
Service = "flyshot-server-host",
|
||||
ServerVersion = _compatService.GetServerVersion(),
|
||||
ClientVersion = _compatService.GetClientVersion(),
|
||||
IsSetup = isSetup,
|
||||
RobotName = robotName,
|
||||
DegreesOfFreedom = degreesOfFreedom,
|
||||
UploadedTrajectories = uploadedTrajectories,
|
||||
ServerVersion = metadata.ServerVersion,
|
||||
ClientVersion = metadata.ClientVersion,
|
||||
IsSetup = metadata.IsSetup,
|
||||
RobotName = metadata.RobotName,
|
||||
DegreesOfFreedom = metadata.DegreesOfFreedom,
|
||||
UploadedTrajectories = metadata.UploadedTrajectories,
|
||||
Snapshot = snapshot
|
||||
});
|
||||
}
|
||||
|
||||
@@ -16,6 +16,17 @@ const state = {
|
||||
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 之外的位置时使用。 */
|
||||
function escapeHtml(value) {
|
||||
return String(value).replace(/[&<>"']/g, function (ch) {
|
||||
@@ -137,6 +148,11 @@ function storageKey(op) {
|
||||
return STORAGE_PREFIX + op.method + ":" + op.path;
|
||||
}
|
||||
|
||||
/** 返回指定端点的手工调试样例,处理 JsonElement 等 OpenAPI 无法自动推断字段的接口。 */
|
||||
function getRequestBodySample(op) {
|
||||
return requestBodySamples[op.method + " " + op.path] || null;
|
||||
}
|
||||
|
||||
/** 读取本端点最近一次输入;解析失败则当作空。 */
|
||||
function loadInputs(op) {
|
||||
try {
|
||||
@@ -281,7 +297,7 @@ function renderBodyEditor(container, op, savedBody) {
|
||||
if (savedBody !== undefined && savedBody !== null) {
|
||||
initialText = typeof savedBody === "string" ? savedBody : JSON.stringify(savedBody, null, 2);
|
||||
} else {
|
||||
const sample = buildSampleFromSchema(op.bodySchema, 0);
|
||||
const sample = getRequestBodySample(op) || buildSampleFromSchema(op.bodySchema, 0);
|
||||
initialText = sample === null ? "" : JSON.stringify(sample, null, 2);
|
||||
}
|
||||
textarea.value = initialText;
|
||||
|
||||
@@ -188,6 +188,68 @@ dd {
|
||||
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) {
|
||||
.topbar {
|
||||
align-items: flex-start;
|
||||
@@ -199,6 +261,11 @@ dd {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
.jog-settings,
|
||||
.jog-grid {
|
||||
grid-template-columns: repeat(2, minmax(0, 1fr));
|
||||
}
|
||||
|
||||
dl {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
@@ -15,7 +15,28 @@ const fields = {
|
||||
joints: document.getElementById("joints"),
|
||||
pose: document.getElementById("pose"),
|
||||
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) {
|
||||
@@ -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() {
|
||||
fields.refresh.disabled = true;
|
||||
try {
|
||||
const response = await fetch("/api/status/snapshot", { cache: "no-store" });
|
||||
const payload = await response.json();
|
||||
const snapshot = payload.snapshot;
|
||||
jogState.lastSnapshot = snapshot;
|
||||
|
||||
fields.connectionState.textContent = snapshot.connectionState;
|
||||
fields.robotName.textContent = payload.robotName || "--";
|
||||
@@ -88,5 +233,23 @@ async function 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();
|
||||
window.setInterval(refreshStatus, 2000);
|
||||
|
||||
@@ -57,6 +57,36 @@
|
||||
<dt>已上传轨迹</dt><dd id="trajectories" class="empty">--</dd>
|
||||
</dl>
|
||||
</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>
|
||||
</main>
|
||||
<script src="/assets/status.js" defer></script>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -163,6 +163,28 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证直角坐标命令会把 Data format 写为 0,并按通用目标槽位写入 X/Y/Z/W/P/R。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void PackCommandPacket_WritesCartesianDataFormatAndTargetValues()
|
||||
{
|
||||
var command = new FanucJ519Command(
|
||||
sequence: 7,
|
||||
targetValues: [100.0, 200.0, 300.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0],
|
||||
dataStyle: 0);
|
||||
|
||||
var packet = FanucJ519Protocol.PackCommandPacket(command);
|
||||
|
||||
Assert.Equal(0, packet[0x12]);
|
||||
Assert.Equal(100.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
|
||||
Assert.Equal(200.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x20, 4)));
|
||||
Assert.Equal(300.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x24, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x28, 4)));
|
||||
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x2c, 4)));
|
||||
Assert.Equal(3.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证配置 J519 buffer size 后,实际回发命令序号会在状态包序号基础上增加该缓冲深度。
|
||||
/// </summary>
|
||||
|
||||
@@ -956,6 +956,111 @@ public sealed class RuntimeOrchestrationTests
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍执行阻塞在运行时时,状态页元数据快照仍能通过短锁快速返回。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ControllerClientCompatService_GetStatusSnapshotMetadata_DoesNotWaitForRunningFlyshot()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
WriteRobotConfigWithDemoTrajectory(configRoot);
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
ConfigRoot = configRoot
|
||||
};
|
||||
var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
var service = new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader());
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
service.SetActiveController(sim: false);
|
||||
service.Connect("192.168.10.101");
|
||||
service.EnableRobot(2);
|
||||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||||
|
||||
var executing = Task.Run(() => service.ExecuteTrajectoryByName(
|
||||
"demo-flyshot",
|
||||
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
|
||||
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2)));
|
||||
|
||||
var metadataTask = Task.Run(() => service.GetStatusSnapshotMetadata());
|
||||
var completed = await Task.WhenAny(metadataTask, Task.Delay(TimeSpan.FromMilliseconds(150)));
|
||||
|
||||
runtime.ReleaseExecution();
|
||||
await executing;
|
||||
|
||||
Assert.Same(metadataTask, completed);
|
||||
var metadata = await metadataTask;
|
||||
Assert.True(metadata.IsSetup);
|
||||
Assert.Equal("FANUC_LR_Mate_200iD", metadata.RobotName);
|
||||
Assert.Equal(["demo-flyshot"], metadata.UploadedTrajectories);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证两个飞拍执行命令必须串行进入 runtime,避免 J519 队列被并发执行覆盖。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ControllerClientCompatService_ExecuteTrajectoryByName_SerializesConcurrentExecutionCommands()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
WriteRobotConfigWithDemoTrajectory(configRoot);
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
ConfigRoot = configRoot
|
||||
};
|
||||
var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
var service = new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader());
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
service.SetActiveController(sim: false);
|
||||
service.Connect("192.168.10.101");
|
||||
service.EnableRobot(2);
|
||||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||||
|
||||
var first = Task.Run(() => service.ExecuteTrajectoryByName(
|
||||
"demo-flyshot",
|
||||
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
|
||||
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2)));
|
||||
|
||||
var second = Task.Run(() => service.ExecuteTrajectoryByName(
|
||||
"demo-flyshot",
|
||||
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
|
||||
await Task.Delay(TimeSpan.FromMilliseconds(100));
|
||||
|
||||
Assert.Equal(1, runtime.ExecuteCallCount);
|
||||
|
||||
runtime.ReleaseExecution();
|
||||
await first;
|
||||
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2), expectedCallCount: 2));
|
||||
runtime.ReleaseExecution();
|
||||
await second;
|
||||
|
||||
Assert.Equal(2, runtime.ExecuteCallCount);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍链路在进入运行时前就会准备最终发送队列,而不是把 speedRatio 重采样留给运行时临场处理。
|
||||
/// </summary>
|
||||
@@ -2005,6 +2110,12 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
|
||||
{
|
||||
LastExecutedResult = result;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
|
||||
{
|
||||
LastExecutedResult = result;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -2166,6 +2277,182 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
|
||||
_jointPositions = finalJointPositions.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 模拟 runtime 执行入口长期占用的测试运行时,用于验证兼容层锁边界。
|
||||
/// </summary>
|
||||
internal sealed class BlockingExecutionControllerRuntime : IControllerRuntime
|
||||
{
|
||||
private readonly object _lock = new();
|
||||
private readonly ManualResetEventSlim _executionStarted = new(false);
|
||||
private readonly ManualResetEventSlim _releaseExecution = new(false);
|
||||
private readonly double[] _jointPositions;
|
||||
private bool _isEnabled;
|
||||
private bool _isInMotion;
|
||||
private int _executeCallCount;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化一份会阻塞 ExecuteTrajectory 的测试运行时。
|
||||
/// </summary>
|
||||
/// <param name="initialJointPositions">初始关节反馈。</param>
|
||||
public BlockingExecutionControllerRuntime(IReadOnlyList<double> initialJointPositions)
|
||||
{
|
||||
_jointPositions = initialJointPositions.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取 runtime 执行入口被调用的次数。
|
||||
/// </summary>
|
||||
public int ExecuteCallCount
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _executeCallCount;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 等待指定序号的执行调用进入 runtime。
|
||||
/// </summary>
|
||||
/// <param name="timeout">最长等待时间。</param>
|
||||
/// <param name="expectedCallCount">期望已经进入的执行次数。</param>
|
||||
/// <returns>是否在超时前等到。</returns>
|
||||
public bool WaitForExecutionStarted(TimeSpan timeout, int expectedCallCount = 1)
|
||||
{
|
||||
var deadline = DateTimeOffset.UtcNow.Add(timeout);
|
||||
while (DateTimeOffset.UtcNow < deadline)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (_executeCallCount >= expectedCallCount)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
_executionStarted.Wait(TimeSpan.FromMilliseconds(10));
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放当前阻塞的执行调用。
|
||||
/// </summary>
|
||||
public void ReleaseExecution()
|
||||
{
|
||||
_releaseExecution.Set();
|
||||
}
|
||||
|
||||
public void ResetRobot(RobotProfile robot, string robotName)
|
||||
{
|
||||
}
|
||||
|
||||
public void SetActiveController(bool sim)
|
||||
{
|
||||
}
|
||||
|
||||
public void Connect(string robotIp)
|
||||
{
|
||||
}
|
||||
|
||||
public void Disconnect()
|
||||
{
|
||||
}
|
||||
|
||||
public void EnableRobot(int bufferSize)
|
||||
{
|
||||
_isEnabled = true;
|
||||
}
|
||||
|
||||
public void DisableRobot()
|
||||
{
|
||||
_isEnabled = false;
|
||||
}
|
||||
|
||||
public void StopMove()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_isInMotion = false;
|
||||
}
|
||||
|
||||
ReleaseExecution();
|
||||
}
|
||||
|
||||
public double GetSpeedRatio() => 1.0;
|
||||
|
||||
public void SetSpeedRatio(double ratio)
|
||||
{
|
||||
}
|
||||
|
||||
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
|
||||
|
||||
public void SetTcp(double x, double y, double z)
|
||||
{
|
||||
}
|
||||
|
||||
public bool GetIo(int port, string ioType) => false;
|
||||
|
||||
public void SetIo(int port, bool value, string ioType)
|
||||
{
|
||||
}
|
||||
|
||||
public IReadOnlyList<double> GetJointPositions()
|
||||
{
|
||||
return _jointPositions.ToArray();
|
||||
}
|
||||
|
||||
public IReadOnlyList<double> GetPose() => [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
|
||||
|
||||
public ControllerStateSnapshot GetSnapshot()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return new ControllerStateSnapshot(
|
||||
capturedAt: DateTimeOffset.UtcNow,
|
||||
connectionState: "Connected",
|
||||
isEnabled: _isEnabled,
|
||||
isInMotion: _isInMotion,
|
||||
speedRatio: 1.0,
|
||||
jointPositions: _jointPositions.ToArray(),
|
||||
cartesianPose: Array.Empty<double>(),
|
||||
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||||
}
|
||||
}
|
||||
|
||||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_executeCallCount++;
|
||||
_isInMotion = true;
|
||||
_executionStarted.Set();
|
||||
_releaseExecution.Reset();
|
||||
}
|
||||
|
||||
_releaseExecution.Wait();
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_isInMotion = false;
|
||||
_executionStarted.Reset();
|
||||
}
|
||||
}
|
||||
|
||||
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
|
||||
{
|
||||
ExecuteTrajectory(result, _jointPositions);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -2292,4 +2579,8 @@ internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime
|
||||
_jointPositions = finalJointPositions.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
@@ -44,6 +44,28 @@ public sealed class DebugConsoleEndpointTests(FlyshotServerFactory factory) : IC
|
||||
Assert.Contains("/api/debug/config", script, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 调试页应当为 MovePose 提供可直接发送的六字段请求体模板。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetDebugScript_ContainsMovePoseRequestSample()
|
||||
{
|
||||
using var configuredFactory = CreateFactoryWithSwaggerEnabled(true);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var scriptResponse = await client.GetAsync("/assets/debug.js");
|
||||
Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode);
|
||||
|
||||
var script = await scriptResponse.Content.ReadAsStringAsync();
|
||||
Assert.Contains("/move_pose/", script, StringComparison.Ordinal);
|
||||
Assert.Contains("x: 100.0", script, StringComparison.Ordinal);
|
||||
Assert.Contains("y: 200.0", script, StringComparison.Ordinal);
|
||||
Assert.Contains("z: 300.0", script, StringComparison.Ordinal);
|
||||
Assert.Contains("w: 0.0", script, StringComparison.Ordinal);
|
||||
Assert.Contains("p: 45.0", script, StringComparison.Ordinal);
|
||||
Assert.Contains("r: 0.0", script, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 当 Swagger 启用时,调试配置 API 应当返回实际 Swagger JSON 地址。
|
||||
/// </summary>
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using System.Net;
|
||||
using System.Net.Http.Json;
|
||||
using System.Text;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace Flyshot.Server.IntegrationTests;
|
||||
@@ -131,6 +132,13 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
|
||||
Assert.Equal("robot moved", moveJointJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
|
||||
using (var movePoseResponse = await client.PostAsJsonAsync("/move_pose/", new { x = 100.0, y = 200.0, z = 300.0, w = 1.0, p = 2.0, r = 3.0 }))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, movePoseResponse.StatusCode);
|
||||
using var movePoseJson = await ReadJsonAsync(movePoseResponse);
|
||||
Assert.Equal("robot moved", movePoseJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
|
||||
using (var getJointPositionResponse = await client.GetAsync("/get_joint_position/"))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, getJointPositionResponse.StatusCode);
|
||||
@@ -165,6 +173,32 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 MovePose 请求必须显式提供六个有限直角坐标字段,避免缺字段被模型绑定静默补 0。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":2.0}""")]
|
||||
[InlineData("null")]
|
||||
[InlineData("""{"x":1e999,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")]
|
||||
[InlineData("""{"x":1000.1,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")]
|
||||
[InlineData("""{"x":100.0,"y":-1000.1,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")]
|
||||
[InlineData("""{"x":100.0,"y":200.0,"z":-0.1,"w":1.0,"p":2.0,"r":3.0}""")]
|
||||
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":-180.1,"p":2.0,"r":3.0}""")]
|
||||
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":90.1,"r":3.0}""")]
|
||||
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":180.1}""")]
|
||||
public async Task MovePose_InvalidPayload_ReturnsLegacyBadRequest(string payload)
|
||||
{
|
||||
using var client = factory.CreateClient();
|
||||
await InitializeRobotAsync(client);
|
||||
|
||||
using var content = new StringContent(payload, Encoding.UTF8, "application/json");
|
||||
using var response = await client.PostAsync("/move_pose/", content);
|
||||
|
||||
Assert.Equal(HttpStatusCode.BadRequest, response.StatusCode);
|
||||
using var json = await ReadJsonAsync(response);
|
||||
Assert.Equal("MovePose failed", json.RootElement.GetProperty("detail").GetString());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍 HTTP 接口可以按旧 API 层的路径和字段完成上传、列出、执行与删除。
|
||||
/// </summary>
|
||||
|
||||
@@ -34,6 +34,39 @@ public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFi
|
||||
Assert.Contains("/api/status/snapshot", script, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 状态页应当提供直角坐标点动按钮,并复用现有 MovePose HTTP 接口。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetStatusPageAssets_ExposeCartesianJogControls()
|
||||
{
|
||||
using var client = factory.CreateClient();
|
||||
|
||||
using var htmlResponse = await client.GetAsync("/status.html");
|
||||
Assert.Equal(HttpStatusCode.OK, htmlResponse.StatusCode);
|
||||
|
||||
var html = await htmlResponse.Content.ReadAsStringAsync();
|
||||
Assert.Contains("直角坐标点动", html, StringComparison.Ordinal);
|
||||
Assert.Contains("id=\"linear-step\"", html, StringComparison.Ordinal);
|
||||
Assert.Contains("id=\"angular-step\"", html, StringComparison.Ordinal);
|
||||
|
||||
foreach (var axis in new[] { "x", "y", "z", "w", "p", "r" })
|
||||
{
|
||||
Assert.Contains($"data-axis=\"{axis}\"", html, StringComparison.Ordinal);
|
||||
Assert.Contains($"data-axis=\"{axis}\" data-direction=\"1\"", html, StringComparison.Ordinal);
|
||||
Assert.Contains($"data-axis=\"{axis}\" data-direction=\"-1\"", html, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
using var scriptResponse = await client.GetAsync("/assets/status.js");
|
||||
Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode);
|
||||
|
||||
var script = await scriptResponse.Content.ReadAsStringAsync();
|
||||
Assert.Contains("/move_pose/", script, StringComparison.Ordinal);
|
||||
Assert.Contains("cartesianPose", script, StringComparison.Ordinal);
|
||||
Assert.Contains("pointerdown", script, StringComparison.Ordinal);
|
||||
Assert.Contains("pointerup", script, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态快照 API 会返回运行时连接、使能、速度和机器人元数据。
|
||||
/// </summary>
|
||||
|
||||
Reference in New Issue
Block a user