1003 lines
34 KiB
Python
1003 lines
34 KiB
Python
from __future__ import annotations
|
|
|
|
import csv
|
|
import json
|
|
import math
|
|
import time
|
|
from dataclasses import asdict, dataclass, replace
|
|
from pathlib import Path
|
|
|
|
import h5py
|
|
import matplotlib
|
|
import numpy as np
|
|
import yaml
|
|
|
|
matplotlib.use("Agg")
|
|
import matplotlib.pyplot as plt
|
|
|
|
from xr_rm_teleop.placo_ik_solver import (
|
|
QP_MAX_ITERATIONS,
|
|
QP_ORIENTATION_TOLERANCE_RAD,
|
|
QP_POSITION_TOLERANCE_M,
|
|
PlacoIkSolver,
|
|
_validated_transform,
|
|
)
|
|
from xr_rm_teleop.single_arm_velocity_teleop import (
|
|
SingleArmVelocityTeleop,
|
|
_matrix_to_quaternion,
|
|
_quaternion_to_matrix,
|
|
_so3_log,
|
|
)
|
|
|
|
METHOD_STYLE = {
|
|
"pinv": ("Jacobian 伪逆", "#D55E00", "-"),
|
|
"dls": ("DLS", "#0072B2", "--"),
|
|
"qp": ("优化 QP", "#009E73", "-."),
|
|
}
|
|
plt.rcParams.update(
|
|
{
|
|
"font.family": "sans-serif",
|
|
"font.sans-serif": [
|
|
"Noto Sans CJK JP",
|
|
"WenQuanYi Micro Hei",
|
|
"DejaVu Sans",
|
|
],
|
|
"axes.unicode_minus": False,
|
|
"figure.dpi": 120,
|
|
"savefig.dpi": 300,
|
|
}
|
|
)
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class EpisodeTrajectory:
|
|
source_path: Path
|
|
times_s: np.ndarray
|
|
target_poses: np.ndarray
|
|
initial_joints: np.ndarray
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class MethodSummary:
|
|
method: str
|
|
damping: float | None
|
|
success_rate: float
|
|
position_rmse_m: float
|
|
orientation_rmse_rad: float
|
|
max_joint_speed_deg_s: float
|
|
min_joint_margin: float = 0.0
|
|
mean_solve_ms: float = 0.0
|
|
max_solve_ms: float = 0.0
|
|
failure_count: int = 0
|
|
longest_failure_streak: int = 0
|
|
command_limited_count: int = 0
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class ReplayResult:
|
|
method: str
|
|
times_s: np.ndarray
|
|
target_poses: np.ndarray
|
|
actual_poses: np.ndarray
|
|
joints: np.ndarray
|
|
velocities: np.ndarray
|
|
position_errors_m: np.ndarray
|
|
orientation_errors_rad: np.ndarray
|
|
joint_margins: np.ndarray
|
|
solve_durations_ms: np.ndarray
|
|
success: np.ndarray
|
|
command_limited: np.ndarray
|
|
|
|
|
|
def _normalized_quaternion(values: np.ndarray) -> np.ndarray:
|
|
quaternion = np.asarray(values, dtype=float)
|
|
if quaternion.shape != (4,) or not np.isfinite(quaternion).all():
|
|
raise ValueError("quaternion must contain 4 finite values")
|
|
norm = float(np.linalg.norm(quaternion))
|
|
if norm <= 1e-12:
|
|
raise ValueError("quaternion norm must be positive")
|
|
return quaternion / norm
|
|
|
|
|
|
def _slerp_quaternion(
|
|
start: np.ndarray,
|
|
end: np.ndarray,
|
|
fraction: float,
|
|
) -> np.ndarray:
|
|
first = _normalized_quaternion(start)
|
|
second = _normalized_quaternion(end)
|
|
dot = float(np.dot(first, second))
|
|
if dot < 0.0:
|
|
second = -second
|
|
dot = -dot
|
|
dot = float(np.clip(dot, -1.0, 1.0))
|
|
if dot > 1.0 - 1e-8:
|
|
return _normalized_quaternion(
|
|
first + float(fraction) * (second - first)
|
|
)
|
|
angle = float(np.arccos(dot))
|
|
sine = float(np.sin(angle))
|
|
return _normalized_quaternion(
|
|
np.sin((1.0 - fraction) * angle) / sine * first
|
|
+ np.sin(fraction * angle) / sine * second
|
|
)
|
|
|
|
|
|
def resample_trajectory(
|
|
trajectory: EpisodeTrajectory,
|
|
sample_rate_hz: float,
|
|
) -> EpisodeTrajectory:
|
|
if not np.isfinite(sample_rate_hz) or sample_rate_hz <= 0.0:
|
|
raise ValueError("sample_rate_hz must be finite and positive")
|
|
source_times = np.asarray(trajectory.times_s, dtype=float)
|
|
poses = np.asarray(trajectory.target_poses, dtype=float)
|
|
if source_times.ndim != 1 or poses.shape != (source_times.size, 7):
|
|
raise ValueError("trajectory must contain N timestamps and N x 7 poses")
|
|
if source_times.size < 2 or np.any(np.diff(source_times) <= 0.0):
|
|
raise ValueError("trajectory timestamps must be strictly increasing")
|
|
|
|
duration = float(source_times[-1] - source_times[0])
|
|
count = int(round(duration * sample_rate_hz)) + 1
|
|
target_times = np.linspace(source_times[0], source_times[-1], count)
|
|
target_poses = np.empty((count, 7), dtype=float)
|
|
for axis in range(3):
|
|
target_poses[:, axis] = np.interp(
|
|
target_times,
|
|
source_times,
|
|
poses[:, axis],
|
|
)
|
|
for index, timestamp in enumerate(target_times):
|
|
right = int(np.searchsorted(source_times, timestamp, side="right"))
|
|
right = min(max(right, 1), source_times.size - 1)
|
|
left = right - 1
|
|
interval = source_times[right] - source_times[left]
|
|
fraction = float((timestamp - source_times[left]) / interval)
|
|
target_poses[index, 3:] = _slerp_quaternion(
|
|
poses[left, 3:], poses[right, 3:], fraction
|
|
)
|
|
target_poses[0] = poses[0]
|
|
target_poses[-1] = poses[-1]
|
|
return EpisodeTrajectory(
|
|
source_path=trajectory.source_path,
|
|
times_s=target_times - target_times[0],
|
|
target_poses=target_poses,
|
|
initial_joints=np.asarray(trajectory.initial_joints, dtype=float).copy(),
|
|
)
|
|
|
|
|
|
def _longest_true_run(mask: np.ndarray) -> slice:
|
|
values = np.asarray(mask, dtype=bool)
|
|
best_start = best_stop = start = 0
|
|
for index, enabled in enumerate(np.r_[values, False]):
|
|
if enabled:
|
|
continue
|
|
if index - start > best_stop - best_start:
|
|
best_start, best_stop = start, index
|
|
start = index + 1
|
|
if best_stop - best_start < 2:
|
|
raise ValueError("episode has no valid teleoperation run")
|
|
return slice(best_start, best_stop)
|
|
|
|
|
|
def load_episode(path: Path) -> EpisodeTrajectory:
|
|
source = Path(path).expanduser().resolve()
|
|
if not source.is_file() or source.suffix.lower() not in (".h5", ".hdf5"):
|
|
raise FileNotFoundError(f"episode not found: {source}")
|
|
with h5py.File(source, "r") as handle:
|
|
if str(handle.attrs.get("arm", "")) != "right_rm75":
|
|
raise ValueError("episode arm must be right_rm75")
|
|
if str(handle.attrs.get("pose_order", "")) != "x,y,z,qx,qy,qz,qw":
|
|
raise ValueError("episode pose_order is unsupported")
|
|
required = (
|
|
"debug/tcp/final_target_pose",
|
|
"debug/timestamps/control_monotonic_ns",
|
|
"debug/control/teleop_active",
|
|
"debug/control/action_valid",
|
|
"debug/control/command_sent",
|
|
"observations/qpos",
|
|
)
|
|
missing = [name for name in required if name not in handle]
|
|
if missing:
|
|
raise ValueError(f"episode datasets missing: {missing}")
|
|
poses = np.asarray(handle[required[0]], dtype=float)
|
|
timestamps = np.asarray(handle[required[1]], dtype=np.int64)
|
|
mask = np.logical_and.reduce(
|
|
[
|
|
np.asarray(handle[required[2]], dtype=bool),
|
|
np.asarray(handle[required[3]], dtype=bool),
|
|
np.asarray(handle[required[4]], dtype=bool),
|
|
]
|
|
)
|
|
qpos = np.asarray(handle[required[5]], dtype=float)
|
|
if poses.shape != (timestamps.size, 7) or qpos.shape != (timestamps.size, 8):
|
|
raise ValueError("episode arrays have inconsistent shapes")
|
|
if not np.isfinite(poses).all() or not np.isfinite(qpos).all():
|
|
raise ValueError("episode contains NaN/Inf")
|
|
selected = _longest_true_run(mask)
|
|
selected_times = timestamps[selected]
|
|
if np.any(np.diff(selected_times) <= 0):
|
|
raise ValueError("episode timestamps must be strictly increasing")
|
|
selected_poses = poses[selected].copy()
|
|
selected_poses[:, 3:] = np.asarray(
|
|
[_normalized_quaternion(value) for value in selected_poses[:, 3:]]
|
|
)
|
|
return EpisodeTrajectory(
|
|
source_path=source,
|
|
times_s=(selected_times - selected_times[0]) * 1e-9,
|
|
target_poses=selected_poses,
|
|
initial_joints=qpos[selected.start, :7].copy(),
|
|
)
|
|
|
|
|
|
def load_right_config(path: Path) -> dict[str, float]:
|
|
with Path(path).open("r", encoding="utf-8") as stream:
|
|
document = yaml.safe_load(stream)
|
|
parameters = document["single_arm_velocity_teleop"]["ros__parameters"]
|
|
names = (
|
|
"qp_j3_reference_deg",
|
|
"qp_j3_weight",
|
|
"qp_j4_min_deg",
|
|
"qp_j4_warn_deg",
|
|
"qp_j4_weight",
|
|
"qp_manipulability_sigma_stop",
|
|
"qp_manipulability_sigma_warn",
|
|
"qp_manipulability_weight",
|
|
"joint_max_speed",
|
|
"joint_max_acc",
|
|
)
|
|
return {name: float(parameters[name]) for name in names}
|
|
|
|
|
|
def _rotation_z(angle: float) -> np.ndarray:
|
|
cosine, sine = math.cos(angle), math.sin(angle)
|
|
return np.asarray(
|
|
[
|
|
[cosine, -sine, 0.0],
|
|
[sine, cosine, 0.0],
|
|
[0.0, 0.0, 1.0],
|
|
]
|
|
)
|
|
|
|
|
|
def orientation_error_rad(actual: np.ndarray, target: np.ndarray) -> float:
|
|
delta = np.asarray(target) @ np.asarray(actual).T
|
|
cosine = float(np.clip((np.trace(delta) - 1.0) * 0.5, -1.0, 1.0))
|
|
return float(math.acos(cosine))
|
|
|
|
|
|
def normalized_joint_margin(
|
|
joints: np.ndarray,
|
|
lower: np.ndarray,
|
|
upper: np.ndarray,
|
|
) -> float:
|
|
values = np.asarray(joints, dtype=float)
|
|
lower_values = np.asarray(lower, dtype=float)
|
|
upper_values = np.asarray(upper, dtype=float)
|
|
span = upper_values - lower_values
|
|
if np.any(span <= 0.0):
|
|
raise ValueError("joint limits must have positive spans")
|
|
margins = np.minimum(
|
|
values - lower_values,
|
|
upper_values - values,
|
|
) / span
|
|
return float(np.min(margins))
|
|
|
|
|
|
def choose_dls_damping(candidates: list[MethodSummary]) -> float:
|
|
if not candidates or any(value.damping is None for value in candidates):
|
|
raise ValueError("DLS candidates must contain damping values")
|
|
selected = min(
|
|
candidates,
|
|
key=lambda value: (
|
|
-value.success_rate,
|
|
value.position_rmse_m / 0.002
|
|
+ value.orientation_rmse_rad / 0.005,
|
|
value.max_joint_speed_deg_s,
|
|
),
|
|
)
|
|
return float(selected.damping)
|
|
|
|
|
|
def _longest_failure_streak(success: np.ndarray) -> int:
|
|
longest = current = 0
|
|
for value in np.asarray(success, dtype=bool):
|
|
current = 0 if value else current + 1
|
|
longest = max(longest, current)
|
|
return longest
|
|
|
|
|
|
def summarize_result(
|
|
result: ReplayResult,
|
|
damping: float | None = None,
|
|
) -> MethodSummary:
|
|
return MethodSummary(
|
|
method=result.method,
|
|
damping=damping,
|
|
success_rate=float(np.mean(result.success)),
|
|
position_rmse_m=float(
|
|
np.sqrt(np.mean(result.position_errors_m**2))
|
|
),
|
|
orientation_rmse_rad=float(
|
|
np.sqrt(np.mean(result.orientation_errors_rad**2))
|
|
),
|
|
max_joint_speed_deg_s=float(
|
|
np.max(np.abs(np.degrees(result.velocities)))
|
|
),
|
|
min_joint_margin=float(np.min(result.joint_margins)),
|
|
mean_solve_ms=float(np.mean(result.solve_durations_ms)),
|
|
max_solve_ms=float(np.max(result.solve_durations_ms)),
|
|
failure_count=int(np.count_nonzero(~result.success)),
|
|
longest_failure_streak=_longest_failure_streak(result.success),
|
|
command_limited_count=int(np.count_nonzero(result.command_limited)),
|
|
)
|
|
|
|
|
|
def limit_joint_command(
|
|
*,
|
|
target: np.ndarray,
|
|
previous_target: np.ndarray,
|
|
previous_velocity: np.ndarray,
|
|
max_speed: float,
|
|
max_acceleration: float,
|
|
dt: float,
|
|
) -> tuple[np.ndarray, np.ndarray, bool]:
|
|
limited_target, limited_velocity = (
|
|
SingleArmVelocityTeleop._limit_joint_command_step(
|
|
target=np.asarray(target, dtype=float).tolist(),
|
|
previous_target=np.asarray(previous_target, dtype=float).tolist(),
|
|
previous_velocity=np.asarray(previous_velocity, dtype=float).tolist(),
|
|
max_speed=max_speed,
|
|
max_acceleration=max_acceleration,
|
|
dt=dt,
|
|
)
|
|
)
|
|
target_array = np.asarray(limited_target, dtype=float)
|
|
velocity_array = np.asarray(limited_velocity, dtype=float)
|
|
return (
|
|
target_array,
|
|
velocity_array,
|
|
not np.allclose(target_array, target, atol=1e-12, rtol=0.0),
|
|
)
|
|
|
|
|
|
def _pose_to_transform(pose: np.ndarray) -> np.ndarray:
|
|
values = np.asarray(pose, dtype=float)
|
|
transform = np.eye(4)
|
|
transform[:3, 3] = values[:3]
|
|
transform[:3, :3] = _quaternion_to_matrix(tuple(values[3:]))
|
|
return transform
|
|
|
|
|
|
def _transform_to_pose(transform: np.ndarray) -> np.ndarray:
|
|
quaternion = _matrix_to_quaternion(transform[:3, :3])
|
|
return np.asarray([*transform[:3, 3], *quaternion], dtype=float)
|
|
|
|
|
|
class DifferentialIkSolver:
|
|
def __init__(
|
|
self,
|
|
urdf_path: Path,
|
|
dt: float,
|
|
method: str,
|
|
damping: float = 0.0,
|
|
) -> None:
|
|
if method not in ("pinv", "dls"):
|
|
raise ValueError("method must be pinv or dls")
|
|
if method == "dls" and damping <= 0.0:
|
|
raise ValueError("DLS damping must be positive")
|
|
self._kinematics = PlacoIkSolver(str(urdf_path), dt, "right")
|
|
self._dt = dt
|
|
self._method = method
|
|
self._damping = float(damping)
|
|
self._actual_joints: np.ndarray | None = None
|
|
|
|
@property
|
|
def joint_limits(self) -> np.ndarray:
|
|
return self._kinematics._joint_limits.copy()
|
|
|
|
def update_joint_state(self, joints: list[float]) -> np.ndarray:
|
|
self._actual_joints = np.asarray(joints, dtype=float).copy()
|
|
return self._kinematics.update_joint_state(joints)
|
|
|
|
def _set_internal_joints(self, joints: np.ndarray) -> None:
|
|
robot = self._kinematics._robot
|
|
robot.state.q[self._kinematics._q_offsets] = joints
|
|
robot.update_kinematics()
|
|
|
|
def _errors(self, target: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
|
robot = self._kinematics._robot
|
|
world_base = robot.get_T_world_frame(self._kinematics._base_frame)
|
|
world_tcp = robot.get_T_world_frame(self._kinematics._tcp_frame)
|
|
world_target = world_base @ target
|
|
position = world_target[:3, 3] - world_tcp[:3, 3]
|
|
orientation = _so3_log(
|
|
world_target[:3, :3] @ world_tcp[:3, :3].T
|
|
)
|
|
return position, orientation
|
|
|
|
def solve(self, target: np.ndarray) -> list[float]:
|
|
if self._actual_joints is None:
|
|
raise RuntimeError("joint state must be initialized before IK solve")
|
|
target = _validated_transform(target)
|
|
actual = self._actual_joints.copy()
|
|
result = actual.copy()
|
|
try:
|
|
for _ in range(QP_MAX_ITERATIONS):
|
|
self._set_internal_joints(result)
|
|
position, orientation = self._errors(target)
|
|
if (
|
|
np.linalg.norm(position) <= QP_POSITION_TOLERANCE_M
|
|
and np.linalg.norm(orientation)
|
|
<= QP_ORIENTATION_TOLERANCE_RAD
|
|
):
|
|
return result.tolist()
|
|
jacobian = self._kinematics._active_tcp_jacobian()
|
|
desired_twist = np.r_[position, orientation] / self._dt
|
|
if self._method == "pinv":
|
|
joint_velocity = np.linalg.pinv(jacobian) @ desired_twist
|
|
else:
|
|
system = (
|
|
jacobian @ jacobian.T
|
|
+ self._damping**2 * np.eye(6)
|
|
)
|
|
joint_velocity = jacobian.T @ np.linalg.solve(
|
|
system, desired_twist
|
|
)
|
|
candidate = result + joint_velocity * self._dt
|
|
self._kinematics._validate_result(candidate, result)
|
|
result = candidate
|
|
raise RuntimeError(
|
|
f"{self._method} did not converge after "
|
|
f"{QP_MAX_ITERATIONS} iterations"
|
|
)
|
|
except Exception:
|
|
self._set_internal_joints(actual)
|
|
raise
|
|
|
|
|
|
class QpSolverAdapter:
|
|
def __init__(self, solver: PlacoIkSolver) -> None:
|
|
self._solver = solver
|
|
|
|
@property
|
|
def joint_limits(self) -> np.ndarray:
|
|
return self._solver._joint_limits.copy()
|
|
|
|
def update_joint_state(self, joints: list[float]) -> np.ndarray:
|
|
return self._solver.update_joint_state(joints)
|
|
|
|
def solve(self, target: np.ndarray) -> list[float]:
|
|
return self._solver.solve(target)
|
|
|
|
|
|
def make_qp_solver(
|
|
urdf_path: Path,
|
|
dt: float,
|
|
config: dict[str, float],
|
|
) -> QpSolverAdapter:
|
|
return QpSolverAdapter(
|
|
PlacoIkSolver(
|
|
str(urdf_path),
|
|
dt,
|
|
"right",
|
|
j3_reference_deg=config["qp_j3_reference_deg"],
|
|
j3_weight=config["qp_j3_weight"],
|
|
j4_min_deg=config["qp_j4_min_deg"],
|
|
j4_warn_deg=config["qp_j4_warn_deg"],
|
|
j4_weight=config["qp_j4_weight"],
|
|
manipulability_sigma_stop=config[
|
|
"qp_manipulability_sigma_stop"
|
|
],
|
|
manipulability_sigma_warn=config[
|
|
"qp_manipulability_sigma_warn"
|
|
],
|
|
manipulability_weight=config["qp_manipulability_weight"],
|
|
)
|
|
)
|
|
|
|
|
|
def run_replay(
|
|
method: str,
|
|
solver,
|
|
trajectory: EpisodeTrajectory,
|
|
*,
|
|
max_speed: float,
|
|
max_acceleration: float,
|
|
measure_time: bool = True,
|
|
) -> ReplayResult:
|
|
count = trajectory.times_s.size
|
|
dt = float(np.median(np.diff(trajectory.times_s)))
|
|
joints = np.empty((count, 7))
|
|
velocities = np.zeros((count, 7))
|
|
actual_poses = np.empty((count, 7))
|
|
position_errors = np.empty(count)
|
|
orientation_errors = np.empty(count)
|
|
margins = np.empty(count)
|
|
durations = np.zeros(count)
|
|
success = np.zeros(count, dtype=bool)
|
|
command_limited = np.zeros(count, dtype=bool)
|
|
current = trajectory.initial_joints.copy()
|
|
previous_velocity = np.zeros(7)
|
|
lower, upper = solver.joint_limits.T
|
|
|
|
for index, pose in enumerate(trajectory.target_poses):
|
|
solver.update_joint_state(current.tolist())
|
|
started = time.perf_counter_ns()
|
|
try:
|
|
candidate = np.asarray(
|
|
solver.solve(_pose_to_transform(pose)), dtype=float
|
|
)
|
|
success[index] = True
|
|
except Exception:
|
|
candidate = current.copy()
|
|
previous_velocity = np.zeros(7)
|
|
durations[index] = (
|
|
(time.perf_counter_ns() - started) * 1e-6 if measure_time else 0.0
|
|
)
|
|
if success[index]:
|
|
current, previous_velocity, command_limited[index] = (
|
|
limit_joint_command(
|
|
target=candidate,
|
|
previous_target=current,
|
|
previous_velocity=previous_velocity,
|
|
max_speed=max_speed,
|
|
max_acceleration=max_acceleration,
|
|
dt=dt,
|
|
)
|
|
)
|
|
actual_transform = solver.update_joint_state(current.tolist())
|
|
actual_pose = _transform_to_pose(actual_transform)
|
|
joints[index] = current
|
|
velocities[index] = previous_velocity
|
|
actual_poses[index] = actual_pose
|
|
position_errors[index] = np.linalg.norm(pose[:3] - actual_pose[:3])
|
|
orientation_errors[index] = orientation_error_rad(
|
|
_quaternion_to_matrix(tuple(actual_pose[3:])),
|
|
_quaternion_to_matrix(tuple(pose[3:])),
|
|
)
|
|
margins[index] = normalized_joint_margin(current, lower, upper)
|
|
|
|
return ReplayResult(
|
|
method=method,
|
|
times_s=trajectory.times_s.copy(),
|
|
target_poses=trajectory.target_poses.copy(),
|
|
actual_poses=actual_poses,
|
|
joints=joints,
|
|
velocities=velocities,
|
|
position_errors_m=position_errors,
|
|
orientation_errors_rad=orientation_errors,
|
|
joint_margins=margins,
|
|
solve_durations_ms=durations,
|
|
success=success,
|
|
command_limited=command_limited,
|
|
)
|
|
|
|
|
|
DLS_DAMPING_CANDIDATES = (0.001, 0.003, 0.01, 0.03, 0.1, 0.3)
|
|
|
|
|
|
def evaluate_methods(
|
|
trajectory: EpisodeTrajectory,
|
|
urdf_path: Path,
|
|
config: dict[str, float],
|
|
timing_repeats: int,
|
|
) -> tuple[dict[str, ReplayResult], dict[str, MethodSummary], float]:
|
|
if timing_repeats < 1:
|
|
raise ValueError("timing_repeats must be positive")
|
|
dt = float(np.median(np.diff(trajectory.times_s)))
|
|
speed = math.radians(config["joint_max_speed"])
|
|
acceleration = math.radians(config["joint_max_acc"])
|
|
damping_results = []
|
|
for damping in DLS_DAMPING_CANDIDATES:
|
|
replay = run_replay(
|
|
"dls",
|
|
DifferentialIkSolver(urdf_path, dt, "dls", damping),
|
|
trajectory,
|
|
max_speed=speed,
|
|
max_acceleration=acceleration,
|
|
measure_time=False,
|
|
)
|
|
damping_results.append(summarize_result(replay, damping))
|
|
selected_damping = choose_dls_damping(damping_results)
|
|
|
|
factories = {
|
|
"pinv": lambda: DifferentialIkSolver(urdf_path, dt, "pinv"),
|
|
"dls": lambda: DifferentialIkSolver(
|
|
urdf_path, dt, "dls", selected_damping
|
|
),
|
|
"qp": lambda: make_qp_solver(urdf_path, dt, config),
|
|
}
|
|
results = {}
|
|
summaries = {}
|
|
for name, factory in factories.items():
|
|
run_replay(
|
|
name,
|
|
factory(),
|
|
trajectory,
|
|
max_speed=speed,
|
|
max_acceleration=acceleration,
|
|
)
|
|
result = run_replay(
|
|
name,
|
|
factory(),
|
|
trajectory,
|
|
max_speed=speed,
|
|
max_acceleration=acceleration,
|
|
)
|
|
timed = []
|
|
for _ in range(timing_repeats):
|
|
repeated = run_replay(
|
|
name,
|
|
factory(),
|
|
trajectory,
|
|
max_speed=speed,
|
|
max_acceleration=acceleration,
|
|
)
|
|
if not np.allclose(
|
|
repeated.joints,
|
|
result.joints,
|
|
atol=1e-10,
|
|
rtol=0.0,
|
|
):
|
|
raise RuntimeError(f"{name} replay is not deterministic")
|
|
timed.append(repeated.solve_durations_ms)
|
|
timing_matrix = np.stack(timed)
|
|
result = replace(
|
|
result,
|
|
solve_durations_ms=np.mean(timing_matrix, axis=0),
|
|
)
|
|
results[name] = result
|
|
summaries[name] = replace(
|
|
summarize_result(
|
|
result,
|
|
selected_damping if name == "dls" else None,
|
|
),
|
|
mean_solve_ms=float(np.mean(timing_matrix)),
|
|
max_solve_ms=float(np.max(timing_matrix)),
|
|
)
|
|
return results, summaries, selected_damping
|
|
|
|
|
|
def _save_figure(fig, output_dir: Path, name: str) -> None:
|
|
fig.savefig(output_dir / f"{name}.svg", bbox_inches="tight")
|
|
fig.savefig(
|
|
output_dir / f"{name}.png",
|
|
dpi=300,
|
|
bbox_inches="tight",
|
|
facecolor="white",
|
|
)
|
|
plt.close(fig)
|
|
|
|
|
|
def _style_axes(axes) -> None:
|
|
for axis in np.asarray(axes).flat:
|
|
axis.grid(True, alpha=0.2, linewidth=0.6)
|
|
axis.spines["top"].set_visible(False)
|
|
axis.spines["right"].set_visible(False)
|
|
|
|
|
|
def plot_tracking_error(
|
|
results: dict[str, ReplayResult],
|
|
output_dir: Path,
|
|
) -> None:
|
|
fig, axes = plt.subplots(
|
|
2, 1, figsize=(7.2, 5.2), sharex=True, constrained_layout=True
|
|
)
|
|
for name, result in results.items():
|
|
label, color, linestyle = METHOD_STYLE[name]
|
|
values = (
|
|
result.position_errors_m * 1000.0,
|
|
np.degrees(result.orientation_errors_rad),
|
|
)
|
|
for axis, series in zip(axes, values):
|
|
axis.plot(
|
|
result.times_s,
|
|
series,
|
|
color=color,
|
|
linestyle=linestyle,
|
|
linewidth=1.15,
|
|
label=label,
|
|
)
|
|
failed = ~result.success
|
|
axis.scatter(
|
|
result.times_s[failed],
|
|
series[failed],
|
|
color=color,
|
|
marker="x",
|
|
s=10,
|
|
linewidths=0.7,
|
|
zorder=3,
|
|
)
|
|
axes[0].set_ylabel("位置误差 (mm)")
|
|
axes[1].set_ylabel("姿态误差 (°)")
|
|
axes[1].set_xlabel("时间 (s)")
|
|
axes[0].legend(frameon=False, ncol=3, loc="upper right")
|
|
_style_axes(axes)
|
|
_save_figure(fig, output_dir, "figure_2_11_tracking_error")
|
|
|
|
|
|
def plot_joint_constraints(
|
|
results: dict[str, ReplayResult],
|
|
output_dir: Path,
|
|
) -> None:
|
|
fig, axes = plt.subplots(
|
|
2, 1, figsize=(7.2, 5.2), sharex=True, constrained_layout=True
|
|
)
|
|
for name, result in results.items():
|
|
label, color, linestyle = METHOD_STYLE[name]
|
|
speed = np.max(np.abs(np.degrees(result.velocities)), axis=1)
|
|
axes[0].plot(
|
|
result.times_s,
|
|
speed,
|
|
color=color,
|
|
linestyle=linestyle,
|
|
linewidth=1.15,
|
|
label=label,
|
|
)
|
|
axes[1].plot(
|
|
result.times_s,
|
|
result.joint_margins,
|
|
color=color,
|
|
linestyle=linestyle,
|
|
linewidth=1.15,
|
|
label=label,
|
|
)
|
|
axes[0].axhline(
|
|
180.0,
|
|
color="#B2182B",
|
|
linestyle=":",
|
|
linewidth=1.0,
|
|
label="速度上限 180°/s",
|
|
)
|
|
axes[0].set_ylabel("最大关节速度 (°/s)")
|
|
axes[1].set_ylabel("最小归一化关节裕度")
|
|
axes[1].set_xlabel("时间 (s)")
|
|
axes[0].legend(frameon=False, ncol=2, loc="upper right")
|
|
_style_axes(axes)
|
|
_save_figure(fig, output_dir, "figure_2_12_joint_constraints")
|
|
|
|
|
|
def _annotate_bars(axis, bars) -> None:
|
|
for bar in bars:
|
|
value = float(bar.get_height())
|
|
axis.annotate(
|
|
f"{value:.3g}",
|
|
(bar.get_x() + bar.get_width() / 2.0, value),
|
|
xytext=(0, 3 if value >= 0.0 else -9),
|
|
textcoords="offset points",
|
|
ha="center",
|
|
va="bottom" if value >= 0.0 else "top",
|
|
fontsize=6.5,
|
|
)
|
|
|
|
|
|
def plot_summary(
|
|
summaries: dict[str, MethodSummary],
|
|
output_dir: Path,
|
|
) -> None:
|
|
names = list(METHOD_STYLE)
|
|
labels = [METHOD_STYLE[name][0] for name in names]
|
|
colors = [METHOD_STYLE[name][1] for name in names]
|
|
values = [
|
|
[summaries[name].position_rmse_m * 1000.0 for name in names],
|
|
[np.degrees(summaries[name].orientation_rmse_rad) for name in names],
|
|
[summaries[name].max_joint_speed_deg_s for name in names],
|
|
[summaries[name].min_joint_margin for name in names],
|
|
[summaries[name].success_rate * 100.0 for name in names],
|
|
]
|
|
titles = [
|
|
"(a) 位置 RMSE (mm)",
|
|
"(b) 姿态 RMSE (°)",
|
|
"(c) 最大关节速度 (°/s)",
|
|
"(d) 最小归一化关节裕度",
|
|
"(f) 求解成功率 (%)",
|
|
]
|
|
fig, axes = plt.subplots(2, 3, figsize=(8.2, 5.4), constrained_layout=True)
|
|
flat_axes = axes.flat
|
|
for axis, data, title in zip(
|
|
[flat_axes[0], flat_axes[1], flat_axes[2], flat_axes[3], flat_axes[5]],
|
|
values,
|
|
titles,
|
|
):
|
|
bars = axis.bar(
|
|
labels,
|
|
data,
|
|
color=colors,
|
|
edgecolor="black",
|
|
linewidth=0.5,
|
|
hatch=["//", "\\\\", ".."],
|
|
)
|
|
axis.set_title(title, fontsize=9)
|
|
axis.tick_params(axis="x", labelrotation=18, labelsize=7)
|
|
_annotate_bars(axis, bars)
|
|
timing_axis = flat_axes[4]
|
|
x = np.arange(len(names))
|
|
width = 0.36
|
|
mean_bars = timing_axis.bar(
|
|
x - width / 2.0,
|
|
[summaries[name].mean_solve_ms for name in names],
|
|
width,
|
|
color=colors,
|
|
edgecolor="black",
|
|
linewidth=0.5,
|
|
label="平均",
|
|
)
|
|
max_bars = timing_axis.bar(
|
|
x + width / 2.0,
|
|
[summaries[name].max_solve_ms for name in names],
|
|
width,
|
|
color=colors,
|
|
alpha=0.45,
|
|
edgecolor="black",
|
|
linewidth=0.5,
|
|
label="最大",
|
|
)
|
|
timing_axis.set_title("(e) 单周期求解时间 (ms)", fontsize=9)
|
|
timing_axis.set_xticks(x, labels, rotation=18)
|
|
timing_axis.tick_params(axis="x", labelsize=7)
|
|
timing_axis.legend(frameon=False, fontsize=7)
|
|
_annotate_bars(timing_axis, mean_bars)
|
|
_annotate_bars(timing_axis, max_bars)
|
|
_style_axes(axes)
|
|
_save_figure(fig, output_dir, "figure_2_13_summary")
|
|
|
|
|
|
def _write_samples(
|
|
path: Path,
|
|
results: dict[str, ReplayResult],
|
|
) -> None:
|
|
pose_names = [
|
|
"x", "y", "z", "qx", "qy", "qz", "qw",
|
|
]
|
|
fields = (
|
|
["method", "time_s"]
|
|
+ [f"target_{name}" for name in pose_names]
|
|
+ [f"actual_{name}" for name in pose_names]
|
|
+ ["position_error_m", "orientation_error_rad"]
|
|
+ [f"q{index}" for index in range(1, 8)]
|
|
+ [f"qd{index}" for index in range(1, 8)]
|
|
+ ["joint_margin", "solve_ms", "success", "command_limited"]
|
|
)
|
|
with path.open("w", encoding="utf-8", newline="") as stream:
|
|
writer = csv.writer(stream)
|
|
writer.writerow(fields)
|
|
for name, result in results.items():
|
|
for index, timestamp in enumerate(result.times_s):
|
|
writer.writerow(
|
|
[name, timestamp]
|
|
+ result.target_poses[index].tolist()
|
|
+ result.actual_poses[index].tolist()
|
|
+ [
|
|
result.position_errors_m[index],
|
|
result.orientation_errors_rad[index],
|
|
]
|
|
+ result.joints[index].tolist()
|
|
+ result.velocities[index].tolist()
|
|
+ [
|
|
result.joint_margins[index],
|
|
result.solve_durations_ms[index],
|
|
bool(result.success[index]),
|
|
bool(result.command_limited[index]),
|
|
]
|
|
)
|
|
|
|
|
|
def _method_list_at_extreme(
|
|
summaries: dict[str, MethodSummary],
|
|
attribute: str,
|
|
*,
|
|
maximum: bool,
|
|
) -> str:
|
|
values = {name: getattr(summary, attribute) for name, summary in summaries.items()}
|
|
extreme = (max if maximum else min)(values.values())
|
|
return "、".join(
|
|
METHOD_STYLE[name][0]
|
|
for name, value in values.items()
|
|
if math.isclose(value, extreme, rel_tol=1e-9, abs_tol=1e-12)
|
|
)
|
|
|
|
|
|
def _write_analysis(
|
|
path: Path,
|
|
summaries: dict[str, MethodSummary],
|
|
selected_damping: float,
|
|
source_path: Path,
|
|
sample_rate_hz: float,
|
|
) -> None:
|
|
rows = []
|
|
for name in METHOD_STYLE:
|
|
summary = summaries[name]
|
|
rows.append(
|
|
"| "
|
|
+ " | ".join(
|
|
[
|
|
METHOD_STYLE[name][0],
|
|
f"{summary.position_rmse_m:.6f}",
|
|
f"{summary.orientation_rmse_rad:.6f}",
|
|
f"{summary.max_joint_speed_deg_s:.3f}",
|
|
f"{summary.min_joint_margin:.4f}",
|
|
f"{summary.mean_solve_ms:.3f} / {summary.max_solve_ms:.3f}",
|
|
f"{summary.success_rate * 100.0:.2f}%",
|
|
]
|
|
)
|
|
+ " |"
|
|
)
|
|
position_best = _method_list_at_extreme(
|
|
summaries, "position_rmse_m", maximum=False
|
|
)
|
|
orientation_best = _method_list_at_extreme(
|
|
summaries, "orientation_rmse_rad", maximum=False
|
|
)
|
|
success_best = _method_list_at_extreme(
|
|
summaries, "success_rate", maximum=True
|
|
)
|
|
text = f"""# 2.3.4 三种逆运动学方法对比补充分析
|
|
|
|
本结果是基于真实遥操作目标轨迹的离线运动学对比，不代表真机闭环实验。数据来自
|
|
`{source_path}`，目标位姿以 {sample_rate_hz:.1f} Hz 重采样；三种方法使用同一初始
|
|
关节状态、同一 URDF、相同收敛阈值和共同的输出速度/加速度限制。
|
|
|
|
DLS 扫描的固定阻尼候选为 {", ".join(map(str, DLS_DAMPING_CANDIDATES))}，本轨迹选定
|
|
`{selected_damping:g}`。该参数是在当前评价轨迹上选优，不应解释为跨轨迹最优参数。
|
|
|
|
| 方法 | 位置 RMSE (m) | 姿态 RMSE (rad) | 最大关节速度 (°/s) | 最小归一化裕度 | 平均/最大求解时间 (ms) | 成功率 |
|
|
| --- | ---: | ---: | ---: | ---: | ---: | ---: |
|
|
{chr(10).join(rows)}
|
|
|
|
图 2-11 三种逆运动学方法的末端位置与姿态跟踪误差。曲线来自统一时间轴，叉号表示该
|
|
周期数值求解失败并保持上一安全关节状态。
|
|
|
|
图 2-12 三种逆运动学方法的最大关节速度与最小归一化关节安全裕度。红色虚线表示
|
|
180°/s 输出速度上限，裕度越大表示离关节位置边界越远。
|
|
|
|
图 2-13 三种逆运动学方法的综合性能对比，包括误差、关节运动、求解时间和成功率。
|
|
|
|
按本次单轨迹数值比较，位置 RMSE 最低的方法为{position_best}，姿态 RMSE 最低的方法为
|
|
{orientation_best}，成功率最高的方法为{success_best}。这些结论只描述本次离线复放，
|
|
未进行统计显著性检验。
|
|
|
|
当前优化 QP 除六维末端主任务外，还保留项目中的 J3 参考软任务、J4 硬下界与软缓冲，
|
|
以及按最小奇异值动态激活的六维可操作度任务；伪逆和 DLS 基线不包含这些附加任务。
|
|
"""
|
|
path.write_text(text, encoding="utf-8")
|
|
|
|
|
|
def write_outputs(
|
|
output_dir: Path,
|
|
results: dict[str, ReplayResult],
|
|
summaries: dict[str, MethodSummary],
|
|
*,
|
|
selected_damping: float,
|
|
source_path: Path,
|
|
git_commit: str,
|
|
) -> None:
|
|
output_dir = Path(output_dir)
|
|
output_dir.mkdir(parents=True, exist_ok=True)
|
|
sample_rate_hz = 1.0 / float(
|
|
np.median(np.diff(next(iter(results.values())).times_s))
|
|
)
|
|
_write_samples(output_dir / "samples.csv", results)
|
|
payload = {
|
|
"source_episode": str(source_path),
|
|
"git_commit": git_commit,
|
|
"sample_rate_hz": sample_rate_hz,
|
|
"selected_dls_damping": selected_damping,
|
|
"methods": {
|
|
name: asdict(summary) for name, summary in summaries.items()
|
|
},
|
|
}
|
|
(output_dir / "summary.json").write_text(
|
|
json.dumps(payload, ensure_ascii=False, indent=2) + "\n",
|
|
encoding="utf-8",
|
|
)
|
|
plot_tracking_error(results, output_dir)
|
|
plot_joint_constraints(results, output_dir)
|
|
plot_summary(summaries, output_dir)
|
|
_write_analysis(
|
|
output_dir / "analysis_2.3.4.md",
|
|
summaries,
|
|
selected_damping,
|
|
source_path,
|
|
sample_rate_hz,
|
|
)
|