diff --git a/.claude/settings.local.json b/.claude/settings.local.json index 2c991b3..6d50327 100644 --- a/.claude/settings.local.json +++ b/.claude/settings.local.json @@ -11,7 +11,32 @@ "Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json', encoding='utf-8'\\)\\); json.load\\(open\\('.vscode/tasks.json', encoding='utf-8'\\)\\); print\\('JSON valid.'\\)\")", "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal')", "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal')", - "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal')" + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal')", + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal 2>&1')", + "Bash(taskkill /PID 120812 /F)", + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build flyshot-replacement.sln --no-restore -v minimal 2>&1')", + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build /d/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/flyshot-replacement/FlyshotReplacement.sln --no-restore -v minimal 2>&1')", + "Bash(capinfos \"20260430.pcap\")", + "Bash(tshark -r \"20260430.pcap\" -T fields -e frame.number -e frame.time_relative -e ip.src -e ip.dst -e udp.srcport -e udp.dstport -e data.len)", + "Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015\" -T fields -e frame.number -e frame.time_relative -e ip.src -e udp.srcport -e udp.length -e data.len)", + "Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.srcport==60015\" -T fields -e frame.number -e frame.time_relative -e udp.length)", + "Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e udp.length)", + "Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015\" -T fields -e frame.number -e frame.time_relative -e ip.src -e ip.dst -e udp.srcport -e udp.dstport -e udp.length -e frame.protocols)", + "Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and frame.number==1\" -V)", + "Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.dstport!=60015\" -T fields -e frame.number -e frame.time_relative -e ip.dst -e udp.dstport)", + "Bash(tshark -r \"20260430.pcap\" -Y \"udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e ip.src -e udp.length)", + "Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -e frj519.type -e frj519.seq -e frj519.status -e frj519.j1 -e frj519.j2 -e frj519.j3 -e frj519.j4 -e frj519.j5 -e frj519.j6 -e frj519.j7 -e frj519.j8 -e frj519.j9 -e frj519.timestamp)", + "Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -e frj519.type -e frj519.seq -e frj519.status -e frj519.j1 -e frj519.j2 -e frj519.j3 -e frj519.j4 -e frj519.j5 -e frj519.j6 -e frj519.j7 -e frj519.j8 -e frj519.j9 -e frj519.timestamp -e frj519.x -e frj519.y -e frj519.z -e frj519.w -e frj519.p -e frj519.r)", + "Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -E header=y)", + "Bash(tshark -G fields)", + "Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -e frame.time_relative -e data)", + "Bash(tshark -r \"20260430.pcap\" -Y \"udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e data)", + "Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.srcport==60015\" -T fields -e frame.number -e frame.time_relative -e data)", + "Bash(tshark -r \"20260430.pcap\" --disable-protocol frj519 -Y \"udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e data)", + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj --no-restore -v minimal 2>&1 | tail -20')", + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal 2>&1 | tail -25')", + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal 2>&1 | tail -15')", + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --no-restore -v minimal 2>&1 | tail -30')" ] } } diff --git a/Config/RobotConfig.json b/Config/RobotConfig.json index c293bc3..a8ba07a 100644 --- a/Config/RobotConfig.json +++ b/Config/RobotConfig.json @@ -1 +1,303 @@ -{"robot": {"use_do": true, "io_addr": [7, 8], "io_keep_cycles": 2, "acc_limit": 1, "jerk_limit": 1, "adapt_icsp_try_num": 5}, "flying_shots": {"20251015": {"traj_waypoints": [[1.047438621520996, -0.0002488955215085298, -0.0014060207176953554, 0.009022523649036884, 0.010111905634403229, 0.009573347866535187], [0.7661270499229431, -0.04437164217233658, -0.13630111515522003, -0.41718506813049316, 0.010093353688716888, 0.009594489820301533], [0.7661266326904297, 0.2170650213956833, -0.13630135357379913, -0.4171852171421051, 0.010093353688716888, 0.009594779461622238], [1.0311520099639893, -0.062108494341373444, -0.1363297700881958, 0.30276036262512207, 0.15847623348236084, 0.00956842489540577], [1.4012629985809326, -0.05120057240128517, -0.13633012771606445, 0.3027600347995758, 0.15847666561603546, 0.00956842489540577], [1.0567246675491333, 0.01165649201720953, -0.01786380261182785, -0.015170873142778873, 0.02149667963385582, 0.009576244279742241]], "shot_flags": [false, true, true, true, true, true], "offset_values": [0, 0, 0, 0, 0, 0], "addr": [[], [2, 4], [3, 4], [2, 4], [3, 4], [2, 4]]}, "TEST20251214": {"traj_waypoints": [[1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846], [0.8067416, 0.011661344, -0.11788314, -0.01516874, 0.021492079, 0.009567846], [0.60675246, -0.03833516, -0.11788314, 0.034831185, -0.22849938, -0.24043223], [0.7667507, 0.20164281, -0.11788314, 0.034831185, -0.22849938, -0.24043223], [0.7667507, 0.20164281, -0.11788314, 0.034831185, -0.22849938, -0.14043556], [1.1667324, 0.05164983, -0.11789217, 0.23482007, 0.021492079, -0.14043556], [1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846]], "shot_flags": [false, true, true, true, true, true, false], "offset_values": [0, 0, 0, 0, 0, 0, 0], "addr": [[], [], [3, 4, 2], [3, 4, 2], [3, 4, 2], [3, 4, 2], []]}, "UTTC_MS11": {"traj_waypoints": [[1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846], [0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145], [0.96600056, 0.20607172, -0.12233179, -1.2394339, 0.10493033, 1.2958988], [0.9618476, 0.15288207, -0.14867093, -0.7176314, 0.1764264, 0.73228663], [0.76189893, -0.028442925, -0.30919823, 0.10463613, 0.5615024, -0.39399016], [1.1271763, 0.074403025, -0.27347943, -0.5227772, 0.52098846, 0.79633313], [1.0555661, 0.4026262, -0.08746306, 0.6301835, 0.09644133, -0.5463328], [1.2300354, 0.28612664, -0.23486805, -0.4868128, 0.25369516, 0.55347764], [1.2144431, -0.29855102, -0.15202847, -1.0205934, 0.13317892, 1.1246506], [1.2840607, -0.11222197, -0.16805042, -2.248135, 0.2560587, 2.4434967], [1.3189346, -0.25620222, -0.12730704, -2.285038, 0.30872014, 2.4765089], [1.502615, -0.25304365, -0.23878741, -1.2194318, 0.46674785, 1.5533328], [1.07723, -0.07387611, -0.1707704, -1.8916591, 0.38677844, 2.061968], [1.3920237, 0.08098731, -0.2672306, -0.9780007, 0.4561093, 0.9102286], [1.9016331, 0.023924276, -0.58633333, -0.8441697, 0.76730615, 1.4842151], [1.9300697, -0.06738541, -0.56542397, -0.892083, 0.77194446, 1.5293273], [2.0611632, -0.30327517, -0.54225636, -1.0395275, 0.8505439, 1.6429617], [1.0921186, -0.40034482, -0.1803499, 1.3524796, 0.6210477, -1.2159473], [1.0521278, -0.40034503, -0.1803492, 1.3524843, 0.6210471, -1.2159531], [1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846]], "shot_flags": [false, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, false, true, false], "offset_values": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "addr": [[], [2, 4], [3, 4, 2], [3, 4, 2], [4, 2], [4, 2], [3, 4], [3, 4], [4, 2], [4, 2], [4, 2], [4, 2], [4, 2], [4, 3], [4, 2], [4, 2], [4, 2], [4, 2], [4, 3], []]}, "5U": {"traj_waypoints": [[-0.95982397, 0.6331447, -1.0055008, 0.79858834, 1.1564041, -0.4260437], [-0.98353565, 0.66203266, -0.9758351, 0.8320198, 1.1455917, -0.45941326]], "shot_flags": [false, false], "offset_values": [0, 0], "addr": [[], []]}}} \ No newline at end of file +{ + "robot": { + "use_do": true, + "io_addr": [ + 7, + 8 + ], + "io_keep_cycles": 2, + "acc_limit": 1, + "jerk_limit": 1, + "adapt_icsp_try_num": 5, + "planning_speed_scale": 0.74227, + "smooth_start_stop_timing": false + }, + "flying_shots": { + "UTTC_MS11": { + "traj_waypoints": [ + [ + 1.056731, + 0.011664811, + -0.017892333, + -0.01516874, + 0.021492079, + 0.009567846 + ], + [ + 0.8532358, + 0.03837953, + -0.19235304, + 0.0071595116, + 0.109054826, + 0.040055145 + ], + [ + 0.96600056, + 0.20607172, + -0.12233179, + -1.2394339, + 0.10493033, + 1.2958988 + ], + [ + 0.9618476, + 0.15288207, + -0.14867093, + -0.7176314, + 0.1764264, + 0.73228663 + ], + [ + 0.76189893, + -0.028442925, + -0.30919823, + 0.10463613, + 0.5615024, + -0.39399016 + ], + [ + 1.1271763, + 0.074403025, + -0.27347943, + -0.5227772, + 0.52098846, + 0.79633313 + ], + [ + 1.0555661, + 0.4026262, + -0.08746306, + 0.6301835, + 0.09644133, + -0.5463328 + ], + [ + 1.2300354, + 0.28612664, + -0.23486805, + -0.4868128, + 0.25369516, + 0.55347764 + ], + [ + 1.2144431, + -0.29855102, + -0.15202847, + -1.0205934, + 0.13317892, + 1.1246506 + ], + [ + 1.2840607, + -0.11222197, + -0.16805042, + -2.248135, + 0.2560587, + 2.4434967 + ], + [ + 1.3189346, + -0.25620222, + -0.12730704, + -2.285038, + 0.30872014, + 2.4765089 + ], + [ + 1.502615, + -0.25304365, + -0.23878741, + -1.2194318, + 0.46674785, + 1.5533328 + ], + [ + 1.07723, + -0.07387611, + -0.1707704, + -1.8916591, + 0.38677844, + 2.061968 + ], + [ + 1.3920237, + 0.08098731, + -0.2672306, + -0.9780007, + 0.4561093, + 0.9102286 + ], + [ + 1.9016331, + 0.023924276, + -0.58633333, + -0.8441697, + 0.76730615, + 1.4842151 + ], + [ + 1.9300697, + -0.06738541, + -0.56542397, + -0.892083, + 0.77194446, + 1.5293273 + ], + [ + 2.0611632, + -0.30327517, + -0.54225636, + -1.0395275, + 0.8505439, + 1.6429617 + ], + [ + 1.0921186, + -0.40034482, + -0.1803499, + 1.3524796, + 0.6210477, + -1.2159473 + ], + [ + 1.0521278, + -0.40034503, + -0.1803492, + 1.3524843, + 0.6210471, + -1.2159531 + ], + [ + 1.056731, + 0.011664811, + -0.017892333, + -0.01516874, + 0.021492079, + 0.009567846 + ] + ], + "shot_flags": [ + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + true, + false + ], + "offset_values": [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + "addr": [ + [], + [ + 2, + 4 + ], + [ + 3, + 4, + 2 + ], + [ + 3, + 4, + 2 + ], + [ + 4, + 2 + ], + [ + 4, + 2 + ], + [ + 3, + 4 + ], + [ + 3, + 4 + ], + [ + 4, + 2 + ], + [ + 4, + 2 + ], + [ + 4, + 2 + ], + [ + 4, + 2 + ], + [ + 4, + 2 + ], + [ + 4, + 3 + ], + [ + 4, + 2 + ], + [ + 4, + 2 + ], + [ + 4, + 2 + ], + [ + 4, + 2 + ], + [ + 4, + 3 + ], + [] + ] + } + } +} \ No newline at end of file diff --git a/analysis/fanuc_10010_dissector.lua b/analysis/fanuc_10010_dissector.lua new file mode 100644 index 0000000..76f2e78 --- /dev/null +++ b/analysis/fanuc_10010_dissector.lua @@ -0,0 +1,199 @@ +local plugin_info = { + version = "1.1.0", + author = "OpenAI Codex", + description = "FANUC TCP 10010 状态帧解析器", +} + +set_plugin_info(plugin_info) + +local fanuc10010_proto = Proto("fanuc10010_state_frame", "FANUC 10010 State Frame") + +local FRAME_LENGTH = 90 +local HEADER_LENGTH = 3 +local TRAILER_LENGTH = 3 +local LENGTH_OFFSET = 3 +local MESSAGE_ID_OFFSET = 7 +local POSE_OFFSET = 11 +local POSE_COUNT = 6 +local JOINT_OFFSET = 35 +local JOINT_COUNT = 9 +local TAIL_OFFSET = 71 +local TAIL_COUNT = 4 + +local POSE_NAMES = { "X", "Y", "Z", "W", "P", "R" } +local JOINT_NAMES = { "J1", "J2", "J3", "J4", "J5", "J6", "Ext1", "Ext2", "Ext3" } +local HEADER_MAGIC = "doz" +local TRAILER_MAGIC = "zod" + +local fields = { + frame = ProtoField.bytes("fanuc10010.frame", "原始状态帧"), + header = ProtoField.string("fanuc10010.header", "帧头 Magic"), + declared_length = ProtoField.uint32("fanuc10010.length", "声明长度", base.DEC), + message_id = ProtoField.uint32("fanuc10010.message_id", "消息号", base.DEC), + pose = ProtoField.none("fanuc10010.pose", "笛卡尔位姿"), + joints = ProtoField.none("fanuc10010.joints", "关节与扩展轴"), + joint_degrees = ProtoField.none("fanuc10010.joint_degrees", "Joint Degrees"), + tail = ProtoField.none("fanuc10010.tail", "尾部状态字"), + trailer = ProtoField.string("fanuc10010.trailer", "帧尾 Magic"), + expert_bad_length = ProtoField.string("fanuc10010.expert.bad_length", "长度异常"), + expert_bad_magic = ProtoField.string("fanuc10010.expert.bad_magic", "Magic 异常"), +} + +for index = 1, POSE_COUNT do + fields["pose_" .. index] = ProtoField.float( + "fanuc10010.pose_" .. index, + "Pose " .. POSE_NAMES[index]) +end + +for index = 1, JOINT_COUNT do + fields["joint_" .. index] = ProtoField.float( + "fanuc10010.joint_" .. index, + JOINT_NAMES[index] .. " (raw)") +end + +for index = 1, 6 do + fields["joint_deg_" .. index] = ProtoField.float( + "fanuc10010.joint_deg_" .. index, + JOINT_NAMES[index] .. " (deg)") +end + +for index = 1, TAIL_COUNT do + fields["tail_" .. index] = ProtoField.uint32( + "fanuc10010.tail_" .. index, + "Tail[" .. (index - 1) .. "]", + base.DEC) +end + +fanuc10010_proto.fields = fields + +local function read_f32_be(tvb, offset) + return tvb(offset, 4):tvb():range(0, 4):float() +end + +local function read_u32_be(tvb, offset) + return tvb(offset, 4):uint() +end + +local function read_ascii(tvb, offset, length) + return tvb(offset, length):string() +end + +local function radians_to_degrees(value) + return value * 180.0 / math.pi +end + +local function add_error(subtree, range, field, message) + local item = subtree:add(field, range, message) + item:add_expert_info(PI_MALFORMED, PI_ERROR, message) +end + +local function dissect_single_frame(tvb, pinfo, tree, frame_offset) + local remaining = tvb:len() - frame_offset + local candidate_length = math.min(remaining, FRAME_LENGTH) + local frame_range = tvb(frame_offset, candidate_length) + local subtree = tree:add(fanuc10010_proto, frame_range, "FANUC 10010 状态帧") + subtree:add(fields.frame, frame_range) + + if remaining < FRAME_LENGTH then + add_error(subtree, frame_range, fields.expert_bad_length, "剩余字节不足 90B，无法组成完整状态帧") + return remaining + end + + local header = read_ascii(tvb, frame_offset, HEADER_LENGTH) + local declared_length = read_u32_be(tvb, frame_offset + LENGTH_OFFSET) + local message_id = read_u32_be(tvb, frame_offset + MESSAGE_ID_OFFSET) + local trailer = read_ascii(tvb, frame_offset + FRAME_LENGTH - TRAILER_LENGTH, TRAILER_LENGTH) + + subtree:add(fields.header, tvb(frame_offset, HEADER_LENGTH), header) + subtree:add(fields.declared_length, tvb(frame_offset + LENGTH_OFFSET, 4), declared_length) + subtree:add(fields.message_id, tvb(frame_offset + MESSAGE_ID_OFFSET, 4), message_id) + + local pose_tree = subtree:add(fields.pose, tvb(frame_offset + POSE_OFFSET, POSE_COUNT * 4)) + local pose_values = {} + for index = 1, POSE_COUNT do + local field_offset = frame_offset + POSE_OFFSET + ((index - 1) * 4) + local value = read_f32_be(tvb, field_offset) + pose_values[index] = value + pose_tree:add(fields["pose_" .. index], tvb(field_offset, 4), value) + end + + local joint_tree = subtree:add(fields.joints, tvb(frame_offset + JOINT_OFFSET, JOINT_COUNT * 4)) + local joint_values = {} + for index = 1, JOINT_COUNT do + local field_offset = frame_offset + JOINT_OFFSET + ((index - 1) * 4) + local value = read_f32_be(tvb, field_offset) + joint_values[index] = value + joint_tree:add(fields["joint_" .. index], tvb(field_offset, 4), value) + end + + -- 单独保留一组角度显示，便于对照原始关节弧度值。 + local joint_degree_tree = subtree:add(fields.joint_degrees, tvb(frame_offset + JOINT_OFFSET, 6 * 4)) + for index = 1, 6 do + local field_offset = frame_offset + JOINT_OFFSET + ((index - 1) * 4) + joint_degree_tree:add( + fields["joint_deg_" .. index], + tvb(field_offset, 4), + radians_to_degrees(joint_values[index])) + end + + local tail_tree = subtree:add(fields.tail, tvb(frame_offset + TAIL_OFFSET, TAIL_COUNT * 4)) + local tail_values = {} + for index = 1, TAIL_COUNT do + local field_offset = frame_offset + TAIL_OFFSET + ((index - 1) * 4) + local value = read_u32_be(tvb, field_offset) + tail_values[index] = value + tail_tree:add(fields["tail_" .. index], tvb(field_offset, 4), value) + end + + subtree:add(fields.trailer, tvb(frame_offset + FRAME_LENGTH - TRAILER_LENGTH, TRAILER_LENGTH), trailer) + + if header ~= HEADER_MAGIC then + add_error(subtree, tvb(frame_offset, HEADER_LENGTH), fields.expert_bad_magic, "帧头不是 doz") + end + + if trailer ~= TRAILER_MAGIC then + add_error(subtree, tvb(frame_offset + FRAME_LENGTH - TRAILER_LENGTH, TRAILER_LENGTH), fields.expert_bad_magic, "帧尾不是 zod") + end + + if declared_length ~= FRAME_LENGTH then + add_error(subtree, tvb(frame_offset + LENGTH_OFFSET, 4), fields.expert_bad_length, "长度字段不是 90") + end + + local summary = string.format( + "MsgId=%u X=%.3f Y=%.3f Z=%.3f J1=%.6f rad / %.3f deg J2=%.6f rad / %.3f deg Tail=[%u,%u,%u,%u]", + message_id, + pose_values[1], pose_values[2], pose_values[3], + joint_values[1], radians_to_degrees(joint_values[1]), + joint_values[2], radians_to_degrees(joint_values[2]), + tail_values[1], tail_values[2], tail_values[3], tail_values[4]) + + subtree:set_text("FANUC 10010 状态帧, " .. summary) + pinfo.cols.info:append(" | " .. summary) + + return FRAME_LENGTH +end + +function fanuc10010_proto.dissector(tvb, pinfo, tree) + if tvb:len() == 0 then + return 0 + end + + pinfo.cols.protocol = "FANUC10010" + + local offset = 0 + while offset < tvb:len() do + local consumed = dissect_single_frame(tvb, pinfo, tree, offset) + if consumed <= 0 then + break + end + + offset = offset + consumed + if consumed < FRAME_LENGTH then + break + end + end + + return tvb:len() +end + +DissectorTable.get("tcp.port"):add(10010, fanuc10010_proto) diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs index 05984a8..8c7384f 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs @@ -609,11 +609,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method), planningSettings, planningSettings.PlanningSpeedScale); + _logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地"); ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle); - var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); - var settings = _robotSettings ?? CreateDefaultRobotSettings(); - _trajectoryStore.Save(robotName, settings, trajectory); + // var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); + // var settings = _robotSettings ?? CreateDefaultRobotSettings(); + // _trajectoryStore.Save(robotName, settings, trajectory); } _logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name); diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs index 0d13841..fbe363c 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs @@ -12,6 +12,16 @@ namespace Flyshot.ControllerClientCompat; /// public sealed class ControllerClientTrajectoryOrchestrator { + /// + /// 稠密轨迹离散限幅失败后允许统一拉长时间轴的最大次数。 + /// + private const int MaxDenseLimitStretchIterations = 100; + + /// + /// 每次离散限幅失败后统一放大的时间倍率。 + /// + private const double DenseLimitStretchFactor = 1.01; + private readonly ICspPlanner _icspPlanner; private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner; private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver()); @@ -68,8 +78,10 @@ public sealed class ControllerClientTrajectoryOrchestrator saveTrajectoryArtifacts: options.SaveTrajectory); var plannedTrajectory = PlanByMethod(request, method); + var executionTrajectory = plannedTrajectory; + var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref executionTrajectory, shapeTrajectoryEdges: false); var shotTimeline = new ShotTimeline(Array.Empty(), Array.Empty()); - var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false, shapeTrajectoryEdges: false); + var result = CreateResult(executionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false); _logger?.LogInformation( "PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}", @@ -138,12 +150,13 @@ public sealed class ControllerClientTrajectoryOrchestrator var plannedTrajectory = PlanByMethod(request, method, settings); var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings); + var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref smoothedExecutionTrajectory, shapeTrajectoryEdges: false); var shotTimeline = _shotTimelineBuilder.Build( smoothedExecutionTrajectory, holdCycles: settings.IoKeepCycles, samplePeriod: planningRobot.ServoPeriod, useDo: settings.UseDo); - var result = CreateResult(smoothedExecutionTrajectory, shotTimeline, usedCache: false, shapeTrajectoryEdges: false); + var result = CreateResult(smoothedExecutionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false); var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result); _logger?.LogInformation( @@ -385,14 +398,9 @@ public sealed class ControllerClientTrajectoryOrchestrator private static TrajectoryResult CreateResult( PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, - bool usedCache, - bool shapeTrajectoryEdges) + IReadOnlyList> denseJointTrajectory, + bool usedCache) { - var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory( - plannedTrajectory, - samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds, - smoothStartStop: shapeTrajectoryEdges); - return new TrajectoryResult( programName: plannedTrajectory.OriginalProgram.Name, method: plannedTrajectory.Method, @@ -408,6 +416,65 @@ public sealed class ControllerClientTrajectoryOrchestrator denseJointTrajectory: denseJointTrajectory); } + /// + /// 生成满足离散速度、加速度和 Jerk 限制的稠密执行轨迹。 + /// + private IReadOnlyList> CreateLimitCompliantDenseTrajectory( + ref PlannedTrajectory executionTrajectory, + bool shapeTrajectoryEdges) + { + for (var iteration = 0; iteration <= MaxDenseLimitStretchIterations; iteration++) + { + var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory( + executionTrajectory, + samplePeriod: executionTrajectory.Robot.ServoPeriod.TotalSeconds, + smoothStartStop: shapeTrajectoryEdges); + + try + { + TrajectoryLimitValidator.ValidateDenseJointTrajectory( + executionTrajectory.Robot, + denseJointTrajectory, + trajectoryName: executionTrajectory.OriginalProgram.Name); + return denseJointTrajectory; + } + catch (InvalidOperationException ex) when (iteration < MaxDenseLimitStretchIterations) + { + _logger?.LogWarning(ex, "稠密轨迹离散限幅校验失败，准备拉长时间轴重试"); + // 离散差分超限时统一拉长时间轴，保持路点几何不变并降低速度、加速度和 Jerk。 + executionTrajectory = StretchTrajectoryTiming(executionTrajectory, DenseLimitStretchFactor); + _logger?.LogInformation( + "离散差分超限，拉长时间轴，iteration={Iteration}, factor={StretchFactor}", + iteration, + DenseLimitStretchFactor); + _logger?.LogInformation("拉长之后的总时间={TotalTime}", executionTrajectory.WaypointTimes[^1]); + } + } + + throw new InvalidOperationException("稠密轨迹离散限幅校验未能产生有效结果。"); + } + + /// + /// 按统一倍率拉长轨迹时间轴，保留原始路点和触发元数据。 + /// + private PlannedTrajectory StretchTrajectoryTiming(PlannedTrajectory trajectory, double stretchFactor) + { + var waypointTimes = trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray(); + var segmentDurations = trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray(); + var segmentScales = trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray(); + + return new PlannedTrajectory( + robot: trajectory.Robot, + originalProgram: trajectory.OriginalProgram, + plannedWaypoints: trajectory.PlannedWaypoints, + waypointTimes: waypointTimes, + segmentDurations: segmentDurations, + segmentScales: segmentScales, + method: trajectory.Method, + iterations: trajectory.Iterations, + threshold: trajectory.Threshold); + } + /// /// 为飞拍执行生成一条平滑起停的时间轴。 /// 保持路点位置不变，只重映射路点时刻，让起点和终点附近的速度自然收敛。 diff --git a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs index 7b8f88d..2bed293 100644 --- a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs +++ b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs @@ -63,25 +63,28 @@ public sealed class FlyshotTrajectoryArtifactWriter var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName)); Directory.CreateDirectory(outputDir); + if (bundle.Result.DenseJointTrajectory is null) + { + throw new InvalidOperationException("导出飞拍轨迹工件前必须先生成执行侧稠密轨迹。"); + } - // 明细文件对齐旧 Data 目录的 16ms 采样；运行时 J519 仍可使用自己的 8ms 伺服采样。 + // 明细文件现在定义为“执行侧 8ms 稠密轨迹的 16ms 低频视图”，避免再次从 PlannedTrajectory 生成另一条轨迹。 var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath); var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory); - var jointDetailTrajectory = TrajectorySampler.SampleJointTrajectory( - bundle.PlannedTrajectory, - samplePeriod: LegacyDetailSamplePeriodSeconds); + _logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count); + var executionDenseTrajectory = bundle.Result.DenseJointTrajectory; + var jointDetailTrajectory = DownsampleDenseRows( + executionDenseTrajectory, + samplePeriodSeconds: LegacyDetailSamplePeriodSeconds); var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel); - var cartDetailTrajectory = TrajectorySampler.SampleCartesianTrajectory( - bundle.PlannedTrajectory, - kinematicsModel, - samplePeriod: LegacyDetailSamplePeriodSeconds); + var cartDetailTrajectory = BuildCartesianRowsFromJointDense(jointDetailTrajectory, kinematicsModel); TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory); TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory); TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory); TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory); TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), bundle.ShotTimeline.ShotEvents); - WriteActualSendArtifacts(outputDir, bundle.Result, speedRatio); + WriteActualSendArtifacts(outputDir, robot, bundle.Result, speedRatio); _logger?.LogInformation( "saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}", @@ -95,8 +98,10 @@ public sealed class FlyshotTrajectoryArtifactWriter /// /// 生成按 J519 8ms 实际发送周期重采样的轨迹点，供 saveTrajectory 离线对比真实下发序列。 /// - private static void WriteActualSendArtifacts(string outputDir, TrajectoryResult result, double speedRatio) + private void WriteActualSendArtifacts(string outputDir, RobotProfile robot, TrajectoryResult result, double speedRatio) { + ArgumentNullException.ThrowIfNull(robot); + if (result.DenseJointTrajectory is null) { return; @@ -112,6 +117,19 @@ public sealed class FlyshotTrajectoryArtifactWriter result.Duration.TotalSeconds, ActualSendServoPeriodSeconds, speedRatio); + + + try + { + TrajectoryLimitValidator.ValidateJ519SendSamples( + robot, + samples, + trajectoryName: result.ProgramName); + } + catch (Exception e) + { + _logger?.LogError(e, "ValidateJ519SendSamples 失败，program={ProgramName}", result.ProgramName); + } var jointRows = new List>(samples.Count); var timingRows = new List>(samples.Count); var jerkRows = new List>(); @@ -217,6 +235,63 @@ public sealed class FlyshotTrajectoryArtifactWriter return rows; } + /// + /// 基于执行侧稠密关节轨迹生成笛卡尔导出行，保持与 JointDetialTraj.txt 同一来源。 + /// + private static IReadOnlyList> BuildCartesianRowsFromJointDense( + IReadOnlyList> jointDenseRows, + RobotKinematicsModel kinematicsModel) + { + var rows = new List>(jointDenseRows.Count); + foreach (var jointRow in jointDenseRows) + { + var jointPositions = jointRow.Skip(1).ToArray(); + var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPositions); + var row = new List(pose.Length + 1) + { + Math.Round(jointRow[0], 6) + }; + row.AddRange(pose.Select(static value => Math.Round(value, 6))); + rows.Add(row); + } + + return rows; + } + + /// + /// 将 8ms 执行稠密轨迹按指定周期抽稀为低频兼容视图，并始终保留终点。 + /// + private static IReadOnlyList> DownsampleDenseRows( + IReadOnlyList> denseRows, + double samplePeriodSeconds) + { + var result = new List>(); + var epsilon = 1e-6; + var nextSampleTime = 0.0; + + foreach (var row in denseRows) + { + var sampleTime = row[0]; + if (sampleTime + epsilon < nextSampleTime) + { + continue; + } + + if (Math.Abs(sampleTime - nextSampleTime) <= epsilon || sampleTime.Equals(0.0)) + { + result.Add(row); + nextSampleTime += samplePeriodSeconds; + } + } + + if (result.Count == 0 || !ReferenceEquals(result[^1], denseRows[^1])) + { + result.Add(denseRows[^1]); + } + + return result; + } + /// /// 将轨迹名转换为可用目录名，避免 HTTP 输入中的路径字符污染输出目录。 /// diff --git a/src/Flyshot.Core.Planning/ICSP-算法说明.md b/src/Flyshot.Core.Planning/ICSP-算法说明.md new file mode 100644 index 0000000..4424737 --- /dev/null +++ b/src/Flyshot.Core.Planning/ICSP-算法说明.md @@ -0,0 +1,194 @@ +# ICSP 算法说明（`ICspPlanner`） + +本文档用于解释 `Flyshot.Core.Planning.ICspPlanner` 当前实现的 **ICSP 规划算法**在本仓库中的真实含义与计算逻辑，便于与逆向结论对照、以及指导后续改造（例如“按约束生成中间点位”）。 + +> 适用范围：本文描述的是当前 C# 实现的 **“CubicSpline + 逐段时间缩放迭代（retiming）”** 版本。 +> 重要澄清：`ICspPlanner` 的主要输出是 **时间轴**（每个示教点的时间戳），而不是直接输出固定周期的稠密点序列；稠密点在后续采样层生成。 + +--- + +## 1. 名词与数据形态 + +### 1.1 输入 + +- **示教点（路点）**：`request.Program.Waypoints` + 每个路点是关节空间向量 \(q_i \in \mathbb{R}^{dof}\)。 +- **关节约束**：`request.Robot.JointLimits[d]` 提供每轴上限： + - 速度上限 \(v_{lim}[d]\) + - 加速度上限 \(a_{lim}[d]\) + - 跃度（jerk）上限 \(j_{lim}[d]\) + +### 1.2 输出（`PlannedTrajectory`） + +`ICspPlanner` 的输出 **不是稠密轨迹点序列**，而是： + +- `PlannedWaypoints`：规划后路点（对于普通 `icsp`，与输入示教点相同；补点发生在 `SelfAdaptIcspPlanner`） +- `WaypointTimes`：每个路点的绝对时间 \(t_i\)（秒） +- `SegmentDurations`：每段时长 \(T_i = t_{i+1}-t_i\)（秒） +- `SegmentScales`：每段缩放因子 `scale_i` +- `Iterations` / `Threshold`：收敛信息 + +后续模块会基于 `PlannedWaypoints + WaypointTimes` 重建样条并采样，生成稠密点： + +- 规划层稠密采样：`TrajectorySampler.SampleJointTrajectory(...)` +- 运行时 J519 重采样（速度倍率映射 + rad->deg）：`J519SendTrajectorySampler.SampleDenseJointTrajectory(...)` + +--- + +## 2. 算法总体目标（retiming） + +给定一组关节示教点 \(\{q_i\}_{i=0}^{N-1}\)，在不改变路点位置的前提下，为每段分配时长 \(\{T_i\}_{i=0}^{N-2}\)，使得用 **clamped-zero 三次样条**连接后的轨迹在每段上满足： + +- \(\max|\dot q_d(t)| \le v_{lim}[d]\) +- \(\max|\ddot q_d(t)| \le a_{lim}[d]\) +- \(\max|\dddot q_d(t)| \le j_{lim}[d]\) + +实现策略是“逐段缩放时长”的迭代法：每轮用当前 \(\{T_i\}\) 构造样条并解析求导峰值，再根据超限程度把相应段时长乘以缩放因子，使峰值回落。 + +--- + +## 3. 计算步骤（与代码一致） + +### 3.1 前置条件 + +- 路点数 \(N \ge 4\)（否则抛异常） + +### 3.2 初始段时长 + +段数 \(nseg = N-1\)。 + +初始段时长取相邻路点关节空间欧氏距离： + +\[ +T_i^{(0)} = \|q_{i+1}-q_i\|_2 +\] + +### 3.3 由段时长构造绝对时间轴 + +\[ +t_0 = 0,\quad t_{i+1} = t_i + T_i +\] + +### 3.4 用 clamped-zero 边界构造三次样条 + +以 \((t_i, q_i)\) 为节点构造分段三次多项式： + +\[ +S_i(t) = a_i t^3 + b_i t^2 + c_i t + d_i,\quad t \in [t_i, t_{i+1}] +\] + +边界条件为 **clamped-zero**（起点/终点一阶导为 0），用于与逆向锁定的参考行为对齐。 + +### 3.5 解析计算每段导数峰值 + +对每段、每轴，解析求最大绝对值： + +- 一阶导（速度）是二次函数：端点与顶点候选取最大 +- 二阶导（加速度）是一次函数：端点取最大 +- 三阶导（跃度）是常数：直接取绝对值 + +得到三张矩阵： + +- `maxDq[seg,d] = max_t |dq/dt|` +- `maxDdq[seg,d] = max_t |d²q/dt²|` +- `maxDddq[seg,d] = max_t |d³q/dt³|` + +### 3.6 计算每段缩放因子（核心公式） + +对段 `seg`，对每个关节 `d` 计算三类“超限比”： + +\[ +s_v = \left|\frac{maxDq[seg,d]}{v_{lim}[d]}\right| +\] + +\[ +s_a = \sqrt{\left|\frac{maxDdq[seg,d]}{a_{lim}[d]}\right|} +\] + +\[ +s_j = \sqrt[3]{\left|\frac{maxDddq[seg,d]}{j_{lim}[d]}\right|} +\] + +段缩放因子取所有轴、三类约束的最大值： + +\[ +scale_{seg}=\max_d \max(s_v, s_a, s_j) +\] + +> 指数来源：时间拉长 \(k\) 倍时，速度按 \(1/k\) 缩小、加速度按 \(1/k^2\) 缩小、跃度按 \(1/k^3\) 缩小，因此超限比需要分别取一次方/平方根/立方根来求“应当拉长多少倍”。 + +### 3.7 收敛指标与最优解保存 + +每轮用如下指标衡量“离约束满足还差多少”： + +\[ +threshold = \sum_{seg} |scale_{seg} - 1| +\] + +若本轮 `threshold` 小于历史最佳，则保存当前解作为 `best`（包含 `bestDurations / bestScales / bestWaypointTimes` 等）。 + +### 3.8 收敛判定与段时长更新 + +- 若 `threshold < _threshold`（默认 `1e-3`），认为收敛并提前结束迭代 +- 否则更新每段时长： + +\[ +T_{seg} \leftarrow T_{seg} \cdot scale_{seg} +\] + +并进入下一轮。 + +--- + +## 4. 最终判定（global_scale） + +迭代结束后取历史最优缩放因子的最大值： + +\[ +globalScale=\max_{seg}(scale_{seg}) +\] + +若启用强制判定（`enforceFinalScale=true`）且： + +\[ +globalScale > 1 + \text{finalScaleTolerance} +\] + +则判定“未收敛/不可执行”并抛异常。默认容差 `finalScaleTolerance=1e-2`，用于容忍 C# spline 与参考实现间的小量数值差异。 + +--- + +## 5. 与“补点/中间点位”的关系（常见误解澄清） + +### 5.1 `ICspPlanner` 不负责生成固定周期的中间点位 + +`ICspPlanner` 的核心工作是 **时间轴规划（retiming）**：在不改变示教点位置的情况下，通过缩放每段时长让样条导数峰值满足约束。 + +固定周期（例如 8ms/16ms）的“中间点位序列”属于 **采样层**： + +- `TrajectorySampler`：按 `samplePeriod` 在样条上取样，得到 `[time, j1..jN]`（关节单位仍为 rad） +- `J519SendTrajectorySampler`：按 `servoPeriod` 生成真实发送序列，用 `speedRatio` 把 `sendTime` 映射到 `trajectoryTime` 并线性插值，再做 `rad -> deg` + +### 5.2 `SelfAdaptIcspPlanner` 才包含“补点”逻辑，但它很粗 + +`self-adapt-icsp` 的补点策略在 `SelfAdaptIcspPlanner` 中：当某些段 `scale > 1 + tolerance` 时，对这些段插入关节空间**中点**再重规划。该策略的目的主要是“救收敛”，不是生成最终稠密序列。 + +--- + +## 6. 后续改造建议（定位落点） + +如果需求是“根据示教点 + v/a/j 限制，直接生成可下发的稠密点位序列”，通常有两条路径： + +1. **保留 ICSP retiming**：继续用 `ICspPlanner` 求时间轴，再在采样层按固定周期生成中间点位（当前架构就是这条路）。此时需要讨论的是采样周期、速度倍率映射、以及是否要对采样序列再做约束校验或二次整形。 +2. **做真正的自适应插点/细分**：把“插点策略”升级为基于约束的细分（而不只是插中点），这更自然的落点是 `SelfAdaptIcspPlanner` 或新增一个“约束驱动细分器”，而不是把稠密点生成塞进 `ICspPlanner`。 + +--- + +## 7. 关联实现位置（便于跳转） + +- 算法入口：`src/Flyshot.Core.Planning/ICspPlanner.cs` +- 自适应补点：`src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs` +- 三次样条实现（clamped-zero + 解析导峰值）：`src/Flyshot.Core.Planning/CubicSplineInterpolator.cs` +- 规划层稠密采样：`src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs` +- J519 实发重采样：`src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs` + diff --git a/src/Flyshot.Core.Planning/Sampling/TrajectoryLimitValidator.cs b/src/Flyshot.Core.Planning/Sampling/TrajectoryLimitValidator.cs new file mode 100644 index 0000000..eb95068 --- /dev/null +++ b/src/Flyshot.Core.Planning/Sampling/TrajectoryLimitValidator.cs @@ -0,0 +1,206 @@ +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Planning.Sampling; + +/// +/// 对最终生成的关节轨迹点做速度、加速度和 Jerk 离散复核。 +/// +public static class TrajectoryLimitValidator +{ + /// + /// 离散差分校验允许的默认浮点容差倍率。 + /// + public const double DefaultLimitTolerance = 1.000001; + + /// + /// 校验弧度制稠密关节轨迹是否满足机器人关节限制。 + /// + /// 机器人约束配置。 + /// 稠密轨迹行，格式为 time + 关节弧度。 + /// 限值容差倍率，用于过滤浮点舍入误差。 + /// 诊断用轨迹名称。 + public static void ValidateDenseJointTrajectory( + RobotProfile robot, + IReadOnlyList> rows, + double toleranceMultiplier = DefaultLimitTolerance, + string? trajectoryName = null) + { + ArgumentNullException.ThrowIfNull(robot); + ArgumentNullException.ThrowIfNull(rows); + ValidateTolerance(toleranceMultiplier); + ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory"); + } + + /// + /// 校验 J519 实际发送采样点是否满足机器人关节限制。 + /// + /// 机器人约束配置。 + /// J519 发送采样点，关节单位为角度。 + /// 限值容差倍率，用于过滤浮点舍入误差。 + /// 诊断用轨迹名称。 + public static void ValidateJ519SendSamples( + RobotProfile robot, + IReadOnlyList samples, + double toleranceMultiplier = DefaultLimitTolerance, + string? trajectoryName = null) + { + ArgumentNullException.ThrowIfNull(robot); + ArgumentNullException.ThrowIfNull(samples); + ValidateTolerance(toleranceMultiplier); + + var rows = new List>(samples.Count); + foreach (var sample in samples) + { + var row = new double[robot.DegreesOfFreedom + 1]; + row[0] = sample.SendTime; + for (var index = 0; index < robot.DegreesOfFreedom; index++) + { + row[index + 1] = DegreesToRadians(sample.JointsDegrees[index]); + } + + rows.Add(row); + } + + ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "j519-send-trajectory"); + } + + /// + /// 校验容差倍率必须为有限正数。 + /// + private static void ValidateTolerance(double toleranceMultiplier) + { + if (toleranceMultiplier <= 0.0 || double.IsNaN(toleranceMultiplier) || double.IsInfinity(toleranceMultiplier)) + { + throw new ArgumentOutOfRangeException(nameof(toleranceMultiplier), "限值容差倍率必须是有限正数。"); + } + } + + /// + /// 对弧度制轨迹行执行统一的离散差分限幅校验。 + /// + private static void ValidateRows( + RobotProfile robot, + IReadOnlyList> rows, + double toleranceMultiplier, + string trajectoryName) + { + double? previousTime = null; + double[]? previousPositions = null; + double[]? previousVelocities = null; + double[]? previousAccelerations = null; + + for (var rowIndex = 0; rowIndex < rows.Count; rowIndex++) + { + var row = rows[rowIndex]; + if (row.Count < robot.DegreesOfFreedom + 1) + { + throw new InvalidOperationException( + $"轨迹 {trajectoryName} 第 {rowIndex + 1} 行关节列数量不足，期望至少 {robot.DegreesOfFreedom + 1} 列，实际 {row.Count} 列。"); + } + + var currentTime = row[0]; + var currentPositions = new double[robot.DegreesOfFreedom]; + for (var index = 0; index < robot.DegreesOfFreedom; index++) + { + currentPositions[index] = row[index + 1]; + } + + if (previousTime is not null && previousPositions is not null) + { + var dt = currentTime - previousTime.Value; + if (dt <= 0.0) + { + throw new InvalidOperationException( + $"轨迹 {trajectoryName} 时间戳必须严格递增，第 {rowIndex + 1} 行 dt={dt:F9}s。"); + } + + var currentVelocities = new double[robot.DegreesOfFreedom]; + var currentAccelerations = new double[robot.DegreesOfFreedom]; + for (var index = 0; index < robot.DegreesOfFreedom; index++) + { + var jointLimit = robot.JointLimits[index]; + currentVelocities[index] = (currentPositions[index] - previousPositions[index]) / dt; + ThrowIfExceeded( + trajectoryName, + rowIndex, + previousTime.Value, + currentTime, + jointLimit.JointName, + "速度", + currentVelocities[index], + jointLimit.VelocityLimit, + toleranceMultiplier); + + currentAccelerations[index] = previousVelocities is null + ? 0.0 + : (currentVelocities[index] - previousVelocities[index]) / dt; + ThrowIfExceeded( + trajectoryName, + rowIndex, + previousTime.Value, + currentTime, + jointLimit.JointName, + "加速度", + currentAccelerations[index], + jointLimit.AccelerationLimit, + toleranceMultiplier); + + if (previousAccelerations is not null) + { + var jerk = (currentAccelerations[index] - previousAccelerations[index]) / dt; + ThrowIfExceeded( + trajectoryName, + rowIndex, + previousTime.Value, + currentTime, + jointLimit.JointName, + "Jerk", + jerk, + jointLimit.JerkLimit, + toleranceMultiplier*4); + } + } + + previousVelocities = currentVelocities; + previousAccelerations = currentAccelerations; + } + + previousTime = currentTime; + previousPositions = currentPositions; + } + } + + /// + /// 当某个差分指标超过限制时抛出包含关节和时间窗的诊断异常。 + /// + private static void ThrowIfExceeded( + string trajectoryName, + int rowIndex, + double previousTime, + double currentTime, + string jointName, + string metricName, + double actual, + double limit, + double toleranceMultiplier) + { + var absActual = Math.Abs(actual); + var effectiveLimit = limit * toleranceMultiplier; + if (absActual <= effectiveLimit) + { + return; + } + + throw new InvalidOperationException( + $"轨迹 {trajectoryName} 第 {rowIndex + 1} 行 {jointName} {metricName}超限: " + + $"time={previousTime:F6}->{currentTime:F6}s, actual={actual:F6}, limit={limit:F6}, ratio={absActual / limit:F4}。"); + } + + /// + /// 角度单位转换：deg -> rad。 + /// + private static double DegreesToRadians(double degrees) + { + return degrees * Math.PI / 180.0; + } +} diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index fdf1e73..21b8496 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -180,6 +180,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable { EnsureConnected(); _bufferSize = bufferSize; + _j519Client.SetSequenceBufferSize(bufferSize); if (IsSimulationMode) { @@ -189,9 +190,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } // 真机模式：按 all-reconnect.pcap 的重连序列启动 RVBUSTSM，暂不发送 StopProg。 - _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); + _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); _commandClient.ResetRobotAsync().GetAwaiter().GetResult(); - _commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult(); + //_commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult(); _commandClient.StartProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); _j519Client.StartMotion(); @@ -575,6 +576,10 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable durationSeconds, servoPeriodSeconds, speedRatio); + TrajectoryLimitValidator.ValidateJ519SendSamples( + _robot!, + samples, + trajectoryName: result.ProgramName); var sampleCount = samples.Count; _logger?.LogInformation( @@ -764,7 +769,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable internal static bool IsJ519ReadyForDenseExecution(FanucJ519Response response) { ArgumentNullException.ThrowIfNull(response); - return response.AcceptsCommand && response.SystemReady; + return response is { AcceptsCommand: true, SystemReady: true }; } /// diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs index c5ba285..7f033c3 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs @@ -24,6 +24,7 @@ public sealed class FanucJ519Client : IDisposable private FanucJ519Response? _latestResponse; private long _slowSendCount; private long _maxReceiveToSendTicks; + private uint _sequenceBufferSize; // 标记 StartMotion 前是否刚装载过新目标，用于区分新命令和上次运动残留目标。 private bool _hasPendingCommandForStart; private bool _motionStarted; @@ -119,6 +120,25 @@ public sealed class FanucJ519Client : IDisposable _logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送"); } + /// + /// 配置状态包驱动回发时附加到机器人 sequence 的前视缓冲深度。 + /// + /// 要附加到状态序号上的缓冲深度。 + public void SetSequenceBufferSize(int bufferSize) + { + ObjectDisposedException.ThrowIf(_disposed, this); + + if (bufferSize < 0) + { + throw new ArgumentOutOfRangeException(nameof(bufferSize), "J519 sequence buffer size 不能为负数。"); + } + + lock (_commandLock) + { + _sequenceBufferSize = (uint)bufferSize; + } + } + /// /// 发送状态输出停止包并停止 J519 命令发送。 /// @@ -194,8 +214,8 @@ public sealed class FanucJ519Client : IDisposable _commandQueueDrainedCompletion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously); _commandHistoryForTests?.AddRange(commands); } - _logger?.LogInformation("J519 命令队列已装载: count={Count}", commands.Count); + _logger?.LogInformation("开始运动前向机器人发送的sequence={Sequence}",_lastSentCommand?.Sequence ?? 0); } /// @@ -526,12 +546,23 @@ public sealed class FanucJ519Client : IDisposable } } + if (command is null) + { + command = TryBuildHoldCommandFromLatestResponse(response); + } + if (command is null) { return; } - FanucJ519Protocol.PackCommandPacket(command, response.Sequence, commandBuffer); + uint sequenceToSend; + lock (_commandLock) + { + sequenceToSend = response.Sequence + _sequenceBufferSize; + } + + FanucJ519Protocol.PackCommandPacket(command, sequenceToSend, commandBuffer); socket.Send(commandBuffer); TrackReceiveToSendLatency(receiveTicks); // _logger?.LogDebug("J519 已回发命令包，seq={Seq}", sequence); @@ -550,6 +581,32 @@ public sealed class FanucJ519Client : IDisposable } } + /// + /// 当当前没有显式目标时，使用最近一帧状态反馈关节角构造保姿命令，维持机器人当前位置。 + /// + /// 当前收到的机器人状态包。 + /// 可用于保姿的临时 J519 命令；若反馈关节不足则返回 null。 + private static FanucJ519Command? TryBuildHoldCommandFromLatestResponse(FanucJ519Response response) + { + if (response.JointDegrees.Count < 6) + { + return null; + } + + // 无运动目标时，持续回发机器人当前反馈关节，保持伺服流与机器人当前位置一致。 + return new FanucJ519Command( + sequence: response.Sequence, + targetJoints: + [ + response.JointDegrees[0], + response.JointDegrees[1], + response.JointDegrees[2], + response.JointDegrees[3], + response.JointDegrees[4], + response.JointDegrees[5] + ]); + } + /// /// 记录状态包到命令包发出的最大耗时和慢发送次数，供低频诊断日志观察调度抖动。 /// diff --git a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs index f297bde..8fcb0a9 100644 --- a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs +++ b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs @@ -163,6 +163,29 @@ public sealed class FanucJ519ClientTests : IDisposable await client.StopMotionAsync(_cts.Token); } + /// + /// 验证配置 J519 buffer size 后，实际回发命令序号会在状态包序号基础上增加该缓冲深度。 + /// + [Fact] + public async Task StartMotion_AddsConfiguredBufferSizeToStatusSequence() + { + using var client = new FanucJ519Client(); + await client.ConnectAsync("127.0.0.1", Port, _cts.Token); + var initResult = await _server.ReceiveAsync(_cts.Token); + + client.SetSequenceBufferSize(8); + client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0])); + client.StartMotion(); + + await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100); + + var result = await _server.ReceiveAsync(_cts.Token); + Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length); + Assert.Equal(108u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4))); + + await client.StopMotionAsync(_cts.Token); + } + /// /// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。 /// @@ -274,6 +297,61 @@ public sealed class FanucJ519ClientTests : IDisposable await client.StopMotionAsync(_cts.Token); } + /// + /// 验证没有显式目标时，会使用最近一帧状态反馈里的关节角持续构造 hold 命令。 + /// + [Fact] + public async Task StartMotion_WithoutExplicitCommand_HoldsLatestResponseJointDegrees() + { + using var client = new FanucJ519Client(); + await client.ConnectAsync("127.0.0.1", Port, _cts.Token); + var initResult = await _server.ReceiveAsync(_cts.Token); + + client.StartMotion(); + + await SendStatusPacketAsync( + initResult.RemoteEndPoint, + sequence: 401, + jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]); + var holdPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token); + + Assert.Equal(FanucJ519Protocol.CommandPacketLength, holdPacket.Buffer.Length); + Assert.Equal(401u, BinaryPrimitives.ReadUInt32BigEndian(holdPacket.Buffer.AsSpan(0x08, 4))); + Assert.Equal(10.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x1c, 4)), precision: 6); + Assert.Equal(20.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x20, 4)), precision: 6); + Assert.Equal(60.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x30, 4)), precision: 6); + + await client.StopMotionAsync(_cts.Token); + } + + /// + /// 验证存在显式目标时，状态反馈生成的 hold 命令不会覆盖当前待发送目标。 + /// + [Fact] + public async Task StartMotion_WithExplicitCommand_PrefersExplicitTargetOverHoldCommand() + { + using var client = new FanucJ519Client(); + await client.ConnectAsync("127.0.0.1", Port, _cts.Token); + var initResult = await _server.ReceiveAsync(_cts.Token); + + client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0])); + client.StartMotion(); + + await SendStatusPacketAsync( + initResult.RemoteEndPoint, + sequence: 402, + jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]); + var commandPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token); + + Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandPacket.Buffer.Length); + Assert.Equal(402u, BinaryPrimitives.ReadUInt32BigEndian(commandPacket.Buffer.AsSpan(0x08, 4))); + Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x1c, 4)), precision: 6); + Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x20, 4)), precision: 6); + Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x30, 4)), precision: 6); + + await client.StopMotionAsync(_cts.Token); + } + /// /// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。 /// @@ -356,13 +434,27 @@ public sealed class FanucJ519ClientTests : IDisposable /// /// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。 /// - private async Task SendStatusPacketAsync(IPEndPoint clientEndpoint, uint sequence) + private async Task SendStatusPacketAsync( + IPEndPoint clientEndpoint, + uint sequence, + IReadOnlyList? jointDegrees = null) { var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength]; BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0); BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1); BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence); responsePacket[0x0c] = 15; + + if (jointDegrees is not null) + { + for (var index = 0; index < Math.Min(6, jointDegrees.Count); index++) + { + BinaryPrimitives.WriteSingleBigEndian( + responsePacket.AsSpan(0x3c + index * sizeof(float), sizeof(float)), + (float)jointDegrees[index]); + } + } + await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token); } diff --git a/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs b/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs index ac4a9c0..afa8c54 100644 --- a/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs +++ b/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs @@ -1,6 +1,7 @@ using Flyshot.Core.Config; using Flyshot.Core.Domain; using Flyshot.Core.Planning; +using Flyshot.Core.Planning.Sampling; using Flyshot.Core.Triggering; namespace Flyshot.Core.Tests; @@ -171,6 +172,28 @@ public sealed class PlanningCompatibilityTests Assert.Equal(2, doEvent.HoldCycles); } + /// + /// 验证稠密轨迹生成后会复核离散加速度，避免采样结果绕过 ICSP 段 scale 校验。 + /// + [Fact] + public void TrajectoryLimitValidator_Throws_WhenDenseTrajectoryAccelerationExceedsLimit() + { + var robot = CreateRobotProfile([100.0], [10.0], [1000.0]); + + var exception = Assert.Throws(() => + TrajectoryLimitValidator.ValidateDenseJointTrajectory( + robot, + [ + [0.0, 0.0], + [0.008, 0.0], + [0.016, 0.01] + ], + trajectoryName: "dense-acceleration-check")); + + Assert.Contains("加速度", exception.Message); + Assert.Contains("dense-acceleration-check", exception.Message); + } + /// /// 构造一个最小 RobotProfile，便于规划层单元测试聚焦在时间轴逻辑上。 /// diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index 41f4d1f..9468a80 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -914,7 +914,7 @@ public sealed class RuntimeOrchestrationTests } /// - /// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位，并应用当前 speed_ratio。 + /// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位，并按执行侧稠密轨迹时长应用当前 speed_ratio。 /// [Fact] public void ControllerClientCompatService_SaveTrajectoryInfo_ExportsActualSendRowsWithSpeedRatio() @@ -945,8 +945,10 @@ public sealed class RuntimeOrchestrationTests var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray(); var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray(); - var duration = double.Parse(File.ReadLines(Path.Combine(outputDir, "JointTraj.txt")).Last().Split(' ')[0], CultureInfo.InvariantCulture); - var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (duration / (0.008 * 0.5)) - 1e-9)) + 1; + var executionDuration = double.Parse( + File.ReadLines(Path.Combine(outputDir, "JointDetialTraj.txt")).Last().Split(' ')[0], + CultureInfo.InvariantCulture); + var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (0.008 * 0.5)) - 1e-9)) + 1; Assert.Equal(expectedRows, pointRows.Length); Assert.Equal(expectedRows, timingRows.Length); @@ -961,6 +963,54 @@ public sealed class RuntimeOrchestrationTests } } + /// + /// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自执行侧 8ms 稠密轨迹的 16ms 视图， + /// 而不是再次从 PlannedTrajectory 独立重采样。 + /// + [Fact] + public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExecutionDenseDownsample() + { + var fixture = LoadUttcMs11RuntimeFixture(); + var configRoot = CreateTempConfigRoot(); + try + { + var options = new ControllerClientCompatOptions { ConfigRoot = configRoot }; + var writer = new FlyshotTrajectoryArtifactWriter(options, new RobotModelLoader()); + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var bundle = orchestrator.PlanUploadedFlyshot( + fixture.Robot, + fixture.Uploaded, + settings: EnableSmoothStartStopTiming(fixture.Settings), + planningSpeedScale: 1.0); + + writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, bundle, speedRatio: 1.0); + + var outputDir = Path.Combine(configRoot, "Data", "UTTC_MS11"); + var exportedRows = File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt")) + .Select(ParseSpaceSeparatedDoubles) + .ToArray(); + var expectedRows = DownsampleDenseRows( + bundle.Result.DenseJointTrajectory!, + samplePeriodSeconds: 0.016) + .Select(static row => row.ToArray()) + .ToArray(); + + Assert.Equal(expectedRows.Length, exportedRows.Length); + for (var rowIndex = 0; rowIndex < expectedRows.Length; rowIndex++) + { + Assert.Equal(expectedRows[rowIndex].Length, exportedRows[rowIndex].Length); + for (var columnIndex = 0; columnIndex < expectedRows[rowIndex].Length; columnIndex++) + { + Assert.Equal(expectedRows[rowIndex][columnIndex], exportedRows[rowIndex][columnIndex], precision: 6); + } + } + } + finally + { + Directory.Delete(configRoot, recursive: true); + } + } + /// /// 创建只包含当前支持机器人 JSON 模型和 RobotConfig.json 的临时运行配置根。 /// @@ -1276,6 +1326,40 @@ public sealed class RuntimeOrchestrationTests .ToArray(); } + /// + /// 将 8ms 执行稠密轨迹按目标周期抽稀为低频视图，并始终保留终点。 + /// + private static IReadOnlyList> DownsampleDenseRows( + IReadOnlyList> denseRows, + double samplePeriodSeconds) + { + var result = new List>(); + var epsilon = 1e-6; + var nextSampleTime = 0.0; + + foreach (var row in denseRows) + { + var sampleTime = row[0]; + if (sampleTime + epsilon < nextSampleTime) + { + continue; + } + + if (Math.Abs(sampleTime - nextSampleTime) <= epsilon || sampleTime.Equals(0.0)) + { + result.Add(row); + nextSampleTime += samplePeriodSeconds; + } + } + + if (result.Count == 0 || !ReferenceEquals(result[^1], denseRows[^1])) + { + result.Add(denseRows[^1]); + } + + return result; + } + /// /// 用真实可运行轨迹前 4 个采样点拟合局部三次曲线，返回相对首点时间的系数。 /// 曲线形式为 p(t)=c3*t^3+c2*t^2+c1*t+c0，单位保持输入文件的角度制。