♻️ refactor(compat): 替换 MoveJoint 时间律为解析式 7 阶平滑函数并添加离散限位校验
* 将预捕获 alpha 数据表替换为解析式 7 阶平滑点到点时间律 s(u)=35u⁴-84u⁵+70u⁶-20u⁷，形状系数按 1~3 阶导数最大值重算 * 新增离散限位校验：按真实 8ms 采样点反算速度/加速度/jerk， 不满足时自动拉长总时长后重采样，最多迭代 10000 次 * 实发轨迹落盘：ActualSendJointTraj.txt（角度制）、 ActualSendJerkStats.txt（点间跃度统计），按时间目录归档 * J519 AcceptsCommand 门控：只有机器人就绪时才发送下一帧， 减少无效下发；状态日志附带最近发送目标关节轴 * FanucControllerRuntime 构造函数改为必选 ILogger 注入， 确保 DI 解析时稳定拿到日志实例 * LegacyHttpApiController 移除已废弃的 ConnectServer 调用， EnableRobot 参数从 2 改为 4 * 新增跃度报警分析文档和六轴限值表，补充反馈远离拒绝测试 Co-authored-by: Copilot <copilot@github.com>
This commit is contained in:
@@ -112,6 +112,15 @@ flyshot-replacement/
|
||||
- 所有静态变量都必须提供 XML 注释。
|
||||
- 关键代码块必须补充单行注释，说明该段逻辑为什么存在、在做什么，不允许只写空泛注释。
|
||||
|
||||
### 4.5 机器人模型字段约定
|
||||
|
||||
- 当任务涉及六轴 `velocity / acceleration / jerk` 来源时，默认先查看机器人模型文件中的 `joint.limit`，不要先从抓包、导出轨迹或聊天记录反推。
|
||||
- 当前仓库约定：`velocity_eff = velocity_base`，`acceleration_eff = acceleration_base * acc_limit`，`jerk_eff = jerk_base * jerk_limit`。
|
||||
- `acc_limit / jerk_limit` 来自运行时 `RobotConfig.json`，它们是全局倍率，不是每轴单独配置。
|
||||
- 模型里的 `limit.effort` 目前只能当静态模型字段记录，不能直接当现场真实电流。
|
||||
- 如果用户问“电流是不是从这个模型文件提取的”，默认先明确区分：模型里的 `effort` 不等于 J519 反馈里的电机电流。
|
||||
- 相关固定表格文档见 `docs/robot-joint-limit-table-20260505.md`。
|
||||
|
||||
## 5. 构建与验证命令
|
||||
|
||||
在当前环境中，推荐使用下面两条命令：
|
||||
|
||||
435
analysis/calc_jointdetail_acceleration.py
Normal file
435
analysis/calc_jointdetail_acceleration.py
Normal file
@@ -0,0 +1,435 @@
|
||||
#!/usr/bin/env python3
|
||||
"""计算 JointDetialTraj 类文件的速度 / 加速度 / 跃度峰值，并与当前生效轴限位对比。
|
||||
|
||||
输入格式:
|
||||
time joint1 joint2 ... jointN
|
||||
|
||||
本脚本采用的规则:
|
||||
1. 将轨迹按离散时间采样点读取，允许时间轴非等间隔。
|
||||
2. 自动推断角度单位:
|
||||
- 任一关节绝对值超过 2*pi*1.5，则按 degree 处理
|
||||
- 否则按 radian 处理
|
||||
3. 使用后向差分计算导数:
|
||||
v_i = (q_i - q_{i-1}) / dt_i
|
||||
a_i = (v_i - v_{i-1}) / dt_i
|
||||
j_i = (a_i - a_{i-1}) / dt_i
|
||||
4. 所有导数量统一换算成 rad 基单位，再与当前生效的机器人限值比较。
|
||||
|
||||
当前生效限值来源:
|
||||
.robot limit.velocity
|
||||
.robot limit.acceleration * RobotConfig.robot.acc_limit
|
||||
.robot limit.jerk * RobotConfig.robot.jerk_limit
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Iterable
|
||||
|
||||
|
||||
AUTO_DEG_THRESHOLD = 2.0 * math.pi * 1.5
|
||||
DEFAULT_VELOCITY_LIMITS = [6.45, 5.41, 7.15, 9.59, 9.51, 17.45]
|
||||
DEFAULT_ACCELERATION_LIMITS = [26.90, 22.54, 29.81, 39.99, 39.63, 72.72]
|
||||
DEFAULT_JERK_LIMITS = [224.22, 187.86, 248.46, 333.30, 330.27, 606.01]
|
||||
DEFAULT_JOINT_NAMES = [f"Joint{index}" for index in range(1, 7)]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointLimit:
|
||||
name: str
|
||||
velocity: float
|
||||
acceleration: float
|
||||
jerk: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PeakMetric:
|
||||
joint_name: str
|
||||
axis_index: int
|
||||
window_start: float
|
||||
window_end: float
|
||||
row_number: int
|
||||
metric_native: float
|
||||
metric_rad: float
|
||||
effective_limit_rad: float
|
||||
|
||||
@property
|
||||
def ratio_vs_limit(self) -> float:
|
||||
return abs(self.metric_rad) / self.effective_limit_rad
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EffectiveLimits:
|
||||
joints: list[JointLimit]
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Calculate velocity / acceleration / jerk peaks from JointDetialTraj.txt and compare with built-in effective robot limits."
|
||||
)
|
||||
parser.add_argument("joint_detail", type=Path, help="Path to JointDetialTraj.txt")
|
||||
parser.add_argument(
|
||||
"--limit-csv",
|
||||
type=Path,
|
||||
default=None,
|
||||
help="Optional CSV file with columns: Joint,Velocity,Acceleration,Jerk . If omitted, use built-in 1/1 effective limits.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--unit",
|
||||
choices=("auto", "rad", "deg"),
|
||||
default="auto",
|
||||
help="Input joint-angle unit. Default: auto.",
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def resolve_path(path: Path) -> Path:
|
||||
return path if path.is_absolute() else (Path.cwd() / path).resolve()
|
||||
|
||||
|
||||
def read_joint_rows(path: Path) -> list[list[float]]:
|
||||
rows: list[list[float]] = []
|
||||
for raw_line in path.read_text(encoding="utf-8").splitlines():
|
||||
line = raw_line.strip()
|
||||
if not line:
|
||||
continue
|
||||
rows.append([float(part) for part in line.split()])
|
||||
|
||||
if len(rows) < 4:
|
||||
raise ValueError(f"{path} must contain at least 4 rows to calculate jerk.")
|
||||
|
||||
width = len(rows[0])
|
||||
if width < 3:
|
||||
raise ValueError(f"{path} must contain time + at least 2 joint columns.")
|
||||
|
||||
for index, row in enumerate(rows, start=1):
|
||||
if len(row) != width:
|
||||
raise ValueError(f"{path} line {index} has inconsistent column count.")
|
||||
|
||||
return rows
|
||||
|
||||
|
||||
def infer_unit(rows: Iterable[list[float]], requested_unit: str) -> str:
|
||||
if requested_unit != "auto":
|
||||
return requested_unit
|
||||
|
||||
max_abs_joint = max(abs(value) for row in rows for value in row[1:])
|
||||
return "deg" if max_abs_joint > AUTO_DEG_THRESHOLD else "rad"
|
||||
|
||||
|
||||
def read_limit_csv(path: Path) -> list[JointLimit]:
|
||||
rows = [line.strip() for line in path.read_text(encoding="utf-8").splitlines() if line.strip()]
|
||||
if len(rows) < 2:
|
||||
raise ValueError(f"{path} must contain a header and at least one data row.")
|
||||
|
||||
header = [part.strip().lower() for part in rows[0].split(",")]
|
||||
expected = ["joint", "velocity", "acceleration", "jerk"]
|
||||
if header != expected:
|
||||
raise ValueError(f"{path} header must be: Joint,Velocity,Acceleration,Jerk")
|
||||
|
||||
limits: list[JointLimit] = []
|
||||
for row_index, row in enumerate(rows[1:], start=2):
|
||||
parts = [part.strip() for part in row.split(",")]
|
||||
if len(parts) != 4:
|
||||
raise ValueError(f"{path} line {row_index} must contain 4 columns.")
|
||||
limits.append(
|
||||
JointLimit(
|
||||
name=parts[0],
|
||||
velocity=float(parts[1]),
|
||||
acceleration=float(parts[2]),
|
||||
jerk=float(parts[3]),
|
||||
)
|
||||
)
|
||||
return limits
|
||||
|
||||
|
||||
def load_effective_limits(limit_csv_path: Path | None) -> EffectiveLimits:
|
||||
if limit_csv_path is not None:
|
||||
limits = read_limit_csv(resolve_path(limit_csv_path))
|
||||
else:
|
||||
limits = [
|
||||
JointLimit(
|
||||
name=name,
|
||||
velocity=velocity,
|
||||
acceleration=acceleration,
|
||||
jerk=jerk,
|
||||
)
|
||||
for name, velocity, acceleration, jerk in zip(
|
||||
DEFAULT_JOINT_NAMES,
|
||||
DEFAULT_VELOCITY_LIMITS,
|
||||
DEFAULT_ACCELERATION_LIMITS,
|
||||
DEFAULT_JERK_LIMITS,
|
||||
strict=True,
|
||||
)
|
||||
]
|
||||
|
||||
return EffectiveLimits(joints=limits)
|
||||
|
||||
|
||||
def to_radians(value: float, unit: str) -> float:
|
||||
return math.radians(value) if unit == "deg" else value
|
||||
|
||||
|
||||
def to_native_from_rad(value: float, unit: str) -> float:
|
||||
return math.degrees(value) if unit == "deg" else value
|
||||
|
||||
|
||||
def calculate_velocity_peaks(rows: list[list[float]], unit: str, limits: list[JointLimit]) -> list[PeakMetric]:
|
||||
joint_count = len(rows[0]) - 1
|
||||
if joint_count != len(limits):
|
||||
raise ValueError(f"Joint column count ({joint_count}) does not match robot limit count ({len(limits)}).")
|
||||
|
||||
peaks: list[PeakMetric | None] = [None] * joint_count
|
||||
|
||||
for row_index in range(1, len(rows)):
|
||||
previous = rows[row_index - 1]
|
||||
current = rows[row_index]
|
||||
dt = current[0] - previous[0]
|
||||
if dt <= 0.0:
|
||||
raise ValueError(f"Non-positive dt at line {row_index + 1}: {dt}")
|
||||
|
||||
for joint_index in range(joint_count):
|
||||
dq_native = current[joint_index + 1] - previous[joint_index + 1]
|
||||
dq_rad = to_radians(dq_native, unit)
|
||||
velocity_rad = dq_rad / dt
|
||||
velocity_native = to_native_from_rad(velocity_rad, unit)
|
||||
|
||||
candidate = PeakMetric(
|
||||
joint_name=limits[joint_index].name,
|
||||
axis_index=joint_index + 1,
|
||||
window_start=previous[0],
|
||||
window_end=current[0],
|
||||
row_number=row_index + 1,
|
||||
metric_native=velocity_native,
|
||||
metric_rad=velocity_rad,
|
||||
effective_limit_rad=limits[joint_index].velocity,
|
||||
)
|
||||
|
||||
current_peak = peaks[joint_index]
|
||||
if current_peak is None or abs(candidate.metric_rad) > abs(current_peak.metric_rad):
|
||||
peaks[joint_index] = candidate
|
||||
|
||||
return [peak for peak in peaks if peak is not None]
|
||||
|
||||
|
||||
def calculate_acceleration_peaks(rows: list[list[float]], unit: str, limits: list[JointLimit]) -> list[PeakMetric]:
|
||||
joint_count = len(rows[0]) - 1
|
||||
if joint_count != len(limits):
|
||||
raise ValueError(f"Joint column count ({joint_count}) does not match robot limit count ({len(limits)}).")
|
||||
|
||||
velocities_rad: list[list[float]] = []
|
||||
velocity_windows: list[tuple[float, float, int]] = []
|
||||
|
||||
for row_index in range(1, len(rows)):
|
||||
previous = rows[row_index - 1]
|
||||
current = rows[row_index]
|
||||
dt = current[0] - previous[0]
|
||||
if dt <= 0.0:
|
||||
raise ValueError(f"Non-positive dt at line {row_index + 1}: {dt}")
|
||||
|
||||
velocity_row = []
|
||||
for joint_index in range(joint_count):
|
||||
dq_native = current[joint_index + 1] - previous[joint_index + 1]
|
||||
dq_rad = to_radians(dq_native, unit)
|
||||
velocity_row.append(dq_rad / dt)
|
||||
|
||||
velocities_rad.append(velocity_row)
|
||||
velocity_windows.append((previous[0], current[0], row_index + 1))
|
||||
|
||||
peaks: list[PeakMetric | None] = [None] * joint_count
|
||||
|
||||
for velocity_index in range(1, len(velocities_rad)):
|
||||
dt = velocity_windows[velocity_index][1] - velocity_windows[velocity_index][0]
|
||||
for joint_index in range(joint_count):
|
||||
acceleration_rad = (velocities_rad[velocity_index][joint_index] - velocities_rad[velocity_index - 1][joint_index]) / dt
|
||||
acceleration_native = to_native_from_rad(acceleration_rad, unit)
|
||||
|
||||
candidate = PeakMetric(
|
||||
joint_name=limits[joint_index].name,
|
||||
axis_index=joint_index + 1,
|
||||
window_start=velocity_windows[velocity_index][0],
|
||||
window_end=velocity_windows[velocity_index][1],
|
||||
row_number=velocity_windows[velocity_index][2],
|
||||
metric_native=acceleration_native,
|
||||
metric_rad=acceleration_rad,
|
||||
effective_limit_rad=limits[joint_index].acceleration,
|
||||
)
|
||||
|
||||
current_peak = peaks[joint_index]
|
||||
if current_peak is None or abs(candidate.metric_rad) > abs(current_peak.metric_rad):
|
||||
peaks[joint_index] = candidate
|
||||
|
||||
return [peak for peak in peaks if peak is not None]
|
||||
|
||||
|
||||
def calculate_jerk_peaks(rows: list[list[float]], unit: str, limits: list[JointLimit]) -> list[PeakMetric]:
|
||||
joint_count = len(rows[0]) - 1
|
||||
if joint_count != len(limits):
|
||||
raise ValueError(f"Joint column count ({joint_count}) does not match robot limit count ({len(limits)}).")
|
||||
|
||||
velocities_rad: list[list[float]] = []
|
||||
velocity_windows: list[tuple[float, float, int]] = []
|
||||
for row_index in range(1, len(rows)):
|
||||
previous = rows[row_index - 1]
|
||||
current = rows[row_index]
|
||||
dt = current[0] - previous[0]
|
||||
if dt <= 0.0:
|
||||
raise ValueError(f"Non-positive dt at line {row_index + 1}: {dt}")
|
||||
|
||||
velocity_row = []
|
||||
for joint_index in range(joint_count):
|
||||
dq_native = current[joint_index + 1] - previous[joint_index + 1]
|
||||
dq_rad = to_radians(dq_native, unit)
|
||||
velocity_row.append(dq_rad / dt)
|
||||
|
||||
velocities_rad.append(velocity_row)
|
||||
velocity_windows.append((previous[0], current[0], row_index + 1))
|
||||
|
||||
accelerations_rad: list[list[float]] = []
|
||||
acceleration_windows: list[tuple[float, float, int]] = []
|
||||
for velocity_index in range(1, len(velocities_rad)):
|
||||
dt = velocity_windows[velocity_index][1] - velocity_windows[velocity_index][0]
|
||||
acceleration_row = []
|
||||
for joint_index in range(joint_count):
|
||||
acceleration_row.append((velocities_rad[velocity_index][joint_index] - velocities_rad[velocity_index - 1][joint_index]) / dt)
|
||||
accelerations_rad.append(acceleration_row)
|
||||
acceleration_windows.append((velocity_windows[velocity_index][0], velocity_windows[velocity_index][1], velocity_windows[velocity_index][2]))
|
||||
|
||||
peaks: list[PeakMetric | None] = [None] * joint_count
|
||||
|
||||
for acceleration_index in range(1, len(accelerations_rad)):
|
||||
dt = acceleration_windows[acceleration_index][1] - acceleration_windows[acceleration_index][0]
|
||||
for joint_index in range(joint_count):
|
||||
jerk_rad = (accelerations_rad[acceleration_index][joint_index] - accelerations_rad[acceleration_index - 1][joint_index]) / dt
|
||||
jerk_native = to_native_from_rad(jerk_rad, unit)
|
||||
|
||||
candidate = PeakMetric(
|
||||
joint_name=limits[joint_index].name,
|
||||
axis_index=joint_index + 1,
|
||||
window_start=acceleration_windows[acceleration_index][0],
|
||||
window_end=acceleration_windows[acceleration_index][1],
|
||||
row_number=acceleration_windows[acceleration_index][2],
|
||||
metric_native=jerk_native,
|
||||
metric_rad=jerk_rad,
|
||||
effective_limit_rad=limits[joint_index].jerk,
|
||||
)
|
||||
|
||||
current_peak = peaks[joint_index]
|
||||
if current_peak is None or abs(candidate.metric_rad) > abs(current_peak.metric_rad):
|
||||
peaks[joint_index] = candidate
|
||||
|
||||
return [peak for peak in peaks if peak is not None]
|
||||
|
||||
|
||||
def format_table(peaks: list[PeakMetric], native_unit: str, rad_unit: str, limit_header: str) -> str:
|
||||
lines = [
|
||||
f"{'Joint':<8} {'Window(s)':<20} {'Line':>6} {'Peak(' + native_unit + ')':>18} {'Peak(' + rad_unit + ')':>18} {limit_header:>20} {'Ratio':>10}",
|
||||
"-" * 108,
|
||||
]
|
||||
for peak in peaks:
|
||||
lines.append(
|
||||
f"{peak.joint_name:<8} "
|
||||
f"{peak.window_start:>7.6f}->{peak.window_end:<10.6f} "
|
||||
f"{peak.row_number:>6} "
|
||||
f"{peak.metric_native:>18.6f} "
|
||||
f"{peak.metric_rad:>18.6f} "
|
||||
f"{peak.effective_limit_rad:>20.6f} "
|
||||
f"{peak.ratio_vs_limit:>10.4f}"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def print_metric_section(title: str, peaks: list[PeakMetric], unit: str, native_suffix: str, rad_suffix: str, limit_header: str) -> None:
|
||||
print(build_metric_section(title, peaks, unit, native_suffix, rad_suffix, limit_header))
|
||||
|
||||
|
||||
def build_metric_section(title: str, peaks: list[PeakMetric], unit: str, native_suffix: str, rad_suffix: str, limit_header: str) -> str:
|
||||
native_unit = f"deg/{native_suffix}" if unit == "deg" else f"rad/{native_suffix}"
|
||||
rad_unit = f"rad/{rad_suffix}"
|
||||
lines = [title, format_table(peaks, native_unit, rad_unit, limit_header)]
|
||||
worst = max(peaks, key=lambda item: item.ratio_vs_limit)
|
||||
metric_key = title.lower().replace(" ", "_")
|
||||
lines.append(
|
||||
f"worst_{metric_key}="
|
||||
f"{worst.joint_name}, window={worst.window_start:.6f}->{worst.window_end:.6f}, "
|
||||
f"peak_rad={worst.metric_rad:.6f}, limit_rad={worst.effective_limit_rad:.6f}, "
|
||||
f"ratio={worst.ratio_vs_limit:.4f}"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def build_report_text(
|
||||
joint_detail_path: Path,
|
||||
rows: list[list[float]],
|
||||
unit: str,
|
||||
max_abs_joint: float,
|
||||
limit_source_text: str,
|
||||
velocity_peaks: list[PeakMetric],
|
||||
acceleration_peaks: list[PeakMetric],
|
||||
jerk_peaks: list[PeakMetric],
|
||||
) -> str:
|
||||
lines = [
|
||||
f"joint_detail={joint_detail_path}",
|
||||
limit_source_text,
|
||||
f"row_count={len(rows)}",
|
||||
f"joint_count={len(rows[0]) - 1}",
|
||||
f"inferred_unit={unit}",
|
||||
f"max_abs_joint_value={max_abs_joint:.6f}",
|
||||
"",
|
||||
build_metric_section("Velocity Peaks", velocity_peaks, unit, "s", "s", "VelLimit(rad/s)"),
|
||||
"",
|
||||
build_metric_section("Acceleration Peaks", acceleration_peaks, unit, "s^2", "s^2", "AccLimit(rad/s^2)"),
|
||||
"",
|
||||
build_metric_section("Jerk Peaks", jerk_peaks, unit, "s^3", "s^3", "JerkLimit(rad/s^3)"),
|
||||
"",
|
||||
]
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
joint_detail_path = resolve_path(args.joint_detail)
|
||||
rows = read_joint_rows(joint_detail_path)
|
||||
unit = infer_unit(rows, args.unit)
|
||||
limits_info = load_effective_limits(args.limit_csv)
|
||||
|
||||
velocity_peaks = calculate_velocity_peaks(rows, unit, limits_info.joints)
|
||||
acceleration_peaks = calculate_acceleration_peaks(rows, unit, limits_info.joints)
|
||||
jerk_peaks = calculate_jerk_peaks(rows, unit, limits_info.joints)
|
||||
|
||||
max_abs_joint = max(abs(value) for row in rows for value in row[1:])
|
||||
|
||||
if args.limit_csv is None:
|
||||
limit_source_text = "limit_source=built-in fixed effective limits (acc_limit=1, jerk_limit=1)"
|
||||
else:
|
||||
limit_source_text = f"limit_source_csv={resolve_path(args.limit_csv)}"
|
||||
|
||||
report_text = build_report_text(
|
||||
joint_detail_path=joint_detail_path,
|
||||
rows=rows,
|
||||
unit=unit,
|
||||
max_abs_joint=max_abs_joint,
|
||||
limit_source_text=limit_source_text,
|
||||
velocity_peaks=velocity_peaks,
|
||||
acceleration_peaks=acceleration_peaks,
|
||||
jerk_peaks=jerk_peaks,
|
||||
)
|
||||
|
||||
print(report_text, end="")
|
||||
|
||||
output_path = joint_detail_path.with_suffix(".analysis.txt")
|
||||
try:
|
||||
output_path.write_text(report_text, encoding="utf-8")
|
||||
print(f"saved_report={output_path}")
|
||||
except PermissionError as error:
|
||||
print(f"save_report_failed={output_path}")
|
||||
print(f"save_report_error={error}")
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
7
analysis/segment_02_acc_jerk_summary.csv
Normal file
7
analysis/segment_02_acc_jerk_summary.csv
Normal file
@@ -0,0 +1,7 @@
|
||||
Axis,AccPeakRadPerS2,AccLimitRadPerS2,AccRatio,AccWindowStartS,AccWindowEndS,AccLine,JerkPeakRadPerS3,JerkLimitRadPerS3,JerkRatio,JerkWindowStartS,JerkWindowEndS,JerkLine
|
||||
Joint1,16.638678,26.900000,0.618538,0.128012,0.135939,18,902.687973,224.220000,4.025903,6.312082,6.320127,791
|
||||
Joint2,14.521836,22.540000,0.644270,3.088013,3.096086,388,888.335197,187.860000,4.728709,2.904052,2.912069,365
|
||||
Joint3,14.267221,29.810000,0.478605,0.128012,0.135939,18,728.505873,248.460000,2.932085,0.135939,0.143989,19
|
||||
Joint4,-34.694506,39.990000,0.867580,6.832125,6.840105,856,-2222.596524,333.300000,6.668456,6.312082,6.320127,791
|
||||
Joint5,-16.329775,39.630000,0.412056,6.840105,6.848111,857,842.738923,330.270000,2.551667,6.936077,6.944096,869
|
||||
Joint6,34.766065,72.720000,0.478081,1.392021,1.399995,176,2678.050822,606.010000,4.419153,6.312082,6.320127,791
|
||||
|
136
docs/move-joint-jerk-comparison-20260505.md
Normal file
136
docs/move-joint-jerk-comparison-20260505.md
Normal file
@@ -0,0 +1,136 @@
|
||||
# MoveJoint 失败样本六轴限值与 ActualSendJerkStats 对比
|
||||
|
||||
记录时间：2026-05-05
|
||||
|
||||
## 1. 目的
|
||||
|
||||
本文档固定记录以下三类证据，避免后续继续混用测试基线、旧文档结论和当前运行目录中的真实模型数据：
|
||||
|
||||
- 当前运行目录 `.robot` 模型中的六轴基础 `velocity / acceleration / jerk`
|
||||
- 当前运行目录 `RobotConfig.json` 中的 `acc_limit / jerk_limit`
|
||||
- 当前失败样本 `ActualSendJerkStats.txt` 中的逐轴实发跃度峰值
|
||||
|
||||
本次样本对应目录：
|
||||
|
||||
- `.robot`：`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/Models/LR_Mate_200iD_7L.robot`
|
||||
- 配置：`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/RobotConfig.json`
|
||||
- 实发跃度：`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/Data/move-joint/DenseSend/20260505_203416_563/ActualSendJerkStats.txt`
|
||||
- 抓包：`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/Data/move-joint/DenseSend/20260505_203416_563/移动点 跃度过大.pcap`
|
||||
|
||||
## 2. 当前运行模型的真实六轴限值
|
||||
|
||||
当前仓库运行时通过 `RobotModelLoader.LoadProfile(...)` 从 `.robot` 中读取每轴 `limit.velocity / limit.acceleration / limit.jerk`，然后只对加速度和 jerk 叠加 `RobotConfig.json` 的全局倍率：
|
||||
|
||||
- `velocity_eff = velocity_base`
|
||||
- `acceleration_eff = acceleration_base * acc_limit`
|
||||
- `jerk_eff = jerk_base * jerk_limit`
|
||||
|
||||
当前运行目录 `RobotConfig.json` 中：
|
||||
|
||||
- `acc_limit = 0.74`
|
||||
- `jerk_limit = 0.74`
|
||||
|
||||
按当前运行目录真实模型解出的六轴基础值与生效值如下：
|
||||
|
||||
| Joint | vel_base | acc_base | jerk_base | vel_eff | acc_eff | jerk_eff(rad/s^3) | jerk_eff(deg/s^3) |
|
||||
| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
|
||||
| Joint1 | 6.45 | 26.90 | 224.22 | 6.45 | 19.9060 | 165.9228 | 9506.6762 |
|
||||
| Joint2 | 5.41 | 22.54 | 187.86 | 5.41 | 16.6796 | 139.0164 | 7965.0530 |
|
||||
| Joint3 | 7.15 | 29.81 | 248.46 | 7.15 | 22.0594 | 183.8604 | 10534.4249 |
|
||||
| Joint4 | 9.59 | 39.99 | 333.30 | 9.59 | 29.5926 | 246.6420 | 14131.5457 |
|
||||
| Joint5 | 9.51 | 39.63 | 330.27 | 9.51 | 29.3262 | 244.3998 | 14003.0771 |
|
||||
| Joint6 | 17.45 | 72.72 | 606.01 | 17.45 | 53.8128 | 448.4474 | 25694.1434 |
|
||||
|
||||
重要结论：
|
||||
|
||||
- 当前运行目录中，`Joint1.jerk_base` 不是测试基线里常见的 `272.7`，而是 `224.22`。
|
||||
- 因此当前样本的 `Joint1` 生效 jerk 上限应按 `224.22 * 0.74 = 165.9228 rad/s^3` 计算。
|
||||
|
||||
## 3. ActualSendJerkStats 的单位边界
|
||||
|
||||
`ActualSendJerkStats.txt` 的代码注释写的是 `rad/s^3`，但当前实现里真实输入不是弧度，而是角度：
|
||||
|
||||
1. `SampleDenseJointTrajectoryDegrees(...)` 先把轨迹点从 `rad` 转成 `deg`
|
||||
2. `BuildDenseSendJointRow(...)` 把这组角度制关节写入 `ActualSendJointTraj.txt`
|
||||
3. `BuildDenseSendJerkRow(...)` 再直接基于这组角度制关节做三阶差分
|
||||
|
||||
因此当前这份 `ActualSendJerkStats.txt` 的逐轴跃度应按以下方式理解：
|
||||
|
||||
- 文本中的数值口径：`deg/s^3`
|
||||
- 若要与 `.robot` / `RobotProfile` 中的 jerk limit 比较，需要先换算为 `rad/s^3`
|
||||
- 换算公式：`jerk_rad = jerk_deg * π / 180`
|
||||
|
||||
## 4. 全文件逐轴最大跃度对比
|
||||
|
||||
扫描整份 `ActualSendJerkStats.txt` 后，各轴绝对值最大跃度如下：
|
||||
|
||||
| Joint | peak window(s) | peak line | peak actual(deg/s^3) | peak actual(rad/s^3) | jerk_eff(rad/s^3) | peak/limit |
|
||||
| --- | --- | ---: | ---: | ---: | ---: | ---: |
|
||||
| Joint1 | `1.056 -> 1.064` | 133 | 21868.115990 | 381.670625 | 165.922800 | 2.3003 |
|
||||
| Joint2 | `1.056 -> 1.064` | 133 | 40.271793 | 0.702875 | 139.016400 | 0.0051 |
|
||||
| Joint3 | `1.056 -> 1.064` | 133 | 98.314401 | 1.715910 | 183.860400 | 0.0093 |
|
||||
| Joint4 | `1.056 -> 1.064` | 133 | 0.207266 | 0.003617 | 246.642000 | 0.0000 |
|
||||
| Joint5 | `1.056 -> 1.064` | 133 | 26.759688 | 0.467045 | 244.399800 | 0.0019 |
|
||||
| Joint6 | `1.056 -> 1.064` | 133 | 2.328736 | 0.040644 | 448.447400 | 0.0001 |
|
||||
|
||||
结论非常明确：
|
||||
|
||||
- 全文件范围内，只有 `Joint1` 的实发跃度显著超过当前生效 jerk 上限。
|
||||
- 其余 5 个轴即使取全文件峰值，也远低于各自当前生效 jerk limit。
|
||||
- 当前样本本质上是一个“J1 主导”的跃度问题，而不是六轴普遍同时逼近上限。
|
||||
|
||||
## 5. 报警窗口逐轴对比
|
||||
|
||||
结合抓包与 J519 序号，报警前最后一个关键窗口是：
|
||||
|
||||
- `seq=41552`
|
||||
- 轨迹时间窗口：`0.296 -> 0.304s`
|
||||
- `ActualSendJerkStats.txt` 行号：38
|
||||
|
||||
该窗口逐轴跃度如下：
|
||||
|
||||
| Joint | alarm window jerk(deg/s^3) | alarm window jerk(rad/s^3) | jerk_eff(rad/s^3) | alarm/limit |
|
||||
| --- | ---: | ---: | ---: | ---: |
|
||||
| Joint1 | -20395.713579 | 355.972355 | 165.922800 | 2.1454 |
|
||||
| Joint2 | -37.560252 | 0.655550 | 139.016400 | 0.0047 |
|
||||
| Joint3 | 91.694793 | 1.600376 | 183.860400 | 0.0087 |
|
||||
| Joint4 | -0.193310 | 0.003374 | 246.642000 | 0.0000 |
|
||||
| Joint5 | 24.957931 | 0.435598 | 244.399800 | 0.0018 |
|
||||
| Joint6 | 2.171939 | 0.037907 | 448.447400 | 0.0001 |
|
||||
|
||||
该窗口的直接结论与全局扫描一致：
|
||||
|
||||
- 机器人开始报警的 `0.296 -> 0.304s` 窗口里，真正越限的仍然只有 `Joint1`
|
||||
- `Joint1` 在报警窗口内已经达到当前生效 jerk limit 的 `2.1454x`
|
||||
- 其余 5 轴在同一窗口仍远低于生效 jerk 上限
|
||||
|
||||
## 6. 报警窗口与全局峰值窗口的关系
|
||||
|
||||
本次样本不能简单理解为“最大峰值出现的位置就是首次报警位置”。
|
||||
|
||||
当前证据表明：
|
||||
|
||||
- 首次报警相关窗口在 `0.296 -> 0.304s`
|
||||
- 全文件最大的 J1 跃度峰值出现在更后面的 `1.056 -> 1.064s`
|
||||
|
||||
这说明至少有两件事需要分开：
|
||||
|
||||
1. 机器人第一次进入异常态时，`Joint1` 已经在 `0.296 -> 0.304s` 超限约 `2.15x`
|
||||
2. 即便忽略第一次报警，后续轨迹中仍存在更高的 J1 跃度峰值，说明当前 `MoveJoint` 临时轨迹整体都偏激，不只是单个孤立点异常
|
||||
|
||||
## 7. 当前可落地的结论
|
||||
|
||||
基于当前运行目录的真实模型、配置和实发跃度文件，本次失败样本可以先固定为下面这组结论：
|
||||
|
||||
- 当前运行模型 `Joint1.jerk_base = 224.22`，不是 `272.7`
|
||||
- 当前样本 `jerk_limit = 0.74`，所以 `Joint1.jerk_eff = 165.9228 rad/s^3`
|
||||
- `ActualSendJerkStats.txt` 需要按 `deg/s^3` 理解，再换算成 `rad/s^3` 后与模型 jerk limit 对比
|
||||
- 无论看报警窗口还是看全文件峰值，越限主体都只有 `Joint1`
|
||||
- 报警窗口 `0.296 -> 0.304s` 中，`Joint1` 已经约为当前生效 jerk 上限的 `2.1454x`
|
||||
- 全文件最大峰值窗口 `1.056 -> 1.064s` 中，`Joint1` 约为当前生效 jerk 上限的 `2.3003x`
|
||||
|
||||
因此当前最合理的根因指向仍然是：
|
||||
|
||||
- `MoveJoint` 临时轨迹生成得过于激进
|
||||
- 当前问题首先应按 `Joint1` 的 jerk 约束失配来处理
|
||||
- 暂时没有证据支持“六轴普遍一起逼近限制”或“网络链路导致跃度统计失真”这类解释
|
||||
62
docs/robot-joint-limit-table-20260505.md
Normal file
62
docs/robot-joint-limit-table-20260505.md
Normal file
@@ -0,0 +1,62 @@
|
||||
# 机器人六轴限值提取表
|
||||
|
||||
记录时间：2026-05-05
|
||||
|
||||
## 1. 目的
|
||||
|
||||
本文档把当前机器人模型中的六轴基础限值整理成一份固定表格，明确区分以下几类信息：
|
||||
|
||||
- 模型原始值：来自 `LR_Mate_200iD_7L_clean.json` 中每个关节的 `limit.velocity / limit.acceleration / limit.jerk / limit.effort`
|
||||
- 配置倍率：来自当前运行配置 `Config/RobotConfig.json` 中的 `acc_limit / jerk_limit`
|
||||
- 运行时有效值：模型原始值叠加当前配置倍率后的结果
|
||||
|
||||
本表只覆盖六个旋转关节 `Joint1` 到 `Joint6`。
|
||||
`JointEffector` 属于末端固定关节，不计入“每个轴”的速度、加速度、跃度统计。
|
||||
|
||||
## 2. 当前取值规则
|
||||
|
||||
当前仓库运行时对六轴限值的读取规则是：
|
||||
|
||||
- `velocity_eff = velocity_base`
|
||||
- `acceleration_eff = acceleration_base * acc_limit`
|
||||
- `jerk_eff = jerk_base * jerk_limit`
|
||||
|
||||
当前 `Config/RobotConfig.json` 中的倍率为：
|
||||
|
||||
- `acc_limit = 1`
|
||||
- `jerk_limit = 1`
|
||||
|
||||
因此本次表格中的“基础值”和“有效值”数值相同。
|
||||
|
||||
## 3. 六轴限值表
|
||||
|
||||
| Joint | velocity_base | acceleration_base | jerk_base | effort_raw | acc_limit | jerk_limit | velocity_eff | acceleration_eff | jerk_eff | 备注 |
|
||||
| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | --- |
|
||||
| Joint1 | 6.45 | 26.90 | 224.22 | 0.0 | 1 | 1 | 6.45 | 26.90 | 224.22 | 模型字段存在，`effort` 当前为 0 |
|
||||
| Joint2 | 5.41 | 22.54 | 187.86 | 0.0 | 1 | 1 | 5.41 | 22.54 | 187.86 | 模型字段存在，`effort` 当前为 0 |
|
||||
| Joint3 | 7.15 | 29.81 | 248.46 | 0.0 | 1 | 1 | 7.15 | 29.81 | 248.46 | 模型字段存在，`effort` 当前为 0 |
|
||||
| Joint4 | 9.59 | 39.99 | 333.30 | 0.0 | 1 | 1 | 9.59 | 39.99 | 333.30 | 模型字段存在，`effort` 当前为 0 |
|
||||
| Joint5 | 9.51 | 39.63 | 330.27 | 0.0 | 1 | 1 | 9.51 | 39.63 | 330.27 | 模型字段存在，`effort` 当前为 0 |
|
||||
| Joint6 | 17.45 | 72.72 | 606.01 | 0.0 | 1 | 1 | 17.45 | 72.72 | 606.01 | 模型字段存在，`effort` 当前为 0 |
|
||||
|
||||
## 4. 关于“电流信息”的说明
|
||||
|
||||
这份模型文件里确实有 `limit.effort` 字段，但当前证据只能说明：
|
||||
|
||||
- 它是模型文件中的一个静态字段
|
||||
- 当前六轴以及 `JointEffector` 的 `effort` 都是 `0.0`
|
||||
- 当前 `flyshot-replacement` 代码链路没有把它当作实时电流来源来使用
|
||||
|
||||
因此当前结论应固定为：
|
||||
|
||||
- `velocity / acceleration / jerk` 可以从这份模型文档中提取
|
||||
- `effort` 只能当作模型原始字段记录
|
||||
- `effort` 不能直接解释为现场真实电流，也不能替代 J519 反馈中的电机电流数据
|
||||
|
||||
## 5. 后续使用约定
|
||||
|
||||
后续如果有人问“每个轴的速度、加速度、跃度是不是从这个文档来的”，默认回答应为：
|
||||
|
||||
- 是，六轴基础限值来自模型文件中的 `joint.limit`
|
||||
- 运行时有效加速度和有效跃度还要再乘 `RobotConfig.json` 的全局倍率
|
||||
- 不是，真实电流不要从这个文件里的 `effort` 去推断
|
||||
@@ -3,59 +3,66 @@ using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// MoveJoint 轨迹生成器。
|
||||
/// 将起始关节角到目标关节角的单段运动，按速度、加速度、jerk 约束生成稠密点到点轨迹，
|
||||
/// 供 FANUC J519 伺服流逐周期下发。
|
||||
///
|
||||
/// 核心思路：路径只取关节空间直线 q=q0+(q1-q0)*s(t)，时间律使用 7 阶平滑函数；
|
||||
/// 生成后再按离散采样点反算速度、加速度和 jerk，确保真实下发点也满足限制。
|
||||
/// </summary>
|
||||
internal static class MoveJointTrajectoryGenerator
|
||||
{
|
||||
private const double BaseMoveJointDurationSeconds = 0.320;
|
||||
private const double VelocityShapeCoefficient = 2.0759961613199973;
|
||||
private const double AccelerationShapeCoefficient = 7.986313199999984;
|
||||
private const double JerkShapeCoefficient = 36.12609273600853;
|
||||
/// <summary>
|
||||
/// 最小段基础时长（秒）。零位移时仍由采样对齐逻辑生成起点和终点两帧。
|
||||
/// </summary>
|
||||
private const double MinimumMoveJointDurationSeconds = 0.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>
|
||||
/// 单次 MoveJoint 最大采样点数上限，防止极端配置下生成过大的轨迹数组。
|
||||
/// </summary>
|
||||
private const int MaxMoveJointSampleCount = 1_000_000;
|
||||
|
||||
private static readonly double[] CapturedMvpointAlpha =
|
||||
[
|
||||
0.000000000000,
|
||||
0.000012196163,
|
||||
0.000106156906,
|
||||
0.000764380061,
|
||||
0.002550804028,
|
||||
0.006029689194,
|
||||
0.011765134027,
|
||||
0.020321400844,
|
||||
0.032262426551,
|
||||
0.048152469303,
|
||||
0.068555498563,
|
||||
0.093895155669,
|
||||
0.124210027377,
|
||||
0.159174512929,
|
||||
0.198230386318,
|
||||
0.240813559900,
|
||||
0.286359937276,
|
||||
0.334305411725,
|
||||
0.384085546646,
|
||||
0.435136609163,
|
||||
0.486894129077,
|
||||
0.538794033110,
|
||||
0.590272360135,
|
||||
0.640764719629,
|
||||
0.689707151220,
|
||||
0.736535405849,
|
||||
0.780685354316,
|
||||
0.821592775628,
|
||||
0.858693734065,
|
||||
0.891423926949,
|
||||
0.919286047395,
|
||||
0.942156722091,
|
||||
0.960255163676,
|
||||
0.974119666692,
|
||||
0.984314536393,
|
||||
0.991403790959,
|
||||
0.995951593494,
|
||||
0.998522142663,
|
||||
0.999679443354,
|
||||
0.999987892657,
|
||||
1.000000000000
|
||||
];
|
||||
/// <summary>
|
||||
/// 离散限位校验允许的浮点容差。
|
||||
/// </summary>
|
||||
private const double DiscreteLimitTolerance = 1.000001;
|
||||
|
||||
/// <summary>
|
||||
/// 离散限位校验失败时最多拉长的采样周期次数。
|
||||
/// </summary>
|
||||
private const int MaxDiscreteLimitStretchIterations = 10_000;
|
||||
|
||||
/// <summary>
|
||||
/// 计算 MoveJoint 轨迹的完整结果。
|
||||
///
|
||||
/// 处理流程：
|
||||
/// 1. 根据关节限位计算连续时间律理论最短时长
|
||||
/// 2. 按 speedRatio 换算轨迹采样周期，并将时长对齐到整数个采样间隔
|
||||
/// 3. 用 7 阶平滑点到点时间律生成稠密轨迹点
|
||||
/// 4. 按离散点反查速度、加速度和 jerk，必要时拉长时长重算
|
||||
/// 5. 封装为 TrajectoryResult 返回
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人配置，含自由度数和关节限位。</param>
|
||||
/// <param name="startJoints">起始关节角（弧度）。</param>
|
||||
/// <param name="targetJoints">目标关节角（弧度）。</param>
|
||||
/// <param name="speedRatio">速度倍率，必须大于 0；当前链路中用于换算轨迹采样周期。</param>
|
||||
/// <param name="logger">可选的诊断日志。</param>
|
||||
public static TrajectoryResult CreateResult(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<double> startJoints,
|
||||
@@ -82,9 +89,28 @@ internal static class MoveJointTrajectoryGenerator
|
||||
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
|
||||
var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
|
||||
|
||||
var stretchCount = 0;
|
||||
while (!SatisfiesDiscreteJointLimits(robot, denseJointTrajectory))
|
||||
{
|
||||
stretchCount++;
|
||||
if (stretchCount > MaxDiscreteLimitStretchIterations)
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint duration cannot be stretched enough to satisfy joint limits.");
|
||||
}
|
||||
|
||||
// 连续时间律满足限位后，仍以实际离散点为准；不满足时逐周期拉长后重采样。
|
||||
durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
|
||||
denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
|
||||
}
|
||||
|
||||
logger?.LogDebug(
|
||||
"MoveJointTrajectoryGenerator: 请求时长={RequestedDuration:F4}s, 对齐后时长={Duration:F4}s, speedRatio={SpeedRatio}, 采样周期={SamplePeriod:F6}s, 采样数={SampleCount}",
|
||||
requestedDurationSeconds, durationSeconds, speedRatio, samplePeriodSeconds, denseJointTrajectory.Count);
|
||||
"MoveJointTrajectoryGenerator: requestedDuration={RequestedDuration:F4}s, duration={Duration:F4}s, speedRatio={SpeedRatio}, samplePeriod={SamplePeriod:F6}s, sampleCount={SampleCount}, stretchCount={StretchCount}",
|
||||
requestedDurationSeconds,
|
||||
durationSeconds,
|
||||
speedRatio,
|
||||
samplePeriodSeconds,
|
||||
denseJointTrajectory.Count,
|
||||
stretchCount);
|
||||
|
||||
return new TrajectoryResult(
|
||||
programName: "move-joint",
|
||||
@@ -101,9 +127,18 @@ internal static class MoveJointTrajectoryGenerator
|
||||
denseJointTrajectory: denseJointTrajectory);
|
||||
}
|
||||
|
||||
internal static double ResolveDurationSeconds(RobotProfile robot, IReadOnlyList<double> startJoints, IReadOnlyList<double> targetJoints)
|
||||
/// <summary>
|
||||
/// 根据 7 阶平滑点到点时间律和每轴限位，计算 MoveJoint 理论最短时长。
|
||||
///
|
||||
/// 时间律为 s(u)=35u^4-84u^5+70u^6-20u^7，其中 u=t/T。
|
||||
/// 各轴位移 d_i 共用同一个 s(t)，所以每轴分别按 d_i 放大速度、加速度和 jerk，再取全局最大时长。
|
||||
/// </summary>
|
||||
internal static double ResolveDurationSeconds(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var duration = BaseMoveJointDurationSeconds;
|
||||
var duration = MinimumMoveJointDurationSeconds;
|
||||
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
@@ -114,9 +149,9 @@ internal static class MoveJointTrajectoryGenerator
|
||||
}
|
||||
|
||||
var limit = robot.JointLimits[index];
|
||||
var velocityDuration = distance * VelocityShapeCoefficient / limit.VelocityLimit;
|
||||
var accelerationDuration = Math.Sqrt(distance * AccelerationShapeCoefficient / limit.AccelerationLimit);
|
||||
var jerkDuration = Math.Cbrt(distance * JerkShapeCoefficient / limit.JerkLimit);
|
||||
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
|
||||
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
|
||||
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);
|
||||
|
||||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||||
}
|
||||
@@ -124,6 +159,12 @@ internal static class MoveJointTrajectoryGenerator
|
||||
return duration;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将请求时长向上对齐到整数个采样周期，确保轨迹末帧正好落在 duration 处。
|
||||
/// </summary>
|
||||
/// <param name="durationSeconds">请求的理论最短时长（秒）。</param>
|
||||
/// <param name="samplePeriodSeconds">采样周期（秒）。</param>
|
||||
/// <returns>对齐后的时长，为 samplePeriodSeconds 的整数倍。</returns>
|
||||
internal static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
|
||||
@@ -133,26 +174,14 @@ internal static class MoveJointTrajectoryGenerator
|
||||
|
||||
var intervals = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds);
|
||||
|
||||
return Math.Max(1, intervals) * samplePeriodSeconds;
|
||||
}
|
||||
|
||||
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
var rawIntervals = durationSeconds / samplePeriodSeconds;
|
||||
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint sample count is not representable.");
|
||||
}
|
||||
|
||||
var intervals = (long)Math.Ceiling(rawIntervals - 1e-9);
|
||||
if (intervals < 1 || intervals + 1 > MaxMoveJointSampleCount)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint sample count must be between 2 and {MaxMoveJointSampleCount}.");
|
||||
}
|
||||
|
||||
return intervals;
|
||||
return intervals * samplePeriodSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成从起始关节角到目标关节角的稠密等时间隔轨迹点序列。
|
||||
///
|
||||
/// 每行格式：[time_seconds, joint_0, joint_1, ..., joint_n-1]。
|
||||
/// </summary>
|
||||
internal static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
@@ -171,30 +200,128 @@ internal static class MoveJointTrajectoryGenerator
|
||||
return rows;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CreateRow(double timeSeconds, double durationSeconds, IReadOnlyList<double> startJoints, IReadOnlyList<double> targetJoints)
|
||||
/// <summary>
|
||||
/// 计算 7 阶平滑点到点时间律的位置归一化值。
|
||||
/// </summary>
|
||||
/// <param name="normalizedTime">归一化时间 u，取值会被限制在 [0, 1]。</param>
|
||||
/// <returns>归一化位置 s(u)，范围 [0, 1]。</returns>
|
||||
internal static double EvaluateSmoothPtpPositionScale(double normalizedTime)
|
||||
{
|
||||
var clamped = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var u2 = clamped * clamped;
|
||||
var u4 = u2 * u2;
|
||||
|
||||
return u4 * (35.0 + (clamped * (-84.0 + (clamped * (70.0 - (20.0 * clamped))))));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算时长对应的采样间隔数（向上取整）。
|
||||
/// 采样间隔数 + 1 = 采样点数，因为轨迹包含起点和终点。
|
||||
/// </summary>
|
||||
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
var rawIntervals = durationSeconds / samplePeriodSeconds;
|
||||
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint 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 > MaxMoveJointSampleCount)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint sample count must be between 2 and {MaxMoveJointSampleCount}.");
|
||||
}
|
||||
|
||||
return intervals;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造单个轨迹行：[time_seconds, joint_0, ..., joint_N-1]。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<double> CreateRow(
|
||||
double timeSeconds,
|
||||
double durationSeconds,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
|
||||
var alpha = InterpolateCapturedAlpha(u);
|
||||
var scale = EvaluateSmoothPtpPositionScale(u);
|
||||
var row = new double[startJoints.Count + 1];
|
||||
row[0] = Math.Round(timeSeconds, 9);
|
||||
|
||||
for (var index = 0; index < startJoints.Count; index++)
|
||||
{
|
||||
row[index + 1] = startJoints[index] + ((targetJoints[index] - startJoints[index]) * alpha);
|
||||
row[index + 1] = startJoints[index] + ((targetJoints[index] - startJoints[index]) * scale);
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
internal static double InterpolateCapturedAlpha(double normalizedTime)
|
||||
/// <summary>
|
||||
/// 用生成后的离散采样点复核每轴速度、加速度和 jerk，避免连续时间律在采样后仍出现差分越限。
|
||||
/// </summary>
|
||||
private static bool SatisfiesDiscreteJointLimits(RobotProfile robot, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
var clamped = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var scaledIndex = clamped * (CapturedMvpointAlpha.Length - 1);
|
||||
var lower = (int)Math.Floor(scaledIndex);
|
||||
var upper = Math.Min(lower + 1, CapturedMvpointAlpha.Length - 1);
|
||||
var fraction = scaledIndex - lower;
|
||||
double? previousTime = null;
|
||||
double[]? previousPositions = null;
|
||||
double[]? previousVelocities = null;
|
||||
double[]? previousAccelerations = null;
|
||||
|
||||
return CapturedMvpointAlpha[lower]
|
||||
+ ((CapturedMvpointAlpha[upper] - CapturedMvpointAlpha[lower]) * fraction);
|
||||
foreach (var row in rows)
|
||||
{
|
||||
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("MoveJoint dense trajectory timestamps must be strictly increasing.");
|
||||
}
|
||||
|
||||
var velocities = new double[robot.DegreesOfFreedom];
|
||||
var accelerations = new double[robot.DegreesOfFreedom];
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var limit = robot.JointLimits[index];
|
||||
velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
|
||||
if (Math.Abs(velocities[index]) > limit.VelocityLimit * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
accelerations[index] = previousVelocities is null
|
||||
? 0.0
|
||||
: (velocities[index] - previousVelocities[index]) / dt;
|
||||
if (Math.Abs(accelerations[index]) > limit.AccelerationLimit * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (previousAccelerations is not null)
|
||||
{
|
||||
var jerk = (accelerations[index] - previousAccelerations[index]) / dt;
|
||||
if (Math.Abs(jerk) > limit.JerkLimit * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
previousVelocities = velocities;
|
||||
previousAccelerations = accelerations;
|
||||
}
|
||||
|
||||
previousTime = currentTime;
|
||||
previousPositions = currentPositions;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
using System.Diagnostics;
|
||||
using System.Text;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
using Microsoft.Extensions.Logging;
|
||||
using Microsoft.Extensions.Logging.Abstractions;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc;
|
||||
|
||||
@@ -37,15 +39,27 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
/// <summary>
|
||||
/// 初始化 FANUC 控制器运行时。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
||||
public FanucControllerRuntime(ILogger<FanucControllerRuntime>? logger = null)
|
||||
/// <param name="logger">运行时日志记录器。</param>
|
||||
/// <param name="j519Logger">J519 客户端日志记录器。</param>
|
||||
public FanucControllerRuntime(ILogger<FanucControllerRuntime> logger, ILogger<FanucJ519Client> j519Logger)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(logger);
|
||||
ArgumentNullException.ThrowIfNull(j519Logger);
|
||||
|
||||
_commandClient = new FanucCommandClient();
|
||||
_stateClient = new FanucStateClient();
|
||||
_j519Client = new FanucJ519Client();
|
||||
_j519Client = new FanucJ519Client(j519Logger);
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 初始化一份无日志的 FANUC 控制器运行时，供离线测试或手工构造场景使用。
|
||||
/// </summary>
|
||||
public FanucControllerRuntime()
|
||||
: this(NullLogger<FanucControllerRuntime>.Instance, NullLogger<FanucJ519Client>.Instance)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 供测试注入 mock 客户端的内部构造函数。
|
||||
/// </summary>
|
||||
@@ -479,6 +493,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
_isInMotion = true;
|
||||
_sendCts = new CancellationTokenSource();
|
||||
var ct = _sendCts.Token;
|
||||
|
||||
_sendTask = Task.Run(() => SendDenseTrajectory(result, finalJointPositions, ct), ct);
|
||||
_logger?.LogInformation("ExecuteTrajectory 已启动后台稠密发送任务");
|
||||
return;
|
||||
@@ -545,6 +560,13 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
int segmentIndex = 0;
|
||||
long logInterval = Math.Max(1, sampleCount / 10);
|
||||
int triggerFiredCount = 0;
|
||||
var sentJointRows = new List<IReadOnlyList<double>>();
|
||||
var sentJerkRows = new List<IReadOnlyList<double>>();
|
||||
var outputDir = CreateDenseSendOutputDirectory(result.ProgramName);
|
||||
double? previousTime = null;
|
||||
double[]? previousJoints = null;
|
||||
double[]? previousVelocity = null;
|
||||
double[]? previousAcceleration = null;
|
||||
|
||||
try
|
||||
{
|
||||
@@ -597,12 +619,28 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
writeIoValue: ioValue);
|
||||
|
||||
_j519Client.UpdateCommand(command);
|
||||
sentJointRows.Add(BuildDenseSendJointRow(trajectoryTime, joints, ioMask, ioValue));
|
||||
|
||||
if (previousTime is not null && previousJoints is not null)
|
||||
{
|
||||
var jerkRow = BuildDenseSendJerkRow(
|
||||
previousTime.Value,
|
||||
trajectoryTime,
|
||||
previousJoints,
|
||||
joints,
|
||||
ref previousVelocity,
|
||||
ref previousAcceleration);
|
||||
sentJerkRows.Add(jerkRow);
|
||||
}
|
||||
|
||||
if (clearMaskAfterSend)
|
||||
{
|
||||
ioMask = 0;
|
||||
}
|
||||
|
||||
previousTime = trajectoryTime;
|
||||
previousJoints = joints;
|
||||
|
||||
// 周期性记录进度（Debug 级别，避免高频 Info 日志）。
|
||||
if (sampleIndex > 0 && sampleIndex % logInterval == 0)
|
||||
{
|
||||
@@ -631,6 +669,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
}
|
||||
finally
|
||||
{
|
||||
TryWriteDenseSendArtifacts(outputDir, sentJointRows, sentJerkRows);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_isInMotion = false;
|
||||
@@ -735,6 +775,166 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
return radians * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为单次稠密发送创建按日期时间归档的输出目录。
|
||||
/// </summary>
|
||||
private static string CreateDenseSendOutputDirectory(string programName)
|
||||
{
|
||||
var safeProgramName = SanitizeDirectoryName(string.IsNullOrWhiteSpace(programName) ? "unnamed-program" : programName);
|
||||
var timestamp = DateTime.Now.ToString("yyyyMMdd_HHmmss_fff");
|
||||
var outputDir = Path.Combine(
|
||||
AppContext.BaseDirectory,
|
||||
"Config",
|
||||
"Data",
|
||||
safeProgramName,
|
||||
"DenseSend",
|
||||
timestamp);
|
||||
Directory.CreateDirectory(outputDir);
|
||||
return outputDir;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造实际发送点位文本行，格式为 time + 关节角度 + io_mask + io_value。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<double> BuildDenseSendJointRow(double trajectoryTime, IReadOnlyList<double> joints, ushort ioMask, ushort ioValue)
|
||||
{
|
||||
var row = new double[joints.Count + 3];
|
||||
row[0] = Math.Round(trajectoryTime, 6);
|
||||
for (var index = 0; index < joints.Count; index++)
|
||||
{
|
||||
row[index + 1] = Math.Round(joints[index], 6);
|
||||
}
|
||||
|
||||
row[^2] = ioMask;
|
||||
row[^1] = ioValue;
|
||||
return row;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造相邻发送点之间的跃度统计，格式为 start_time + end_time + dt + max_abs_jerk + jerk[j1..jn]。
|
||||
/// </summary>
|
||||
/// <summary>
|
||||
/// 构造相邻发送点之间的跃度统计行，格式为 start_time + end_time + dt + max_abs_jerk + jerk[j1..jn]。
|
||||
/// 通过 ref 参数维护前两帧的速度和加速度状态，从而用二阶后向差分近似跃度。
|
||||
/// </summary>
|
||||
/// <param name="previousTime">上一帧时间戳（秒）。</param>
|
||||
/// <param name="currentTime">当前帧时间戳（秒）。</param>
|
||||
/// <param name="previousJoints">上一帧关节角（rad）。</param>
|
||||
/// <param name="currentJoints">当前帧关节角（rad）。</param>
|
||||
/// <param name="previousVelocity">上一帧关节速度（rad/s），ref 更新为当前帧速度。</param>
|
||||
/// <param name="previousAcceleration">上一帧关节加速度（rad/s²），ref 更新为当前帧加速度。</param>
|
||||
/// <returns>长度为 jointCount + 4 的行向量。</returns>
|
||||
private static IReadOnlyList<double> BuildDenseSendJerkRow(
|
||||
double previousTime,
|
||||
double currentTime,
|
||||
IReadOnlyList<double> previousJoints,
|
||||
IReadOnlyList<double> currentJoints,
|
||||
ref double[]? previousVelocity,
|
||||
ref double[]? previousAcceleration)
|
||||
{
|
||||
// 1. 计算时间步长 dt；若相邻帧时间戳相同（同一批次下发），兜底为极小值防止除零
|
||||
var dt = currentTime - previousTime;
|
||||
if (dt <= 0.0)
|
||||
{
|
||||
dt = 1e-9;
|
||||
}
|
||||
|
||||
// 2. 初始化当前帧的速度、加速度、跃度数组，以及本帧最大绝对值跃度追踪变量
|
||||
var jointCount = currentJoints.Count;
|
||||
var currentVelocity = new double[jointCount];
|
||||
var currentAcceleration = new double[jointCount];
|
||||
var currentJerk = new double[jointCount];
|
||||
var maxAbsJerk = 0.0;
|
||||
|
||||
// 3. 逐关节计算数值微分：一阶后向差分求速度，二阶后向差分求加速度，三阶后向差分求跃度
|
||||
// 由于跃度需要加速度历史，前两帧的加速度不可用时对应的跃度保持为 0
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
// 3a. 一阶差分：v(t) = (q(t) - q(t-1)) / dt
|
||||
currentVelocity[index] = (currentJoints[index] - previousJoints[index]) / dt;
|
||||
|
||||
// 3b. 二阶差分：a(t) = (v(t) - v(t-1)) / dt；至少需要上一帧速度数据
|
||||
if (previousVelocity is not null)
|
||||
{
|
||||
currentAcceleration[index] = (currentVelocity[index] - previousVelocity[index]) / dt;
|
||||
}
|
||||
|
||||
// 3c. 三阶差分：j(t) = (a(t) - a(t-1)) / dt；至少需要上一帧加速度数据
|
||||
if (previousAcceleration is not null)
|
||||
{
|
||||
currentJerk[index] = (currentAcceleration[index] - previousAcceleration[index]) / dt;
|
||||
// 3d. 追踪所有关节中绝对值最大的跃度，用于后续评估轨迹平滑度
|
||||
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(currentJerk[index]));
|
||||
}
|
||||
}
|
||||
|
||||
// 4. 将当前帧的速度和加速度保存为"上一帧"值，供下一帧调用时继续做差分
|
||||
previousVelocity = currentVelocity;
|
||||
previousAcceleration = currentAcceleration;
|
||||
|
||||
// 5. 组装输出行：前 4 列为起始时间、结束时间、步长、最大绝对值跃度，之后按关节顺序排列各关节跃度
|
||||
// 格式设计兼容旧版 JointDetialTraj.txt 的导出风格，便于对比验证
|
||||
var row = new double[jointCount + 4];
|
||||
row[0] = Math.Round(previousTime, 6); // 起始时间戳（秒）
|
||||
row[1] = Math.Round(currentTime, 6); // 结束时间戳（秒）
|
||||
row[2] = Math.Round(dt, 6); // 时间步长（秒）
|
||||
row[3] = Math.Round(maxAbsJerk, 6); // 本次步长内所有关节的最大绝对值跃度
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
row[index + 4] = Math.Round(currentJerk[index], 6); // 各关节的跃度值（rad/s³）
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 尝试把实际发送点位和跃度统计写入纯文本文件；若落盘失败，只记录日志，不影响运动主流程。
|
||||
/// </summary>
|
||||
private void TryWriteDenseSendArtifacts(
|
||||
string outputDir,
|
||||
IReadOnlyList<IReadOnlyList<double>> sentJointRows,
|
||||
IReadOnlyList<IReadOnlyList<double>> sentJerkRows)
|
||||
{
|
||||
try
|
||||
{
|
||||
WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), sentJointRows);
|
||||
WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), sentJerkRows);
|
||||
_logger?.LogInformation(
|
||||
"SendDenseTrajectory 已写出实际发送记录: outputDir={OutputDir}, pointRows={PointRows}, jerkRows={JerkRows}",
|
||||
outputDir,
|
||||
sentJointRows.Count,
|
||||
sentJerkRows.Count);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_logger?.LogWarning(exception, "SendDenseTrajectory 写出实际发送记录失败: outputDir={OutputDir}", outputDir);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 以旧轨迹文本兼容的空格分隔格式写出数值行。
|
||||
/// </summary>
|
||||
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
var sb = new StringBuilder();
|
||||
foreach (var row in rows)
|
||||
{
|
||||
sb.AppendLine(string.Join(" ", row.Select(static value => $"{value:F6}")));
|
||||
}
|
||||
|
||||
File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清理程序名中的非法路径字符，避免输出目录受外部输入污染。
|
||||
/// </summary>
|
||||
private static string SanitizeDirectoryName(string name)
|
||||
{
|
||||
var invalidChars = Path.GetInvalidFileNameChars();
|
||||
var chars = name.Select(ch => invalidChars.Contains(ch) ? '_' : ch).ToArray();
|
||||
return new string(chars);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 取消并等待当前后台发送任务，避免旧任务与新轨迹并发。
|
||||
/// </summary>
|
||||
|
||||
@@ -15,6 +15,7 @@ public sealed class FanucJ519Client : IDisposable
|
||||
private CancellationTokenSource? _cts;
|
||||
private Task? _receiveTask;
|
||||
private FanucJ519Command? _currentCommand;
|
||||
private FanucJ519Command? _lastSentCommand;
|
||||
private List<FanucJ519Command>? _commandHistoryForTests;
|
||||
private FanucJ519Response? _latestResponse;
|
||||
private bool _motionStarted;
|
||||
@@ -28,7 +29,7 @@ public sealed class FanucJ519Client : IDisposable
|
||||
/// <summary>
|
||||
/// 初始化 FANUC J519 客户端。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
||||
public FanucJ519Client(ILogger<FanucJ519Client>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
@@ -81,13 +82,15 @@ public sealed class FanucJ519Client : IDisposable
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = null;
|
||||
_lastSentCommand = null;
|
||||
if (_motionStarted)
|
||||
{
|
||||
_logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
|
||||
return;
|
||||
}
|
||||
|
||||
_motionStarted = true;
|
||||
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送");
|
||||
@@ -131,11 +134,14 @@ public sealed class FanucJ519Client : IDisposable
|
||||
_commandHistoryForTests?.Add(command);
|
||||
}
|
||||
|
||||
_logger?.LogDebug(
|
||||
"J519 UpdateCommand: joints=[{Joints}], ioMask=0x{IoMask:X4}, ioValue=0x{IoValue:X4}",
|
||||
string.Join(", ", command.TargetJoints.Select(j => j.ToString("F2"))),
|
||||
command.WriteIoMask,
|
||||
command.WriteIoValue);
|
||||
if (_logger?.IsEnabled(LogLevel.Debug) == true)
|
||||
{
|
||||
//_logger.LogDebug(
|
||||
// "J519 UpdateCommand: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
|
||||
// command.TargetJoints,
|
||||
// command.WriteIoMask,
|
||||
// command.WriteIoValue);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -223,6 +229,7 @@ public sealed class FanucJ519Client : IDisposable
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = null;
|
||||
_lastSentCommand = null;
|
||||
_commandHistoryForTests = null;
|
||||
_motionStarted = false;
|
||||
}
|
||||
@@ -304,13 +311,18 @@ public sealed class FanucJ519Client : IDisposable
|
||||
}
|
||||
|
||||
receiveCount++;
|
||||
// 判断 ready for command 条件并回发命令包。
|
||||
if (response.AcceptsCommand)
|
||||
{
|
||||
await SendCommandForStatusAsync(response, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
// 仅在状态变化时记录 Info，避免高频日志。
|
||||
if (lastLoggedResponse is null
|
||||
|| lastLoggedResponse.Status != response.Status
|
||||
|| lastLoggedResponse.RobotInMotion != response.RobotInMotion
|
||||
|| lastLoggedResponse.SystemReady != response.SystemReady)
|
||||
|| lastLoggedResponse.SystemReady != response.SystemReady
|
||||
|| lastLoggedResponse.AcceptsCommand != response.AcceptsCommand)
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, sysrdy={SysRdy}, motion={Motion}, pose=[{Pose}], joints=[{Joints}]",
|
||||
@@ -319,8 +331,11 @@ public sealed class FanucJ519Client : IDisposable
|
||||
response.AcceptsCommand,
|
||||
response.SystemReady,
|
||||
response.RobotInMotion,
|
||||
string.Join(", ", response.Pose.Select(v => v.ToString("F1"))),
|
||||
string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F2"))));
|
||||
string.Join(", ", response.Pose.Select(v => v.ToString("F3"))),
|
||||
string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F3"))));
|
||||
|
||||
var lastSentTargetJoints = GetLastSentTargetJointsLogText();
|
||||
_logger?.LogInformation("J519 最后一条发送目标关节轴: joints=[{Joints}]", lastSentTargetJoints);
|
||||
lastLoggedResponse = response;
|
||||
}
|
||||
else if (receiveCount % 1000 == 0)
|
||||
@@ -368,5 +383,24 @@ public sealed class FanucJ519Client : IDisposable
|
||||
|
||||
var packet = FanucJ519Protocol.PackCommandPacket(command);
|
||||
await udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_lastSentCommand = command;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取最近一次已成功发送命令的目标关节轴文本，便于状态日志直接对照控制目标。
|
||||
/// </summary>
|
||||
/// <returns>格式化后的目标关节轴文本；如果尚未发送命令则返回占位符。</returns>
|
||||
private string GetLastSentTargetJointsLogText()
|
||||
{
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _lastSentCommand is null
|
||||
? "n/a"
|
||||
: string.Join(", ", _lastSentCommand.TargetJoints.Take(6).Select(v => v.ToString("F5")));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -709,7 +709,7 @@ public sealed class LegacyHttpApiController : ControllerBase
|
||||
data.robot_name, data.robot_ip, data.sim, data.server_ip, data.port);
|
||||
try
|
||||
{
|
||||
_compatService.ConnectServer(data.server_ip, data.port);
|
||||
|
||||
_compatService.SetUpRobot(data.robot_name);
|
||||
if (!_compatService.IsSetUp)
|
||||
{
|
||||
@@ -719,7 +719,7 @@ public sealed class LegacyHttpApiController : ControllerBase
|
||||
|
||||
_compatService.SetActiveController(data.sim);
|
||||
_compatService.Connect(data.robot_ip);
|
||||
_compatService.EnableRobot(2);
|
||||
_compatService.EnableRobot(4);
|
||||
_logger.LogInformation("InitMpcRobot 成功: robot_name={RobotName}", data.robot_name);
|
||||
return Ok(new { message = "init_Success", returnCode = 0 });
|
||||
}
|
||||
|
||||
@@ -12,9 +12,9 @@ namespace Flyshot.Core.Tests;
|
||||
/// </summary>
|
||||
public sealed class FanucControllerRuntimeDenseTests
|
||||
{
|
||||
private const double CapturedMvpointVelocityShapeCoefficient = 2.0759961613199973;
|
||||
private const double CapturedMvpointAccelerationShapeCoefficient = 7.986313199999984;
|
||||
private const double CapturedMvpointJerkShapeCoefficient = 36.12609273600853;
|
||||
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
|
||||
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
|
||||
private const double SmoothPtpJerkShapeCoefficient = 52.5;
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
|
||||
@@ -61,6 +61,84 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机稠密发送会把实际下发给机器人的整条点位与点间跃度按时间目录落盘。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_WritesActualSentPointsAndJerkStats()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var programName = $"dense-send-{Guid.NewGuid():N}";
|
||||
var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", programName);
|
||||
|
||||
try
|
||||
{
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio: 0.5);
|
||||
|
||||
var denseTrajectory = new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||
};
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: programName,
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.016),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
var runDir = GetSingleDenseSendRunDirectory(outputRoot);
|
||||
var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt");
|
||||
var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt");
|
||||
|
||||
Assert.True(File.Exists(pointsPath));
|
||||
Assert.True(File.Exists(jerkPath));
|
||||
|
||||
var pointLines = File.ReadAllLines(pointsPath);
|
||||
var jerkLines = File.ReadAllLines(jerkPath);
|
||||
Assert.Equal(commands.Count, pointLines.Length);
|
||||
Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);
|
||||
|
||||
var firstColumns = ParseColumns(pointLines[0]);
|
||||
var lastColumns = ParseColumns(pointLines[^1]);
|
||||
Assert.Equal(9, firstColumns.Length);
|
||||
Assert.Equal(9, lastColumns.Length);
|
||||
Assert.Equal(0.0, firstColumns[0], precision: 6);
|
||||
Assert.Equal(180.0, lastColumns[1], precision: 6);
|
||||
|
||||
var firstJerkColumns = ParseColumns(jerkLines[0]);
|
||||
Assert.Equal(10, firstJerkColumns.Length);
|
||||
Assert.Equal(0.0, firstJerkColumns[0], precision: 6);
|
||||
Assert.Equal(0.004, firstJerkColumns[2], precision: 6);
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (Directory.Exists(outputRoot))
|
||||
{
|
||||
Directory.Delete(outputRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
|
||||
/// </summary>
|
||||
@@ -135,9 +213,9 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
|
||||
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
|
||||
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
|
||||
Assert.InRange(fullSpeed.Duration.TotalSeconds, 0.318, 0.322);
|
||||
Assert.True(speed07.Duration.TotalSeconds >= 0.320);
|
||||
Assert.InRange(speed05.Duration.TotalSeconds, 0.318, 0.322);
|
||||
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||||
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||||
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||||
}
|
||||
|
||||
[Fact]
|
||||
@@ -395,7 +473,7 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_MatchesCapturedMvpointAlphaLawAtSpeedOne()
|
||||
public void MoveJointTrajectoryGenerator_GeneratesSmoothJerkLimitedPtpTrajectoryAtSpeedOne()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[]
|
||||
@@ -420,64 +498,46 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
|
||||
Assert.Equal(41, rows.Count);
|
||||
Assert.InRange(result.Duration.TotalSeconds, 0.318, 0.322);
|
||||
Assert.True(rows.Count > 41, $"平滑 MoveJoint 不应再固定输出 41 点，actual={rows.Count}。");
|
||||
Assert.True(result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
AssertAllJointJerksWithinLimits(rows, robot);
|
||||
}
|
||||
|
||||
var expectedAlpha = new[]
|
||||
/// <summary>
|
||||
/// 验证报警样本中的 MoveJoint 预移动作不会因为点到点时间律重采样而产生任一轴跃度尖峰。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_ActualAlarmSampleKeepsJointOneJerkWithinLimit()
|
||||
{
|
||||
0.000000000000,
|
||||
0.000012196163,
|
||||
0.000106156906,
|
||||
0.000764380061,
|
||||
0.002550804028,
|
||||
0.006029689194,
|
||||
0.011765134027,
|
||||
0.020321400844,
|
||||
0.032262426551,
|
||||
0.048152469303,
|
||||
0.068555498563,
|
||||
0.093895155669,
|
||||
0.124210027377,
|
||||
0.159174512929,
|
||||
0.198230386318,
|
||||
0.240813559900,
|
||||
0.286359937276,
|
||||
0.334305411725,
|
||||
0.384085546646,
|
||||
0.435136609163,
|
||||
0.486894129077,
|
||||
0.538794033110,
|
||||
0.590272360135,
|
||||
0.640764719629,
|
||||
0.689707151220,
|
||||
0.736535405849,
|
||||
0.780685354316,
|
||||
0.821592775628,
|
||||
0.858693734065,
|
||||
0.891423926949,
|
||||
0.919286047395,
|
||||
0.942156722091,
|
||||
0.960255163676,
|
||||
0.974119666692,
|
||||
0.984314536393,
|
||||
0.991403790959,
|
||||
0.995951593494,
|
||||
0.998522142663,
|
||||
0.999679443354,
|
||||
0.999987892657,
|
||||
1.000000000000
|
||||
var robot = CreateMoveJointRuntimeLimitRobotProfile();
|
||||
var startJoints = new[]
|
||||
{
|
||||
DegreesToRadians(71.454618),
|
||||
DegreesToRadians(0.688433),
|
||||
DegreesToRadians(-1.074197),
|
||||
DegreesToRadians(-0.869001),
|
||||
DegreesToRadians(1.218057),
|
||||
DegreesToRadians(0.547036)
|
||||
};
|
||||
var targetJoints = new[]
|
||||
{
|
||||
DegreesToRadians(60.546227),
|
||||
DegreesToRadians(0.668344),
|
||||
DegreesToRadians(-1.025155),
|
||||
DegreesToRadians(-0.869105),
|
||||
DegreesToRadians(1.231405),
|
||||
DegreesToRadians(0.548197)
|
||||
};
|
||||
|
||||
for (var index = 0; index < rows.Count; index++)
|
||||
{
|
||||
var actualDegrees = rows[index].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
var alpha = ComputeLineAlpha(actualDegrees, startJoints, targetJoints);
|
||||
Assert.Equal(expectedAlpha[index], alpha, precision: 6);
|
||||
}
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
AssertAllJointJerksWithinLimits(rows, robot);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_DoesNotShortenBaseDurationWhenSpeedRatioDoesNotDivideWindow()
|
||||
public void MoveJointTrajectoryGenerator_DoesNotShortenLimitDurationWhenSpeedRatioDoesNotDivideWindow()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
@@ -486,7 +546,9 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
|
||||
Assert.True(result.Duration.TotalSeconds >= 0.320, $"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
|
||||
Assert.True(
|
||||
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints),
|
||||
$"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
|
||||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
}
|
||||
@@ -513,12 +575,11 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
var expectedVelocityDuration = Math.PI * CapturedMvpointVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
|
||||
var expectedAccelerationDuration = Math.Sqrt(Math.PI * CapturedMvpointAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
|
||||
var expectedJerkDuration = Math.Cbrt(Math.PI * CapturedMvpointJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
|
||||
var expectedVelocityDuration = Math.PI * SmoothPtpVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
|
||||
var expectedAccelerationDuration = Math.Sqrt(Math.PI * SmoothPtpAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
|
||||
var expectedJerkDuration = Math.Cbrt(Math.PI * SmoothPtpJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
|
||||
var expectedMinimumDuration = new[]
|
||||
{
|
||||
0.320,
|
||||
expectedVelocityDuration,
|
||||
expectedAccelerationDuration,
|
||||
expectedJerkDuration
|
||||
@@ -605,6 +666,111 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建当前运行配置下的 MoveJoint 限值模型，覆盖报警样本使用的 acc_limit/jerk_limit=0.74。
|
||||
/// </summary>
|
||||
private static RobotProfile CreateMoveJointRuntimeLimitRobotProfile()
|
||||
{
|
||||
const double limitScale = 0.74;
|
||||
return new RobotProfile(
|
||||
name: "FANUC_LR_Mate_200iD",
|
||||
modelPath: "Models/FANUC_LR_Mate_200iD.robot",
|
||||
degreesOfFreedom: 6,
|
||||
jointLimits:
|
||||
[
|
||||
new JointLimit("J1", 6.45, 26.90 * limitScale, 224.22 * limitScale),
|
||||
new JointLimit("J2", 5.41, 22.54 * limitScale, 187.86 * limitScale),
|
||||
new JointLimit("J3", 7.15, 29.81 * limitScale, 248.46 * limitScale),
|
||||
new JointLimit("J4", 9.59, 39.99 * limitScale, 333.30 * limitScale),
|
||||
new JointLimit("J5", 9.51, 39.63 * limitScale, 330.27 * limitScale),
|
||||
new JointLimit("J6", 17.45, 72.72 * limitScale, 606.01 * limitScale)
|
||||
],
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 以发送周期上的二阶后向差分估算单轴最大绝对跃度，用来复现 ActualSendJerkStats 的判定口径。
|
||||
/// </summary>
|
||||
private static double MaxAbsJointJerk(IReadOnlyList<IReadOnlyList<double>> rows, int jointIndex)
|
||||
{
|
||||
double? previousTime = null;
|
||||
double? previousPosition = null;
|
||||
double? previousVelocity = null;
|
||||
double? previousAcceleration = null;
|
||||
var maxAbsJerk = 0.0;
|
||||
|
||||
foreach (var row in rows)
|
||||
{
|
||||
var currentTime = row[0];
|
||||
var currentPosition = row[jointIndex + 1];
|
||||
if (previousTime is not null && previousPosition is not null)
|
||||
{
|
||||
var dt = currentTime - previousTime.Value;
|
||||
Assert.True(dt > 0.0, "MoveJoint 稠密轨迹时间戳必须严格递增。");
|
||||
|
||||
var velocity = (currentPosition - previousPosition.Value) / dt;
|
||||
var acceleration = previousVelocity is null ? 0.0 : (velocity - previousVelocity.Value) / dt;
|
||||
if (previousAcceleration is not null)
|
||||
{
|
||||
var jerk = (acceleration - previousAcceleration.Value) / dt;
|
||||
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(jerk));
|
||||
}
|
||||
|
||||
previousVelocity = velocity;
|
||||
previousAcceleration = acceleration;
|
||||
}
|
||||
|
||||
previousTime = currentTime;
|
||||
previousPosition = currentPosition;
|
||||
}
|
||||
|
||||
return maxAbsJerk;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证稠密 MoveJoint 轨迹按 8ms 差分后不会超过机器人每轴 jerk 限值。
|
||||
/// </summary>
|
||||
private static void AssertAllJointJerksWithinLimits(IReadOnlyList<IReadOnlyList<double>> rows, RobotProfile robot)
|
||||
{
|
||||
for (var jointIndex = 0; jointIndex < robot.DegreesOfFreedom; jointIndex++)
|
||||
{
|
||||
var peakJerk = MaxAbsJointJerk(rows, jointIndex);
|
||||
var limit = robot.JointLimits[jointIndex].JerkLimit;
|
||||
Assert.True(
|
||||
peakJerk <= limit * 1.000001,
|
||||
$"J{jointIndex + 1} jerk {peakJerk:F6} rad/s^3 exceeds limit {limit:F6} rad/s^3.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按 7 阶平滑点到点时间律的速度、加速度、jerk 形状系数计算理论最小时长。
|
||||
/// </summary>
|
||||
private static double ExpectedSmoothPtpDuration(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var duration = 0.0;
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var distance = Math.Abs(targetJoints[index] - startJoints[index]);
|
||||
if (distance <= 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var limit = robot.JointLimits[index];
|
||||
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
|
||||
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
|
||||
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);
|
||||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||||
}
|
||||
|
||||
return duration;
|
||||
}
|
||||
|
||||
private static double DegreesToRadians(double degrees)
|
||||
{
|
||||
return degrees * Math.PI / 180.0;
|
||||
@@ -631,6 +797,30 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取一次测试执行生成的唯一稠密发送记录目录。
|
||||
/// </summary>
|
||||
private static string GetSingleDenseSendRunDirectory(string outputRoot)
|
||||
{
|
||||
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
|
||||
Assert.True(Directory.Exists(denseSendRoot));
|
||||
|
||||
var runDirectories = Directory.GetDirectories(denseSendRoot);
|
||||
Assert.Single(runDirectories);
|
||||
return runDirectories[0];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析空格分隔的纯文本数值列。
|
||||
/// </summary>
|
||||
private static double[] ParseColumns(string line)
|
||||
{
|
||||
return line
|
||||
.Split(' ', StringSplitOptions.RemoveEmptyEntries)
|
||||
.Select(static value => double.Parse(value))
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static void SetPrivateField<T>(FanucControllerRuntime runtime, string name, T value)
|
||||
{
|
||||
var field = typeof(FanucControllerRuntime).GetField(name, BindingFlags.Instance | BindingFlags.NonPublic);
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
using Flyshot.Runtime.Common;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Microsoft.Extensions.DependencyInjection;
|
||||
using System.Reflection;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 FANUC 运行时通过依赖注入解析时，能够稳定拿到日志依赖。
|
||||
/// </summary>
|
||||
public sealed class FanucControllerRuntimeLoggingRegistrationTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证宿主按 IControllerRuntime 解析 FANUC 运行时时，不会因为可选日志参数而退回到无日志实例。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ServiceProvider_Resolves_FanucControllerRuntime_WithLogger()
|
||||
{
|
||||
var services = new ServiceCollection();
|
||||
services.AddLogging();
|
||||
services.AddSingleton<IControllerRuntime, FanucControllerRuntime>();
|
||||
|
||||
using var serviceProvider = services.BuildServiceProvider();
|
||||
|
||||
var runtime = serviceProvider.GetRequiredService<IControllerRuntime>();
|
||||
var concreteRuntime = Assert.IsType<FanucControllerRuntime>(runtime);
|
||||
var loggerField = typeof(FanucControllerRuntime).GetField("_logger", BindingFlags.Instance | BindingFlags.NonPublic);
|
||||
|
||||
Assert.NotNull(loggerField);
|
||||
Assert.NotNull(loggerField!.GetValue(concreteRuntime));
|
||||
}
|
||||
}
|
||||
@@ -2,6 +2,7 @@ using System.Buffers.Binary;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
@@ -287,6 +288,31 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态变化日志会附带最近一次实际发送的目标关节轴，便于联调时对照控制目标。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ReceiveLoop_LogsLastSentTargetJointsWhenStatusChanges()
|
||||
{
|
||||
var logger = new CapturingLogger<FanucJ519Client>();
|
||||
using var client = new FanucJ519Client(logger);
|
||||
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: 42);
|
||||
_ = await _server.ReceiveAsync(_cts.Token);
|
||||
await Task.Delay(200, _cts.Token);
|
||||
|
||||
Assert.Contains(
|
||||
logger.Entries,
|
||||
entry => entry.Level == LogLevel.Information
|
||||
&& entry.Message.Contains("J519 最后一条发送目标关节轴", StringComparison.Ordinal)
|
||||
&& entry.Message.Contains("1.000, 2.000, 3.000, 4.000, 5.000, 6.000", StringComparison.Ordinal));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
|
||||
/// </summary>
|
||||
@@ -299,4 +325,68 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
responsePacket[0x0c] = 15;
|
||||
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 收集测试过程中的结构化日志，便于断言运行期输出内容。
|
||||
/// </summary>
|
||||
private sealed class CapturingLogger<T> : ILogger<T>
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取已记录的日志条目。
|
||||
/// </summary>
|
||||
public List<LogEntry> Entries { get; } = [];
|
||||
|
||||
/// <summary>
|
||||
/// 开始日志作用域；当前测试无需作用域，直接返回空对象。
|
||||
/// </summary>
|
||||
public IDisposable BeginScope<TState>(TState state)
|
||||
where TState : notnull
|
||||
{
|
||||
return NullScope.Instance;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 指示所有日志级别均启用，便于测试完整捕获输出。
|
||||
/// </summary>
|
||||
public bool IsEnabled(LogLevel logLevel)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 记录一条格式化后的日志消息。
|
||||
/// </summary>
|
||||
public void Log<TState>(
|
||||
LogLevel logLevel,
|
||||
EventId eventId,
|
||||
TState state,
|
||||
Exception? exception,
|
||||
Func<TState, Exception?, string> formatter)
|
||||
{
|
||||
Entries.Add(new LogEntry(logLevel, formatter(state, exception)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示一次日志记录。
|
||||
/// </summary>
|
||||
public sealed record LogEntry(LogLevel Level, string Message);
|
||||
|
||||
/// <summary>
|
||||
/// 提供空日志作用域，避免测试中额外分配。
|
||||
/// </summary>
|
||||
private sealed class NullScope : IDisposable
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取单例空作用域。
|
||||
/// </summary>
|
||||
public static NullScope Instance { get; } = new();
|
||||
|
||||
/// <summary>
|
||||
/// 释放空作用域；无需实际动作。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -337,6 +337,49 @@ public sealed class RuntimeOrchestrationTests
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证正式飞拍前若真实反馈仍未接近起点，则兼容层会拒绝继续执行。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_ExecuteTrajectoryByName_RejectsFlyshotWhenFeedbackIsNotNearStartAfterMoveToStart()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
ConfigRoot = configRoot
|
||||
};
|
||||
var runtime = new StickyFeedbackControllerRuntime(
|
||||
initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(20));
|
||||
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 exception = Assert.Throws<InvalidOperationException>(() =>
|
||||
service.ExecuteTrajectoryByName(
|
||||
"demo-flyshot",
|
||||
new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false)));
|
||||
|
||||
Assert.Contains("not near flyshot start", exception.Message, StringComparison.OrdinalIgnoreCase);
|
||||
Assert.Single(runtime.ExecuteCalls);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 ExecuteFlyShotTraj(wait=true) 会等待正式飞拍轨迹完成后再返回。
|
||||
/// </summary>
|
||||
@@ -897,3 +940,129 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 模拟 move_to_start 逻辑完成后，真实反馈仍停留在旧位置的测试运行时。
|
||||
/// </summary>
|
||||
internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime
|
||||
{
|
||||
private readonly object _lock = new();
|
||||
private readonly TimeSpan _firstMotionCompletionDelay;
|
||||
private double[] _jointPositions;
|
||||
private bool _isEnabled;
|
||||
private bool _isInMotion;
|
||||
private bool _firstMotionCompleted;
|
||||
|
||||
public StickyFeedbackControllerRuntime(
|
||||
IReadOnlyList<double> initialJointPositions,
|
||||
TimeSpan firstMotionCompletionDelay)
|
||||
{
|
||||
_jointPositions = initialJointPositions.ToArray();
|
||||
_firstMotionCompletionDelay = firstMotionCompletionDelay;
|
||||
}
|
||||
|
||||
public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _jointPositions.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
|
||||
|
||||
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)
|
||||
{
|
||||
ExecuteCalls.Add((result, finalJointPositions.ToArray()));
|
||||
if (!_firstMotionCompleted)
|
||||
{
|
||||
_isInMotion = true;
|
||||
_ = Task.Run(async () =>
|
||||
{
|
||||
await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false);
|
||||
lock (_lock)
|
||||
{
|
||||
// 模拟控制链逻辑上结束，但真实反馈仍卡在旧位置。
|
||||
_isInMotion = false;
|
||||
_firstMotionCompleted = true;
|
||||
}
|
||||
});
|
||||
return;
|
||||
}
|
||||
|
||||
_jointPositions = finalJointPositions.ToArray();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user