diff --git a/.script/identification/fit_gravity_torque.py b/.script/identification/fit_gravity_torque.py
new file mode 100644
index 00000000..264d24de
--- /dev/null
+++ b/.script/identification/fit_gravity_torque.py
@@ -0,0 +1,495 @@
+#!/usr/bin/env python3
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+import math
+import sys
+from dataclasses import asdict, dataclass
+from pathlib import Path
+from typing import Iterable
+
+
+def wrap_to_pi(angle: float) -> float:
+ wrapped = math.remainder(angle, 2.0 * math.pi)
+ if wrapped <= -math.pi:
+ wrapped += 2.0 * math.pi
+ return wrapped
+
+
+def circular_mean(angles: list[float]) -> float:
+ sin_sum = sum(math.sin(angle) for angle in angles)
+ cos_sum = sum(math.cos(angle) for angle in angles)
+ if abs(sin_sum) < 1e-12 and abs(cos_sum) < 1e-12:
+ return wrap_to_pi(angles[0])
+ return wrap_to_pi(math.atan2(sin_sum, cos_sum))
+
+
+@dataclass
+class FitResult:
+ signal_name: str
+ sample_count: int
+ angle_min: float
+ angle_max: float
+ angle_span: float
+ gain: float
+ phase: float
+ phase_deg: float
+ zero_crossing_angle: float
+ zero_crossing_angle_deg: float
+ peak_angle: float
+ peak_angle_deg: float
+ rmse: float
+ mae: float
+ r2: float
+ design_condition: float
+
+
+@dataclass
+class PairingSummary:
+ available_point_count: int
+ paired_point_count: int
+ dropped_point_count: int
+ mean_control_friction: float
+ mean_torque_friction: float
+
+
+@dataclass
+class AveragedSample:
+ point_index: int
+ angle: float
+ setpoint: float
+ control_torque: float
+ torque: float
+ control_friction: float
+ torque_friction: float
+
+
+def parse_args() -> argparse.Namespace:
+ parser = argparse.ArgumentParser(
+ description=(
+ "Fit the gray-box gravity torque model u_ff = G * sin(theta + phi) "
+ "from static torque test CSV data."
+ )
+ )
+ parser.add_argument("csv_path", type=Path, help="Path to static_torque_test_controller CSV")
+ parser.add_argument(
+ "--target",
+ type=str,
+ default=None,
+ help="Target prefix such as /gimbal/pitch. If omitted, infer from CSV headers.",
+ )
+ parser.add_argument(
+ "--signal",
+ choices=("control_torque", "torque"),
+ default="torque",
+ help="Primary signal to fit. Defaults to torque.",
+ )
+ parser.add_argument(
+ "--velocity-threshold",
+ type=float,
+ default=0.1,
+ help=(
+ "Discard rows with abs(velocity) larger than this threshold. "
+ "Use a negative value to disable filtering."
+ ),
+ )
+ parser.add_argument(
+ "--json-output",
+ type=Path,
+ default=None,
+ help="Optional path to write fit summary as JSON.",
+ )
+ return parser.parse_args()
+
+
+def infer_target(fieldnames: Iterable[str]) -> str:
+ angle_targets = {name[: -len("/angle")] for name in fieldnames if name.endswith("/angle")}
+ if len(angle_targets) != 1:
+ raise ValueError(
+ "Failed to infer target from CSV headers. Please pass --target explicitly."
+ )
+ return next(iter(angle_targets))
+
+
+def read_rows(path: Path) -> tuple[list[dict[str, str]], list[str]]:
+ with path.open(newline="") as csv_file:
+ reader = csv.DictReader(csv_file)
+ rows = list(reader)
+ if reader.fieldnames is None:
+ raise ValueError(f"CSV file has no header: {path}")
+ return rows, reader.fieldnames
+
+
+def require_columns(fieldnames: list[str], target: str) -> dict[str, str]:
+ columns = {
+ "update_count": "update_count",
+ "elapsed_s": "elapsed_s",
+ "control_torque": f"{target}/control_torque",
+ "torque": f"{target}/torque",
+ "velocity": f"{target}/velocity",
+ "angle": f"{target}/angle",
+ "point_index": "point_index",
+ "approach_direction": "approach_direction",
+ "setpoint": "setpoint",
+ }
+
+ missing = [
+ column
+ for key, column in columns.items()
+ if key in {"update_count", "elapsed_s", "control_torque", "torque", "velocity", "angle"}
+ and column not in fieldnames
+ ]
+ if missing:
+ joined = ", ".join(missing)
+ raise ValueError(f"Missing required CSV columns: {joined}")
+ return columns
+
+
+def select_static_rows(
+ rows: list[dict[str, str]], columns: dict[str, str], velocity_threshold: float
+) -> list[dict[str, str]]:
+ if velocity_threshold < 0.0:
+ return rows
+
+ filtered_rows = []
+ for row in rows:
+ velocity = float(row[columns["velocity"]])
+ if abs(velocity) <= velocity_threshold:
+ filtered_rows.append(row)
+ return filtered_rows
+
+
+def has_bidirectional_columns(fieldnames: list[str], columns: dict[str, str]) -> bool:
+ return all(
+ columns[key] in fieldnames for key in ("point_index", "approach_direction", "setpoint")
+ )
+
+
+def fit_sine_model(samples: list[tuple[float, float]], signal_name: str) -> FitResult:
+ if len(samples) < 3:
+ raise ValueError("Need at least 3 samples to fit the sine model.")
+
+ s_s = 0.0
+ s_c = 0.0
+ c_c = 0.0
+ s_y = 0.0
+ c_y = 0.0
+ angles: list[float] = []
+ values: list[float] = []
+
+ for angle, value in samples:
+ wrapped_angle = wrap_to_pi(angle)
+ sin_theta = math.sin(wrapped_angle)
+ cos_theta = math.cos(wrapped_angle)
+
+ s_s += sin_theta * sin_theta
+ s_c += sin_theta * cos_theta
+ c_c += cos_theta * cos_theta
+ s_y += sin_theta * value
+ c_y += cos_theta * value
+
+ angles.append(wrapped_angle)
+ values.append(value)
+
+ determinant = s_s * c_c - s_c * s_c
+ if abs(determinant) < 1e-12:
+ raise ValueError(
+ "The regression design matrix is singular. The angle coverage is likely too narrow."
+ )
+
+ sin_gain = (s_y * c_c - c_y * s_c) / determinant
+ cos_gain = (s_s * c_y - s_c * s_y) / determinant
+
+ gain = math.hypot(sin_gain, cos_gain)
+ phase = math.atan2(cos_gain, sin_gain)
+
+ predictions = [gain * math.sin(angle + phase) for angle in angles]
+ residuals = [value - prediction for value, prediction in zip(values, predictions)]
+
+ mean_value = sum(values) / len(values)
+ ss_res = sum(residual * residual for residual in residuals)
+ ss_tot = sum((value - mean_value) ** 2 for value in values)
+ rmse = math.sqrt(ss_res / len(values))
+ mae = sum(abs(residual) for residual in residuals) / len(values)
+ r2 = float("nan") if ss_tot <= 0.0 else 1.0 - ss_res / ss_tot
+
+ trace = s_s + c_c
+ disc = math.sqrt(max(0.0, (s_s - c_c) ** 2 + 4.0 * s_c * s_c))
+ lambda_max = 0.5 * (trace + disc)
+ lambda_min = 0.5 * (trace - disc)
+ condition = float("inf") if lambda_min <= 0.0 else lambda_max / lambda_min
+
+ angle_min = min(angles)
+ angle_max = max(angles)
+
+ zero_crossing_angle = wrap_to_pi(-phase)
+ peak_angle = wrap_to_pi(math.pi * 0.5 - phase)
+
+ return FitResult(
+ signal_name=signal_name,
+ sample_count=len(values),
+ angle_min=angle_min,
+ angle_max=angle_max,
+ angle_span=angle_max - angle_min,
+ gain=gain,
+ phase=phase,
+ phase_deg=math.degrees(phase),
+ zero_crossing_angle=zero_crossing_angle,
+ zero_crossing_angle_deg=math.degrees(zero_crossing_angle),
+ peak_angle=peak_angle,
+ peak_angle_deg=math.degrees(peak_angle),
+ rmse=rmse,
+ mae=mae,
+ r2=r2,
+ design_condition=condition,
+ )
+
+
+def pair_bidirectional_rows(
+ rows: list[dict[str, str]], columns: dict[str, str]
+) -> tuple[list[AveragedSample], PairingSummary]:
+ grouped_rows: dict[int, dict[int, dict[str, str]]] = {}
+
+ for row in rows:
+ approach_direction = int(row[columns["approach_direction"]])
+ if approach_direction not in (-1, +1):
+ continue
+
+ point_index = int(row[columns["point_index"]])
+ grouped_rows.setdefault(point_index, {})[approach_direction] = row
+
+ averaged_samples: list[AveragedSample] = []
+ control_friction_values: list[float] = []
+ torque_friction_values: list[float] = []
+
+ for point_index in sorted(grouped_rows):
+ paired_rows = grouped_rows[point_index]
+ if -1 not in paired_rows or +1 not in paired_rows:
+ continue
+
+ negative_row = paired_rows[-1]
+ positive_row = paired_rows[+1]
+
+ angle = circular_mean(
+ [
+ float(negative_row[columns["angle"]]),
+ float(positive_row[columns["angle"]]),
+ ]
+ )
+ setpoint = circular_mean(
+ [
+ float(negative_row[columns["setpoint"]]),
+ float(positive_row[columns["setpoint"]]),
+ ]
+ )
+
+ control_negative = float(negative_row[columns["control_torque"]])
+ control_positive = float(positive_row[columns["control_torque"]])
+ torque_negative = float(negative_row[columns["torque"]])
+ torque_positive = float(positive_row[columns["torque"]])
+
+ control_friction = 0.5 * (control_positive - control_negative)
+ torque_friction = 0.5 * (torque_positive - torque_negative)
+
+ averaged_samples.append(
+ AveragedSample(
+ point_index=point_index,
+ angle=angle,
+ setpoint=setpoint,
+ control_torque=0.5 * (control_positive + control_negative),
+ torque=0.5 * (torque_positive + torque_negative),
+ control_friction=control_friction,
+ torque_friction=torque_friction,
+ )
+ )
+ control_friction_values.append(abs(control_friction))
+ torque_friction_values.append(abs(torque_friction))
+
+ available_point_count = len(grouped_rows)
+ paired_point_count = len(averaged_samples)
+ dropped_point_count = available_point_count - paired_point_count
+
+ summary = PairingSummary(
+ available_point_count=available_point_count,
+ paired_point_count=paired_point_count,
+ dropped_point_count=dropped_point_count,
+ mean_control_friction=(
+ sum(control_friction_values) / len(control_friction_values)
+ if control_friction_values
+ else float("nan")
+ ),
+ mean_torque_friction=(
+ sum(torque_friction_values) / len(torque_friction_values)
+ if torque_friction_values
+ else float("nan")
+ ),
+ )
+ return averaged_samples, summary
+
+
+def print_fit(result: FitResult, label: str) -> None:
+ print(f"{label}:")
+ print(f" signal: {result.signal_name}")
+ print(f" samples: {result.sample_count}")
+ print(
+ " angle span: "
+ f"[{result.angle_min:.6f}, {result.angle_max:.6f}] rad "
+ f"({math.degrees(result.angle_span):.2f} deg)"
+ )
+ print(f" G: {result.gain:.6f}")
+ print(f" phi: {result.phase:.6f} rad ({result.phase_deg:.2f} deg)")
+ print(
+ " model: "
+ f"u_ff = {result.gain:.6f} * sin(theta {'+' if result.phase >= 0.0 else '-'} "
+ f"{abs(result.phase):.6f})"
+ )
+ print(
+ " zero crossing angle (-phi): "
+ f"{result.zero_crossing_angle:.6f} rad ({result.zero_crossing_angle_deg:.2f} deg)"
+ )
+ print(
+ " peak angle (pi/2 - phi): "
+ f"{result.peak_angle:.6f} rad ({result.peak_angle_deg:.2f} deg)"
+ )
+ print(f" RMSE: {result.rmse:.6f}")
+ print(f" MAE: {result.mae:.6f}")
+ print(f" R^2: {result.r2:.6f}")
+ print(f" design condition: {result.design_condition:.3f}")
+
+
+def print_warnings(result: FitResult) -> None:
+ warnings: list[str] = []
+ if math.degrees(result.angle_span) < 90.0:
+ warnings.append(
+ "angle coverage is below 90 deg, so phase identification may be weak or biased"
+ )
+ if result.design_condition > 10.0:
+ warnings.append(
+ "sin/cos regressors are ill-conditioned for this dataset, so phase is sensitive to noise"
+ )
+ if math.isfinite(result.r2) and result.r2 < 0.8:
+ warnings.append(
+ "R^2 is low; gravity may be mixed with friction, backlash, or insufficient settling"
+ )
+
+ if not warnings:
+ return
+
+ print("Warnings:")
+ for item in warnings:
+ print(f" - {item}")
+
+
+def raw_samples_from_rows(
+ rows: list[dict[str, str]], columns: dict[str, str], signal_key: str
+) -> list[tuple[float, float]]:
+ return [
+ (float(row[columns["angle"]]), float(row[columns[signal_key]]))
+ for row in rows
+ ]
+
+
+def averaged_samples_to_tuples(
+ rows: list[AveragedSample], signal_key: str
+) -> list[tuple[float, float]]:
+ return [(row.angle, getattr(row, signal_key)) for row in rows]
+
+
+def main() -> int:
+ args = parse_args()
+
+ rows, fieldnames = read_rows(args.csv_path)
+ target = args.target if args.target is not None else infer_target(fieldnames)
+ columns = require_columns(fieldnames, target)
+
+ static_rows = select_static_rows(rows, columns, args.velocity_threshold)
+ if not static_rows:
+ raise ValueError("No rows remain after velocity filtering.")
+
+ payload: dict[str, object] = {
+ "csv_path": str(args.csv_path),
+ "target": target,
+ "velocity_threshold": args.velocity_threshold,
+ "rows_total": len(rows),
+ "rows_used": len(static_rows),
+ }
+
+ print(f"CSV: {args.csv_path}")
+ print(f"Target: {target}")
+ if args.velocity_threshold >= 0.0:
+ print(f"Velocity filter: abs({columns['velocity']}) <= {args.velocity_threshold}")
+ else:
+ print("Velocity filter: disabled")
+ print(f"Rows used: {len(static_rows)} / {len(rows)}")
+
+ primary_signal_key = args.signal
+ reference_signal_key = "torque" if primary_signal_key == "control_torque" else "control_torque"
+
+ if has_bidirectional_columns(fieldnames, columns):
+ averaged_samples, pairing_summary = pair_bidirectional_rows(static_rows, columns)
+ if not averaged_samples:
+ raise ValueError(
+ "No bidirectional pairs were found. The CSV may be incomplete or the test did not reach both directions."
+ )
+
+ primary_fit = fit_sine_model(
+ averaged_samples_to_tuples(averaged_samples, primary_signal_key),
+ f"paired_avg:{columns[primary_signal_key]}",
+ )
+ reference_fit = fit_sine_model(
+ averaged_samples_to_tuples(averaged_samples, reference_signal_key),
+ f"paired_avg:{columns[reference_signal_key]}",
+ )
+
+ print("Mode: bidirectional pairing with friction cancellation")
+ print(
+ "Pairing: "
+ f"{pairing_summary.paired_point_count} paired / {pairing_summary.available_point_count} available points "
+ f"({pairing_summary.dropped_point_count} dropped)"
+ )
+ print(
+ "Mean half-difference friction: "
+ f"control={pairing_summary.mean_control_friction:.6f}, "
+ f"torque={pairing_summary.mean_torque_friction:.6f}"
+ )
+
+ payload["pairing"] = asdict(pairing_summary)
+ payload["mode"] = "bidirectional"
+ else:
+ primary_fit = fit_sine_model(
+ raw_samples_from_rows(static_rows, columns, primary_signal_key),
+ columns[primary_signal_key],
+ )
+ reference_fit = fit_sine_model(
+ raw_samples_from_rows(static_rows, columns, reference_signal_key),
+ columns[reference_signal_key],
+ )
+ print("Mode: legacy single-direction fit without friction cancellation")
+ payload["mode"] = "legacy_single_direction"
+
+ print_fit(primary_fit, "Primary fit")
+ print_fit(reference_fit, "Reference fit")
+ print_warnings(primary_fit)
+ print("Suggested params:")
+ print(f" gravity_gain: {primary_fit.gain:.6f}")
+ print(f" gravity_phase: {primary_fit.phase:.6f}")
+
+ payload["primary_fit"] = asdict(primary_fit)
+ payload["reference_fit"] = asdict(reference_fit)
+
+ if args.json_output is not None:
+ args.json_output.parent.mkdir(parents=True, exist_ok=True)
+ args.json_output.write_text(json.dumps(payload, indent=2) + "\n")
+
+ return 0
+
+
+if __name__ == "__main__":
+ try:
+ raise SystemExit(main())
+ except ValueError as error:
+ print(f"error: {error}", file=sys.stderr)
+ raise SystemExit(2)
diff --git a/.script/identification/fit_sweep_graybox.py b/.script/identification/fit_sweep_graybox.py
new file mode 100644
index 00000000..5ae87627
--- /dev/null
+++ b/.script/identification/fit_sweep_graybox.py
@@ -0,0 +1,969 @@
+#!/usr/bin/env python3
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+import math
+import sys
+from dataclasses import asdict, dataclass
+from pathlib import Path
+from typing import Iterable
+
+import numpy as np
+
+try:
+ from scipy.optimize import least_squares
+except ModuleNotFoundError:
+ least_squares = None
+
+
+@dataclass
+class OdeSegment:
+ dt: np.ndarray
+ angle: np.ndarray
+ velocity: np.ndarray
+ tau: np.ndarray
+
+
+@dataclass
+class GrayboxFitResult:
+ fit_mode: str
+ signal_name: str
+ residual_name: str
+ gravity_mode: str
+ sample_count: int
+ dt: float
+ window_length: int
+ poly_order: int
+ tanh_gain: float
+ angle_min: float
+ angle_max: float
+ angle_span: float
+ inertia: float
+ viscous_damping: float
+ coulomb_friction: float
+ gravity_gain: float
+ gravity_phase: float
+ gravity_phase_deg: float
+ gravity_zero_crossing: float
+ gravity_zero_crossing_deg: float
+ torque_bias: float
+ rmse: float
+ mae: float
+ r2: float
+ design_condition: float
+ shooting_window: float
+ segment_count: int
+ optimizer_nfev: int
+ optimizer_status: int
+ optimizer_success: bool
+ optimizer_message: str
+
+
+def parse_args() -> argparse.Namespace:
+ parser = argparse.ArgumentParser(
+ description=(
+ "Fit the gray-box sweep model "
+ "J * theta_dd + B * theta_d + Fc * tanh(k * theta_d) + G * sin(theta + phi) = tau_ff"
+ )
+ )
+ parser.add_argument("csv_path", type=Path, help="Path to swept_frequency_controller CSV")
+ parser.add_argument(
+ "--target",
+ type=str,
+ default=None,
+ help="Target prefix such as /gimbal/pitch. If omitted, infer from CSV headers.",
+ )
+ parser.add_argument(
+ "--input-signal",
+ choices=("control_torque", "torque"),
+ default="torque",
+ help="Signal used as tau_ff. Defaults to torque.",
+ )
+ parser.add_argument(
+ "--fit-mode",
+ choices=("linear", "ode-angle"),
+ default="linear",
+ help="linear uses differentiated-velocity regression; ode-angle integrates the ODE and fits angle only.",
+ )
+ parser.add_argument(
+ "--window-length",
+ type=int,
+ default=31,
+ help="Odd Savitzky-Golay window length for smoothing velocity. Defaults to 31.",
+ )
+ parser.add_argument(
+ "--poly-order",
+ type=int,
+ default=3,
+ help="Savitzky-Golay polynomial order for velocity smoothing and linear-mode differentiation.",
+ )
+ parser.add_argument(
+ "--tanh-gain",
+ type=float,
+ default=100.0,
+ help="Gain k used in Fc * tanh(k * theta_d). Defaults to 100.",
+ )
+ parser.add_argument(
+ "--trim-start",
+ type=float,
+ default=0.0,
+ help="Discard samples before this elapsed time in seconds.",
+ )
+ parser.add_argument(
+ "--trim-end",
+ type=float,
+ default=0.0,
+ help="Discard samples within this many seconds from the end of the file.",
+ )
+ parser.add_argument(
+ "--shooting-window",
+ type=float,
+ default=0.5,
+ help="Segment duration in seconds for ode-angle multiple shooting. Defaults to 0.5.",
+ )
+ parser.add_argument(
+ "--ode-max-nfev",
+ type=int,
+ default=200,
+ help="Maximum least-squares function evaluations for ode-angle mode. Defaults to 200.",
+ )
+ parser.add_argument(
+ "--fixed-gravity-gain",
+ type=float,
+ default=None,
+ help="If set together with --fixed-gravity-phase, hold gravity gain G fixed.",
+ )
+ parser.add_argument(
+ "--fixed-gravity-phase",
+ type=float,
+ default=None,
+ help="If set together with --fixed-gravity-gain, hold gravity phase phi fixed in radians.",
+ )
+ parser.add_argument(
+ "--json-output",
+ type=Path,
+ default=None,
+ help="Optional path to write fit summary as JSON.",
+ )
+ return parser.parse_args()
+
+
+def infer_target(fieldnames: Iterable[str]) -> str:
+ angle_targets = {name[: -len("/angle")] for name in fieldnames if name.endswith("/angle")}
+ if len(angle_targets) != 1:
+ raise ValueError(
+ "Failed to infer target from CSV headers. Please pass --target explicitly."
+ )
+ return next(iter(angle_targets))
+
+
+def read_rows(path: Path) -> tuple[list[dict[str, str]], list[str]]:
+ with path.open(newline="") as csv_file:
+ reader = csv.DictReader(csv_file)
+ rows = list(reader)
+ if reader.fieldnames is None:
+ raise ValueError(f"CSV file has no header: {path}")
+ return rows, reader.fieldnames
+
+
+def require_columns(fieldnames: list[str], target: str) -> dict[str, str]:
+ columns = {
+ "elapsed_s": "elapsed_s",
+ "control_torque": f"{target}/control_torque",
+ "torque": f"{target}/torque",
+ "velocity": f"{target}/velocity",
+ "angle": f"{target}/angle",
+ }
+
+ missing = [column for column in columns.values() if column not in fieldnames]
+ if missing:
+ joined = ", ".join(missing)
+ raise ValueError(f"Missing required CSV columns: {joined}")
+ return columns
+
+
+def validate_args(args: argparse.Namespace) -> None:
+ if args.fit_mode == "ode-angle" and least_squares is None:
+ raise ValueError(
+ "ode-angle mode requires scipy. Install it in .venv or run with a Python environment that has scipy."
+ )
+ if args.window_length < 5 or args.window_length % 2 == 0:
+ raise ValueError("window-length must be an odd integer >= 5")
+ if args.poly_order < 2:
+ raise ValueError("poly-order must be >= 2")
+ if args.poly_order >= args.window_length:
+ raise ValueError("poly-order must be smaller than window-length")
+ if args.tanh_gain <= 0.0 or not math.isfinite(args.tanh_gain):
+ raise ValueError("tanh-gain must be finite and positive")
+ if args.trim_start < 0.0 or args.trim_end < 0.0:
+ raise ValueError("trim-start and trim-end must be non-negative")
+ if args.shooting_window <= 0.0 or not math.isfinite(args.shooting_window):
+ raise ValueError("shooting-window must be finite and positive")
+ if args.ode_max_nfev <= 0:
+ raise ValueError("ode-max-nfev must be positive")
+ fixed_gain_set = args.fixed_gravity_gain is not None
+ fixed_phase_set = args.fixed_gravity_phase is not None
+ if fixed_gain_set != fixed_phase_set:
+ raise ValueError(
+ "fixed-gravity-gain and fixed-gravity-phase must be provided together"
+ )
+ if fixed_gain_set and (
+ not math.isfinite(args.fixed_gravity_gain)
+ or not math.isfinite(args.fixed_gravity_phase)
+ or args.fixed_gravity_gain < 0.0
+ ):
+ raise ValueError(
+ "fixed gravity parameters must be finite and fixed-gravity-gain must be non-negative"
+ )
+
+
+def savitzky_golay(y: np.ndarray, dt: float, window_length: int, poly_order: int, deriv: int) -> np.ndarray:
+ half_window = window_length // 2
+ offsets = np.arange(-half_window, half_window + 1, dtype=float) * dt
+ vandermonde = np.vstack([offsets**order for order in range(poly_order + 1)]).T
+ coefficients = math.factorial(deriv) * np.linalg.pinv(vandermonde)[deriv]
+ windows = np.lib.stride_tricks.sliding_window_view(y, window_length)
+ return windows @ coefficients
+
+
+def compute_metrics(measured: np.ndarray, prediction: np.ndarray) -> tuple[float, float, float]:
+ residual = measured - prediction
+ centered = measured - float(np.mean(measured))
+ ss_res = float(residual @ residual)
+ ss_tot = float(centered @ centered)
+ rmse = math.sqrt(ss_res / measured.size)
+ mae = float(np.mean(np.abs(residual)))
+ r2 = float("nan") if ss_tot <= 0.0 else 1.0 - ss_res / ss_tot
+ return rmse, mae, r2
+
+
+def fit_linear_graybox(
+ angle: np.ndarray,
+ velocity: np.ndarray,
+ acceleration: np.ndarray,
+ tau_ff: np.ndarray,
+ signal_name: str,
+ dt: float,
+ window_length: int,
+ poly_order: int,
+ tanh_gain: float,
+) -> GrayboxFitResult:
+ regressors = np.column_stack(
+ [
+ acceleration,
+ velocity,
+ np.tanh(tanh_gain * velocity),
+ np.sin(angle),
+ np.cos(angle),
+ ]
+ )
+
+ coefficients, _, _, _ = np.linalg.lstsq(regressors, tau_ff, rcond=None)
+ prediction = regressors @ coefficients
+
+ inertia, viscous_damping, coulomb_friction, gravity_sin, gravity_cos = coefficients.tolist()
+ gravity_gain = math.hypot(gravity_sin, gravity_cos)
+ gravity_phase = math.atan2(gravity_cos, gravity_sin)
+
+ rmse, mae, r2 = compute_metrics(tau_ff, prediction)
+
+ return GrayboxFitResult(
+ fit_mode="linear",
+ signal_name=signal_name,
+ residual_name=signal_name,
+ gravity_mode="free",
+ sample_count=int(tau_ff.size),
+ dt=dt,
+ window_length=window_length,
+ poly_order=poly_order,
+ tanh_gain=tanh_gain,
+ angle_min=float(np.min(angle)),
+ angle_max=float(np.max(angle)),
+ angle_span=float(np.max(angle) - np.min(angle)),
+ inertia=float(inertia),
+ viscous_damping=float(viscous_damping),
+ coulomb_friction=float(coulomb_friction),
+ gravity_gain=float(gravity_gain),
+ gravity_phase=float(gravity_phase),
+ gravity_phase_deg=float(math.degrees(gravity_phase)),
+ gravity_zero_crossing=float(-gravity_phase),
+ gravity_zero_crossing_deg=float(math.degrees(-gravity_phase)),
+ torque_bias=0.0,
+ rmse=rmse,
+ mae=mae,
+ r2=r2,
+ design_condition=float(np.linalg.cond(regressors)),
+ shooting_window=float("nan"),
+ segment_count=1,
+ optimizer_nfev=0,
+ optimizer_status=0,
+ optimizer_success=True,
+ optimizer_message="linear least squares",
+ )
+
+
+def fit_fixed_gravity_graybox(
+ angle: np.ndarray,
+ velocity: np.ndarray,
+ acceleration: np.ndarray,
+ tau_ff: np.ndarray,
+ signal_name: str,
+ dt: float,
+ window_length: int,
+ poly_order: int,
+ tanh_gain: float,
+ gravity_gain: float,
+ gravity_phase: float,
+) -> GrayboxFitResult:
+ gravity_term = gravity_gain * np.sin(angle + gravity_phase)
+ regressors = np.column_stack(
+ [
+ acceleration,
+ velocity,
+ np.tanh(tanh_gain * velocity),
+ ]
+ )
+
+ coefficients, _, _, _ = np.linalg.lstsq(regressors, tau_ff - gravity_term, rcond=None)
+ prediction = regressors @ coefficients + gravity_term
+
+ inertia, viscous_damping, coulomb_friction = coefficients.tolist()
+
+ rmse, mae, r2 = compute_metrics(tau_ff, prediction)
+
+ return GrayboxFitResult(
+ fit_mode="linear",
+ signal_name=signal_name,
+ residual_name=signal_name,
+ gravity_mode="fixed",
+ sample_count=int(tau_ff.size),
+ dt=dt,
+ window_length=window_length,
+ poly_order=poly_order,
+ tanh_gain=tanh_gain,
+ angle_min=float(np.min(angle)),
+ angle_max=float(np.max(angle)),
+ angle_span=float(np.max(angle) - np.min(angle)),
+ inertia=float(inertia),
+ viscous_damping=float(viscous_damping),
+ coulomb_friction=float(coulomb_friction),
+ gravity_gain=float(gravity_gain),
+ gravity_phase=float(gravity_phase),
+ gravity_phase_deg=float(math.degrees(gravity_phase)),
+ gravity_zero_crossing=float(-gravity_phase),
+ gravity_zero_crossing_deg=float(math.degrees(-gravity_phase)),
+ torque_bias=0.0,
+ rmse=rmse,
+ mae=mae,
+ r2=r2,
+ design_condition=float(np.linalg.cond(regressors)),
+ shooting_window=float("nan"),
+ segment_count=1,
+ optimizer_nfev=0,
+ optimizer_status=0,
+ optimizer_success=True,
+ optimizer_message="linear least squares",
+ )
+
+
+def build_ode_segments(
+ elapsed: np.ndarray,
+ angle: np.ndarray,
+ velocity: np.ndarray,
+ tau_ff: np.ndarray,
+ shooting_window: float,
+ dt: float,
+) -> list[OdeSegment]:
+ segment_samples = max(2, int(round(shooting_window / dt)) + 1)
+ segments: list[OdeSegment] = []
+ start = 0
+ while start < angle.size - 1:
+ stop = min(start + segment_samples, angle.size)
+ if stop - start < 2:
+ break
+ elapsed_segment = elapsed[start:stop]
+ segments.append(
+ OdeSegment(
+ dt=np.diff(elapsed_segment),
+ angle=angle[start:stop],
+ velocity=velocity[start:stop],
+ tau=tau_ff[start:stop],
+ )
+ )
+ if stop == angle.size:
+ break
+ start = stop - 1
+ return segments
+
+
+def build_ode_initial_guess(
+ angle: np.ndarray,
+ velocity: np.ndarray,
+ tau_ff: np.ndarray,
+ tanh_gain: float,
+ fixed_gravity_gain: float | None,
+ fixed_gravity_phase: float | None,
+) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+ tanh_velocity = np.tanh(tanh_gain * velocity)
+ tau_scale = max(1.0, float(np.percentile(np.abs(tau_ff), 95)))
+ velocity_scale = max(1e-3, float(np.percentile(np.abs(velocity), 95)))
+ damping_upper = max(10.0, 50.0 * tau_scale / velocity_scale)
+ friction_upper = max(10.0, 10.0 * tau_scale)
+ gravity_upper = max(10.0, 10.0 * tau_scale)
+ bias_limit = max(10.0, 5.0 * tau_scale)
+
+ if fixed_gravity_gain is None:
+ regressors = np.column_stack(
+ [
+ velocity,
+ tanh_velocity,
+ np.sin(angle),
+ np.cos(angle),
+ np.ones_like(angle),
+ ]
+ )
+ coefficients, _, _, _ = np.linalg.lstsq(regressors, tau_ff, rcond=None)
+ damping0, friction0, gravity_sin0, gravity_cos0, bias0 = coefficients.tolist()
+ gravity_gain0 = math.hypot(gravity_sin0, gravity_cos0)
+ gravity_phase0 = math.atan2(gravity_cos0, gravity_sin0)
+ x0 = np.array(
+ [
+ 0.01,
+ abs(damping0),
+ abs(friction0),
+ max(1e-6, gravity_gain0),
+ gravity_phase0,
+ bias0,
+ ],
+ dtype=float,
+ )
+ lower = np.array([1e-6, 0.0, 0.0, 0.0, -4.0 * math.pi, -bias_limit], dtype=float)
+ upper = np.array(
+ [10.0, damping_upper, friction_upper, gravity_upper, 4.0 * math.pi, bias_limit],
+ dtype=float,
+ )
+ return np.clip(x0, lower, upper), lower, upper
+
+ gravity_term = fixed_gravity_gain * np.sin(angle + fixed_gravity_phase)
+ regressors = np.column_stack([velocity, tanh_velocity, np.ones_like(angle)])
+ coefficients, _, _, _ = np.linalg.lstsq(regressors, tau_ff - gravity_term, rcond=None)
+ damping0, friction0, bias0 = coefficients.tolist()
+ x0 = np.array([0.01, abs(damping0), abs(friction0), bias0], dtype=float)
+ lower = np.array([1e-6, 0.0, 0.0, -bias_limit], dtype=float)
+ upper = np.array([10.0, damping_upper, friction_upper, bias_limit], dtype=float)
+ return np.clip(x0, lower, upper), lower, upper
+
+
+def unpack_ode_parameters(
+ parameters: np.ndarray,
+ fixed_gravity_gain: float | None,
+ fixed_gravity_phase: float | None,
+) -> tuple[float, float, float, float, float, float]:
+ if fixed_gravity_gain is None:
+ inertia, viscous_damping, coulomb_friction, gravity_gain, gravity_phase, torque_bias = (
+ parameters.tolist()
+ )
+ return (
+ float(inertia),
+ float(viscous_damping),
+ float(coulomb_friction),
+ float(gravity_gain),
+ float(gravity_phase),
+ float(torque_bias),
+ )
+
+ inertia, viscous_damping, coulomb_friction, torque_bias = parameters.tolist()
+ return (
+ float(inertia),
+ float(viscous_damping),
+ float(coulomb_friction),
+ float(fixed_gravity_gain),
+ float(fixed_gravity_phase),
+ float(torque_bias),
+ )
+
+
+def simulate_segment_angle(
+ segment: OdeSegment,
+ inertia: float,
+ viscous_damping: float,
+ coulomb_friction: float,
+ gravity_gain: float,
+ gravity_phase: float,
+ tanh_gain: float,
+ torque_bias: float,
+) -> np.ndarray:
+ def angular_acceleration(theta: float, omega: float, tau: float) -> float:
+ return (
+ tau
+ + torque_bias
+ - viscous_damping * omega
+ - coulomb_friction * math.tanh(tanh_gain * omega)
+ - gravity_gain * math.sin(theta + gravity_phase)
+ ) / inertia
+
+ predicted = np.empty_like(segment.angle)
+ theta = float(segment.angle[0])
+ omega = float(segment.velocity[0])
+ predicted[0] = theta
+
+ for index, delta_t in enumerate(segment.dt):
+ tau0 = float(segment.tau[index])
+ tau1 = float(segment.tau[index + 1])
+ tau_mid = 0.5 * (tau0 + tau1)
+
+ k1_theta = omega
+ k1_omega = angular_acceleration(theta, omega, tau0)
+
+ theta_k2 = theta + 0.5 * delta_t * k1_theta
+ omega_k2 = omega + 0.5 * delta_t * k1_omega
+ k2_theta = omega_k2
+ k2_omega = angular_acceleration(theta_k2, omega_k2, tau_mid)
+
+ theta_k3 = theta + 0.5 * delta_t * k2_theta
+ omega_k3 = omega + 0.5 * delta_t * k2_omega
+ k3_theta = omega_k3
+ k3_omega = angular_acceleration(theta_k3, omega_k3, tau_mid)
+
+ theta_k4 = theta + delta_t * k3_theta
+ omega_k4 = omega + delta_t * k3_omega
+ k4_theta = omega_k4
+ k4_omega = angular_acceleration(theta_k4, omega_k4, tau1)
+
+ theta += (delta_t / 6.0) * (k1_theta + 2.0 * k2_theta + 2.0 * k3_theta + k4_theta)
+ omega += (delta_t / 6.0) * (k1_omega + 2.0 * k2_omega + 2.0 * k3_omega + k4_omega)
+ predicted[index + 1] = theta
+
+ return predicted
+
+
+def simulate_shooting_prediction(
+ segments: list[OdeSegment],
+ inertia: float,
+ viscous_damping: float,
+ coulomb_friction: float,
+ gravity_gain: float,
+ gravity_phase: float,
+ tanh_gain: float,
+ torque_bias: float,
+) -> tuple[np.ndarray, np.ndarray]:
+ measured_parts: list[np.ndarray] = []
+ predicted_parts: list[np.ndarray] = []
+ for segment in segments:
+ predicted = simulate_segment_angle(
+ segment,
+ inertia,
+ viscous_damping,
+ coulomb_friction,
+ gravity_gain,
+ gravity_phase,
+ tanh_gain,
+ torque_bias,
+ )
+ measured_parts.append(segment.angle[1:])
+ predicted_parts.append(predicted[1:])
+ return np.concatenate(measured_parts), np.concatenate(predicted_parts)
+
+
+def fit_ode_angle_graybox(
+ elapsed: np.ndarray,
+ angle: np.ndarray,
+ velocity: np.ndarray,
+ tau_ff: np.ndarray,
+ signal_name: str,
+ dt: float,
+ window_length: int,
+ poly_order: int,
+ tanh_gain: float,
+ shooting_window: float,
+ ode_max_nfev: int,
+ fixed_gravity_gain: float | None,
+ fixed_gravity_phase: float | None,
+) -> GrayboxFitResult:
+ segments = build_ode_segments(elapsed, angle, velocity, tau_ff, shooting_window, dt)
+ if not segments:
+ raise ValueError("Too few samples remain to build ODE shooting segments.")
+
+ x0, lower, upper = build_ode_initial_guess(
+ angle, velocity, tau_ff, tanh_gain, fixed_gravity_gain, fixed_gravity_phase
+ )
+
+ def residual_function(parameters: np.ndarray) -> np.ndarray:
+ (
+ inertia,
+ viscous_damping,
+ coulomb_friction,
+ gravity_gain,
+ gravity_phase,
+ torque_bias,
+ ) = unpack_ode_parameters(parameters, fixed_gravity_gain, fixed_gravity_phase)
+ measured, predicted = simulate_shooting_prediction(
+ segments,
+ inertia,
+ viscous_damping,
+ coulomb_friction,
+ gravity_gain,
+ gravity_phase,
+ tanh_gain,
+ torque_bias,
+ )
+ return predicted - measured
+
+ optimization = least_squares(
+ residual_function,
+ x0,
+ bounds=(lower, upper),
+ method="trf",
+ x_scale=np.maximum(np.abs(x0), 1e-3),
+ max_nfev=ode_max_nfev,
+ )
+
+ (
+ inertia,
+ viscous_damping,
+ coulomb_friction,
+ gravity_gain,
+ gravity_phase,
+ torque_bias,
+ ) = unpack_ode_parameters(
+ optimization.x, fixed_gravity_gain, fixed_gravity_phase
+ )
+ measured_angle, predicted_angle = simulate_shooting_prediction(
+ segments,
+ inertia,
+ viscous_damping,
+ coulomb_friction,
+ gravity_gain,
+ gravity_phase,
+ tanh_gain,
+ torque_bias,
+ )
+ rmse, mae, r2 = compute_metrics(measured_angle, predicted_angle)
+
+ return GrayboxFitResult(
+ fit_mode="ode-angle",
+ signal_name=signal_name,
+ residual_name="angle",
+ gravity_mode="fixed" if fixed_gravity_gain is not None else "free",
+ sample_count=int(angle.size),
+ dt=dt,
+ window_length=window_length,
+ poly_order=poly_order,
+ tanh_gain=tanh_gain,
+ angle_min=float(np.min(angle)),
+ angle_max=float(np.max(angle)),
+ angle_span=float(np.max(angle) - np.min(angle)),
+ inertia=float(inertia),
+ viscous_damping=float(viscous_damping),
+ coulomb_friction=float(coulomb_friction),
+ gravity_gain=float(gravity_gain),
+ gravity_phase=float(gravity_phase),
+ gravity_phase_deg=float(math.degrees(gravity_phase)),
+ gravity_zero_crossing=float(-gravity_phase),
+ gravity_zero_crossing_deg=float(math.degrees(-gravity_phase)),
+ torque_bias=float(torque_bias),
+ rmse=rmse,
+ mae=mae,
+ r2=r2,
+ design_condition=float("nan"),
+ shooting_window=shooting_window,
+ segment_count=len(segments),
+ optimizer_nfev=int(optimization.nfev),
+ optimizer_status=int(optimization.status),
+ optimizer_success=bool(optimization.success),
+ optimizer_message=str(optimization.message),
+ )
+
+
+def print_fit(result: GrayboxFitResult, label: str) -> None:
+ print(f"{label}:")
+ print(f" fit mode: {result.fit_mode}")
+ print(f" signal: {result.signal_name}")
+ print(f" residual: {result.residual_name}")
+ print(f" gravity mode: {result.gravity_mode}")
+ print(f" samples: {result.sample_count}")
+ print(
+ " angle span: "
+ f"[{result.angle_min:.6f}, {result.angle_max:.6f}] rad "
+ f"({math.degrees(result.angle_span):.2f} deg)"
+ )
+ print(
+ " model: "
+ f"{result.inertia:.6f} * theta_dd + {result.viscous_damping:.6f} * theta_d "
+ f"+ {result.coulomb_friction:.6f} * tanh({result.tanh_gain:.1f} * theta_d) "
+ f"+ {result.gravity_gain:.6f} * sin(theta {'+' if result.gravity_phase >= 0.0 else '-'} "
+ f"{abs(result.gravity_phase):.6f}) = tau_ff"
+ )
+ print(f" J: {result.inertia:.6f}")
+ print(f" B: {result.viscous_damping:.6f}")
+ print(f" Fc: {result.coulomb_friction:.6f}")
+ print(f" G: {result.gravity_gain:.6f}")
+ print(
+ f" phi: {result.gravity_phase:.6f} rad ({result.gravity_phase_deg:.2f} deg)"
+ )
+ print(
+ " gravity zero crossing (-phi): "
+ f"{result.gravity_zero_crossing:.6f} rad "
+ f"({result.gravity_zero_crossing_deg:.2f} deg)"
+ )
+ if result.fit_mode == "ode-angle":
+ print(
+ f" shooting window: {result.shooting_window:.3f} s "
+ f"({result.segment_count} segments)"
+ )
+ print(f" torque bias: {result.torque_bias:.6f}")
+ print(
+ " optimizer: "
+ f"success={result.optimizer_success}, "
+ f"status={result.optimizer_status}, "
+ f"nfev={result.optimizer_nfev}"
+ )
+ print(f" Angle RMSE: {result.rmse:.6f}")
+ print(f" Angle MAE: {result.mae:.6f}")
+ else:
+ print(f" RMSE: {result.rmse:.6f}")
+ print(f" MAE: {result.mae:.6f}")
+ print(f" R^2: {result.r2:.6f}")
+ if math.isfinite(result.design_condition):
+ print(f" design condition: {result.design_condition:.3f}")
+
+
+def print_warnings(result: GrayboxFitResult) -> None:
+ warnings: list[str] = []
+ if result.inertia <= 0.0:
+ warnings.append("identified inertia J is non-positive")
+ if result.viscous_damping < 0.0:
+ warnings.append("identified viscous damping B is negative")
+ if result.coulomb_friction < 0.0:
+ warnings.append("identified Coulomb friction Fc is negative")
+ if math.degrees(result.angle_span) < 90.0:
+ warnings.append("angle coverage is below 90 deg, so gravity phase may be weakly observable")
+ if result.fit_mode == "linear":
+ if result.design_condition > 100.0:
+ warnings.append("regression matrix is ill-conditioned; parameters may be sensitive to noise")
+ if math.isfinite(result.r2) and result.r2 < 0.7:
+ warnings.append("R^2 is modest; derivative noise or model mismatch may still dominate")
+ else:
+ if not result.optimizer_success:
+ warnings.append(f"optimizer did not report success: {result.optimizer_message}")
+ if abs(result.torque_bias) > max(0.1, 0.2 * result.gravity_gain):
+ warnings.append("identified torque bias is large; input signal may have a non-zero offset")
+ if math.isfinite(result.r2) and result.r2 < 0.7:
+ warnings.append("angle-only fit is modest; excitation or gravity observability may still be weak")
+
+ if not warnings:
+ return
+
+ print("Warnings:")
+ for item in warnings:
+ print(f" - {item}")
+
+
+def main() -> int:
+ args = parse_args()
+ validate_args(args)
+
+ rows, fieldnames = read_rows(args.csv_path)
+ if not rows:
+ raise ValueError("CSV file is empty.")
+
+ target = args.target if args.target is not None else infer_target(fieldnames)
+ columns = require_columns(fieldnames, target)
+
+ elapsed = np.array([float(row[columns["elapsed_s"]]) for row in rows], dtype=float)
+ angle = np.array([float(row[columns["angle"]]) for row in rows], dtype=float)
+ velocity = np.array([float(row[columns["velocity"]]) for row in rows], dtype=float)
+ control_torque = np.array([float(row[columns["control_torque"]]) for row in rows], dtype=float)
+ measured_torque = np.array([float(row[columns["torque"]]) for row in rows], dtype=float)
+
+ if elapsed.size <= args.window_length:
+ raise ValueError("Too few samples for the requested Savitzky-Golay window length.")
+
+ dt = float(np.median(np.diff(elapsed)))
+ if not math.isfinite(dt) or dt <= 0.0:
+ raise ValueError("Failed to determine a valid time step from elapsed_s.")
+
+ half_window = args.window_length // 2
+ velocity_smooth = savitzky_golay(
+ velocity, dt, args.window_length, args.poly_order, deriv=0
+ )
+
+ center_slice = slice(half_window, elapsed.size - half_window)
+ elapsed_valid = elapsed[center_slice]
+ angle_valid = np.unwrap(angle[center_slice])
+ control_torque_valid = control_torque[center_slice]
+ measured_torque_valid = measured_torque[center_slice]
+
+ mask = np.ones_like(elapsed_valid, dtype=bool)
+ if args.trim_start > 0.0:
+ mask &= elapsed_valid >= args.trim_start
+ if args.trim_end > 0.0:
+ mask &= elapsed_valid <= elapsed_valid[-1] - args.trim_end
+
+ if int(np.count_nonzero(mask)) < 10:
+ raise ValueError("Too few samples remain after trimming.")
+
+ elapsed_fit = elapsed_valid[mask]
+ angle_fit = angle_valid[mask]
+ velocity_fit = velocity_smooth[mask]
+ control_torque_fit = control_torque_valid[mask]
+ measured_torque_fit = measured_torque_valid[mask]
+
+ primary_signal = (
+ control_torque_fit if args.input_signal == "control_torque" else measured_torque_fit
+ )
+ primary_signal_name = columns[args.input_signal]
+ reference_signal = (
+ measured_torque_fit if args.input_signal == "control_torque" else control_torque_fit
+ )
+ reference_signal_name = (
+ columns["torque"] if args.input_signal == "control_torque" else columns["control_torque"]
+ )
+
+ if args.fit_mode == "linear":
+ acceleration = savitzky_golay(
+ velocity, dt, args.window_length, args.poly_order, deriv=1
+ )
+ acceleration_fit = acceleration[mask]
+ fit_function = fit_linear_graybox
+ fit_kwargs: dict[str, float] = {}
+ if args.fixed_gravity_gain is not None:
+ fit_function = fit_fixed_gravity_graybox
+ fit_kwargs = {
+ "gravity_gain": args.fixed_gravity_gain,
+ "gravity_phase": args.fixed_gravity_phase,
+ }
+
+ primary_fit = fit_function(
+ angle_fit,
+ velocity_fit,
+ acceleration_fit,
+ primary_signal,
+ primary_signal_name,
+ dt,
+ args.window_length,
+ args.poly_order,
+ args.tanh_gain,
+ **fit_kwargs,
+ )
+ reference_fit = fit_function(
+ angle_fit,
+ velocity_fit,
+ acceleration_fit,
+ reference_signal,
+ reference_signal_name,
+ dt,
+ args.window_length,
+ args.poly_order,
+ args.tanh_gain,
+ **fit_kwargs,
+ )
+ else:
+ primary_fit = fit_ode_angle_graybox(
+ elapsed_fit,
+ angle_fit,
+ velocity_fit,
+ primary_signal,
+ primary_signal_name,
+ dt,
+ args.window_length,
+ args.poly_order,
+ args.tanh_gain,
+ args.shooting_window,
+ args.ode_max_nfev,
+ args.fixed_gravity_gain,
+ args.fixed_gravity_phase,
+ )
+ reference_fit = fit_ode_angle_graybox(
+ elapsed_fit,
+ angle_fit,
+ velocity_fit,
+ reference_signal,
+ reference_signal_name,
+ dt,
+ args.window_length,
+ args.poly_order,
+ args.tanh_gain,
+ args.shooting_window,
+ args.ode_max_nfev,
+ args.fixed_gravity_gain,
+ args.fixed_gravity_phase,
+ )
+
+ print(f"CSV: {args.csv_path}")
+ print(f"Target: {target}")
+ print(f"Rows loaded: {len(rows)}")
+ print(f"Rows used: {primary_fit.sample_count}")
+ print(f"Median dt: {dt:.6f} s")
+ print(f"Fit mode: {args.fit_mode}")
+ if args.fit_mode == "linear":
+ print(
+ f"Savitzky-Golay: window_length={args.window_length}, poly_order={args.poly_order}, "
+ f"source={columns['velocity']}"
+ )
+ else:
+ print(
+ "Velocity smoothing: "
+ f"window_length={args.window_length}, poly_order={args.poly_order}, "
+ f"source={columns['velocity']}"
+ )
+ print(
+ "ODE multiple shooting: "
+ f"window={args.shooting_window:.3f} s, max_nfev={args.ode_max_nfev}"
+ )
+ if args.fixed_gravity_gain is not None:
+ print(
+ "Fixed gravity: "
+ f"G={args.fixed_gravity_gain:.6f}, phi={args.fixed_gravity_phase:.6f} rad"
+ )
+ if args.trim_start > 0.0 or args.trim_end > 0.0:
+ print(f"Trim: start={args.trim_start:.3f} s, end={args.trim_end:.3f} s")
+ print_fit(primary_fit, "Primary fit")
+ print_fit(reference_fit, "Reference fit")
+ print_warnings(primary_fit)
+ print("Suggested params:")
+ print(f" inertia: {primary_fit.inertia:.6f}")
+ print(f" viscous_damping: {primary_fit.viscous_damping:.6f}")
+ print(f" coulomb_friction: {primary_fit.coulomb_friction:.6f}")
+ print(f" gravity_gain: {primary_fit.gravity_gain:.6f}")
+ print(f" gravity_phase: {primary_fit.gravity_phase:.6f}")
+ if args.fit_mode == "ode-angle":
+ print("Nuisance params:")
+ print(f" torque_bias: {primary_fit.torque_bias:.6f}")
+
+ if args.json_output is not None:
+ args.json_output.parent.mkdir(parents=True, exist_ok=True)
+ payload = {
+ "csv_path": str(args.csv_path),
+ "target": target,
+ "input_signal": args.input_signal,
+ "fit_mode": args.fit_mode,
+ "rows_total": len(rows),
+ "rows_used": primary_fit.sample_count,
+ "dt": dt,
+ "window_length": args.window_length,
+ "poly_order": args.poly_order,
+ "tanh_gain": args.tanh_gain,
+ "trim_start": args.trim_start,
+ "trim_end": args.trim_end,
+ "shooting_window": args.shooting_window,
+ "ode_max_nfev": args.ode_max_nfev,
+ "fixed_gravity_gain": args.fixed_gravity_gain,
+ "fixed_gravity_phase": args.fixed_gravity_phase,
+ "primary_fit": asdict(primary_fit),
+ "reference_fit": asdict(reference_fit),
+ }
+ args.json_output.write_text(json.dumps(payload, indent=2) + "\n")
+
+ return 0
+
+
+if __name__ == "__main__":
+ try:
+ raise SystemExit(main())
+ except ValueError as error:
+ print(f"error: {error}", file=sys.stderr)
+ raise SystemExit(2)
diff --git a/librmcs-firmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu b/librmcs-firmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu
new file mode 100644
index 00000000..c3e952e1
Binary files /dev/null and b/librmcs-firmware-c_board-3.0.1-0.dev.4.gbaf538b.dfu differ
diff --git a/librmcs-firmware-c_board-3.1.0.dfu b/librmcs-firmware-c_board-3.1.0.dfu
new file mode 100644
index 00000000..53e26b8e
Binary files /dev/null and b/librmcs-firmware-c_board-3.1.0.dfu differ
diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera
new file mode 160000
index 00000000..212e11be
--- /dev/null
+++ b/rmcs_ws/src/hikcamera
@@ -0,0 +1 @@
+Subproject commit 212e11bea22fd54b2ed4f2a97417ff14fb8c01d3
diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml
new file mode 100644
index 00000000..8fa55078
--- /dev/null
+++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml
@@ -0,0 +1,355 @@
+rmcs_executor:
+ ros__parameters:
+ update_rate: 1000.0
+ components:
+ - rmcs_core::hardware::SteeringHeroLittle -> hero_hardware
+
+ - rmcs_core::referee::Status -> referee_status
+ - rmcs_core::referee::command::Interaction -> referee_interaction
+ - rmcs_core::referee::command::interaction::Ui -> referee_ui
+ - rmcs_core::referee::app::ui::Hero -> referee_ui_hero
+ - rmcs_core::referee::Command -> referee_command
+
+ - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller
+ - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller
+ - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller
+ - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller
+
+ # - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller
+ # - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller
+
+ - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller
+ - rmcs_core::controller::shooting::HeatController -> heat_controller
+ - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller
+ - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller
+ - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller
+ - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller
+ - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller
+ - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder
+
+ - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status
+ - rmcs_core::controller::chassis::ChassisController -> chassis_controller
+ - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller
+ - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller
+ - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller
+
+ # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer
+ # - rmcs_auto_aim::AutoAimController -> auto_aim_controller
+
+ - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster
+ - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster
+
+ - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge
+
+ # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller
+ # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller
+ # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller
+ # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller
+
+
+
+
+hero_hardware:
+ ros__parameters:
+ board_serial_top_board: "AF-BFF7-0B6F-46A5-2B4B-AA20-89C2-E180-64B9"
+ serial_bottom_rmcs_board: "AF-60BB-7484-FA24-F3FC-399B-454D-22FA-1B1D"
+ bottom_yaw_motor_zero_point: 35833
+ pitch_motor_zero_point: 47885
+ top_yaw_motor_zero_point: 49457
+ viewer_motor_zero_point: 3030
+ external_imu_port: /dev/ttyUSB0
+ bullet_feeder_motor_zero_point: 41415
+ left_front_zero_point: 5814
+ right_front_zero_point: 3806
+ left_back_zero_point: 7825
+ right_back_zero_point: 1767
+ # left_front_zero_point: 3473
+ # left_back_zero_point: 6124
+ # right_back_zero_point: 6157
+ # right_front_zero_point: 2723
+
+
+
+
+value_broadcaster:
+ ros__parameters:
+ forward_list:
+ - /chassis/left_front_steering/angle
+ - /chassis/left_front_steering/velocity
+ - /chassis/left_front_steering/control_torque
+ - /chassis/left_front_steering/torque
+ - /chassis/right_front_steering/angle
+ - /chassis/right_front_steering/velocity
+ - /chassis/right_front_steering/control_torque
+ - /chassis/right_front_steering/torque
+ - /chassis/left_back_steering/angle
+ - /chassis/left_back_steering/velocity
+ - /chassis/left_back_steering/control_torque
+ - /chassis/left_back_steering/torque
+ - /chassis/right_back_steering/angle
+ - /chassis/right_back_steering/velocity
+ - /chassis/right_back_steering/control_torque
+ - /chassis/right_back_steering/torque
+
+
+climber_controller:
+ ros__parameters:
+ front_climber_velocity: 60.0
+ back_climber_velocity: 20.0
+ auto_climb_dash_velocity: 3.0
+ auto_climb_support_retract_velocity: 30.0
+ sync_coefficient: 0.3
+ first_stair_approach_pitch: 0.458
+ second_stair_approach_pitch: 0.354
+ front_kp: 1.0
+ front_ki: 0.0
+ front_kd: 0.5
+ back_kp: 1.0
+ back_ki: 0.0
+ back_kd: 0.0
+
+gimbal_controller:
+ ros__parameters:
+ upper_limit: -0.753376
+ lower_limit: 0.257038
+
+dual_yaw_controller:
+ ros__parameters:
+ top_yaw_angle_kp: 22.2
+ top_yaw_angle_ki: 0.0
+ top_yaw_angle_kd: 0.0
+ top_yaw_velocity_kp: 8.92
+ top_yaw_velocity_ki: 0.0026
+ top_yaw_velocity_kd: 0.0
+ top_yaw_velocity_integral_min: -2500.0
+ top_yaw_velocity_integral_max: 2500.0
+ bottom_yaw_angle_kp: 13.9
+ bottom_yaw_angle_ki: 0.0
+ bottom_yaw_angle_kd: 0.0
+ bottom_yaw_velocity_kp: 3.0
+ bottom_yaw_velocity_ki: 0.0006
+ bottom_yaw_velocity_kd: 0.0
+ bottom_yaw_velocity_integral_min: -2500.0
+ bottom_yaw_velocity_integral_max: 2500.0
+
+# dual_yaw_controller:
+# ros__parameters:
+# top_yaw_angle_kp: 24.5
+# top_yaw_angle_ki: 0.0
+# top_yaw_angle_kd: 0.0
+# top_yaw_velocity_kp: 77.4
+# top_yaw_velocity_ki: 0.004
+# top_yaw_velocity_kd: 1.0
+# bottom_yaw_angle_kp: 8.6
+# bottom_yaw_angle_ki: 0.0
+# bottom_yaw_angle_kd: 0.0
+# bottom_yaw_velocity_kp: 25.85
+# bottom_yaw_velocity_ki: 0.0
+# bottom_yaw_velocity_kd: 50.0
+
+pitch_angle_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/pitch/control_angle_error
+ control: /gimbal/pitch/control_velocity
+ kp: 25.6
+ ki: 0.0
+ kd: 0.0
+
+pitch_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/pitch/velocity_imu
+ setpoint: /gimbal/pitch/control_velocity
+ control: /gimbal/pitch/control_torque
+ kp: 14.11
+ ki: 0.0040
+ kd: 0.0
+ integral_min: -2500.0
+ integral_max: 2500.0
+
+gimbal_player_viewer_controller0:
+ ros__parameters:
+ upper_limit: 0.0101
+ lower_limit: 0.6191
+
+viewer_angle_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/player_viewer/control_angle_error
+ control: /gimbal/player_viewer/control_velocity
+ kp: 17.00
+ ki: 0.00
+ kd: 2.00
+
+friction_wheel_controller:
+ ros__parameters:
+ friction_wheels:
+ - /gimbal/first_left_friction
+ - /gimbal/first_right_friction
+ - /gimbal/second_left_friction
+ - /gimbal/second_right_friction
+ friction_velocities:
+ - 368.00
+ - 368.00
+ - 532.00
+ - 532.00
+ friction_soft_start_stop_time: 1.0
+
+heat_controller:
+ ros__parameters:
+ heat_per_shot: 100000
+ reserved_heat: 0
+
+shooting_recorder:
+ ros__parameters:
+ friction_wheel_count: 4
+ aim_velocity: 11.8
+ log_mode: 1 # 1: trigger, 2: timing
+
+first_left_friction_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/first_left_friction/velocity
+ setpoint: /gimbal/first_left_friction/control_velocity
+ control: /gimbal/first_left_friction/control_torque
+ kp: 0.0008
+ ki: 0.00
+ kd: 0.000055
+
+first_right_friction_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/first_right_friction/velocity
+ setpoint: /gimbal/first_right_friction/control_velocity
+ control: /gimbal/first_right_friction/control_torque
+ kp: 0.0008
+ ki: 0.00
+ kd: 0.000055
+
+second_left_friction_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/second_left_friction/velocity
+ setpoint: /gimbal/second_left_friction/control_velocity
+ control: /gimbal/second_left_friction/control_torque
+ kp: 0.0005
+ ki: 0.00
+ kd: 0.00004
+
+second_right_friction_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/second_right_friction/velocity
+ setpoint: /gimbal/second_right_friction/control_velocity
+ control: /gimbal/second_right_friction/control_torque
+ kp: 0.0005
+ ki: 0.00
+ kd: 0.00004
+
+steering_wheel_status:
+ ros__parameters:
+ vehicle_radius: 0.286378
+ wheel_radius: 0.055
+
+steering_wheel_controller:
+ ros__parameters:
+ mess: 22.0
+ moment_of_inertia: 1.08
+ vehicle_radius: 0.318198
+ wheel_radius: 0.055
+ friction_coefficient: 0.6
+ k1: 2.958580e+00
+ k2: 3.082190e-03
+ no_load_power: 11.37
+
+auto_aim_controller:
+ ros__parameters:
+ # capture
+ use_video: false # If true, use video stream instead of camera.
+ video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi"
+ exposure_time: 1
+ invert_image: false
+ # identifier
+ armor_model_path: "/models/mlp.onnx"
+ # pnp
+ fx: 1.722231837421459e+03
+ fy: 1.724876404292754e+03
+ cx: 7.013056440882832e+02
+ cy: 5.645821718351237e+02
+ k1: -0.064232403853946
+ k2: -0.087667493884102
+ k3: 0.792381808294582
+ # tracker
+ armor_predict_duration: 500
+ # controller
+ gimbal_predict_duration: 100
+ yaw_error: 0.
+ pitch_error: 0.
+ shoot_velocity: 28.0
+ predict_sec: 0.095
+ # etc
+ buff_predict_duration: 200
+ buff_model_path: "/models/buff_nocolor_v6.onnx"
+ omni_exposure: 1000.0
+ record_fps: 120
+ debug: false # Setup in actual using.Debug mode is used when referee is not ready
+ debug_color: 0 # 0 For blue while 1 for red. mine
+ debug_robot_id: 4
+ debug_buff_mode: false
+ record: false
+ raw_img_pub: false # Set false in actual use
+ image_viewer_type: 2
+
+hero_auto_aim_bridge:
+ ros__parameters:
+ config_file: "configs/standard3.yaml"
+ bullet_speed_fallback: 11.4
+ result_timeout: 0.1 # 0.08
+ debug: false
+
+pitch_swept_frequency_controller:
+ ros__parameters:
+ target: /gimbal/pitch
+
+ sweep: true
+ logarithmic: true
+ start_freq: 0.1
+ end_freq: 10.0
+ duration: 60.0
+ amplitude: 10.0
+
+ pid: true
+ setpoint: 0.0
+ position_kp: 20.0
+ position_ki: 0.0
+ position_kd: 0.0
+ velocity_kp: 1.65
+ velocity_ki: 0.0
+ velocity_kd: 0.0
+ dc_offset: 0.0
+
+top_yaw_swept_frequency_controller:
+ ros__parameters:
+ target: /gimbal/top_yaw
+
+ sweep: true
+ logarithmic: true
+ start_freq: 0.1
+ end_freq: 10.0
+ duration: 60.0
+ amplitude: 20.0
+
+ pid: true
+ setpoint: 0.0
+ position_kp: 10.0
+ position_ki: 0.0
+ position_kd: 0.0
+ velocity_kp: 3.0
+ velocity_ki: 0.0
+ velocity_kd: 0.0
+ dc_offset: 0.0
+
+bottom_yaw_swept_frequency_controller:
+ ros__parameters:
+ target: /gimbal/bottom_yaw
+
+ sweep: true
+ logarithmic: true
+ start_freq: 0.1
+ end_freq: 4.0
+ duration: 80.0
+ amplitude: 2.0
diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml
new file mode 100644
index 00000000..cf751931
--- /dev/null
+++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml
@@ -0,0 +1,348 @@
+rmcs_executor:
+ ros__parameters:
+ update_rate: 1000.0
+ components:
+ - rmcs_core::hardware::SteeringHero -> hero_hardware
+
+ - rmcs_core::referee::Status -> referee_status
+ - rmcs_core::referee::command::Interaction -> referee_interaction
+ - rmcs_core::referee::command::interaction::Ui -> referee_ui
+ - rmcs_core::referee::app::ui::Hero -> referee_ui_hero
+ - rmcs_core::referee::Command -> referee_command
+
+ - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller
+ - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller
+ - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller
+ - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller
+
+ # - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller
+ # - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller
+
+ - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller
+ - rmcs_core::controller::shooting::HeatController -> heat_controller
+ - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller
+ - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller
+ - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller
+ - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller
+ - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller
+ - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder
+
+ - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status
+ - rmcs_core::controller::chassis::ChassisController -> chassis_controller
+ - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller
+ - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller
+ - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller
+
+ # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer
+ # - rmcs_auto_aim::AutoAimController -> auto_aim_controller
+
+ - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster
+ # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster
+
+ - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge
+
+ # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller
+ # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller
+ # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller
+ # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller
+
+
+
+
+hero_hardware:
+ ros__parameters:
+ board_serial_top_board: "D4-2BCA-2E47-76CD-23BC-0B78-684B"
+ board_serial_bottom_board_one: "D4-7973-19A9-EA40-4A3E-306F-10F9"
+ board_serial_bottom_board_two: "D4-3674-7174-8768-879E-E44A-3931"
+ bottom_yaw_motor_zero_point: 25912
+ pitch_motor_zero_point: 37034
+ top_yaw_motor_zero_point: 65509
+ viewer_motor_zero_point: 3030
+ external_imu_port: /dev/ttyUSB0
+ bullet_feeder_motor_zero_point: 11865
+ left_front_zero_point: 4394
+ right_front_zero_point: 1020
+ left_back_zero_point: 3776
+ right_back_zero_point: 5791
+
+
+
+
+value_broadcaster:
+ ros__parameters:
+ forward_list:
+ # - /shoot/heat
+ # - /chassis/power
+ # - /referee/chassis/power
+ # - /referee/chassis/power_limit
+ # - /chassis/control_power_limit
+ # - /chassis/supercap/voltage
+ # - /gimbal/putter/velocity
+ - /gimbal/first_left_friction/velocity
+ - /gimbal/first_right_friction/velocity
+ - /gimbal/second_left_friction/velocity
+ - /gimbal/second_right_friction/velocity
+ # - /gimbal/first_left_friction/control_torque
+ # - /gimbal/first_second_friction/control_torque
+ # - /gimbal/second_left_friction/control_torque
+ # - /gimbal/second_right_friction/control_torque
+ # - /gimbal/bottom_yaw/torque
+ - /gimbal/bottom_yaw/control_angle_shift
+ - /gimbal/bottom_yaw/angle
+ - /gimbal/top_yaw/angle
+ - /gimbal/pitch/angle
+
+ # - /gimbal/pitch/velocity_imu
+ # - /gimbal/pitch/control_velocity
+ # - /gimbal/pitch/control_torque
+
+ - /gimbal/auto_aim/plan_yaw
+ - /gimbal/auto_aim/plan_pitch
+
+
+climber_controller:
+ ros__parameters:
+ front_climber_velocity: 60.0
+ back_climber_velocity: 20.0
+ auto_climb_dash_velocity: 3.0
+ auto_climb_support_retract_velocity: 30.0
+ sync_coefficient: 0.3
+ first_stair_approach_pitch: 0.485
+ second_stair_approach_pitch: 0.35
+ front_kp: 1.0
+ front_ki: 0.0
+ front_kd: 0.5
+ back_kp: 1.0
+ back_ki: 0.0
+ back_kd: 0.0
+
+gimbal_controller:
+ ros__parameters:
+ upper_limit: -0.753376
+ lower_limit: 0.257038
+
+dual_yaw_controller:
+ ros__parameters:
+ top_yaw_angle_kp: 22.2
+ top_yaw_angle_ki: 0.0
+ top_yaw_angle_kd: 0.0
+ top_yaw_velocity_kp: 8.92
+ top_yaw_velocity_ki: 0.0026
+ top_yaw_velocity_kd: 0.0
+ top_yaw_velocity_integral_min: -2500.0
+ top_yaw_velocity_integral_max: 2500.0
+ bottom_yaw_angle_kp: 13.9
+ bottom_yaw_angle_ki: 0.0
+ bottom_yaw_angle_kd: 0.0
+ bottom_yaw_velocity_kp: 3.0
+ bottom_yaw_velocity_ki: 0.0005
+ bottom_yaw_velocity_kd: 0.0
+ bottom_yaw_velocity_integral_min: -2500.0
+ bottom_yaw_velocity_integral_max: 2500.0
+
+pitch_angle_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/pitch/control_angle_error
+ control: /gimbal/pitch/control_velocity
+ kp: 25.6
+ ki: 0.0
+ kd: 0.0
+
+pitch_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/pitch/velocity_imu
+ setpoint: /gimbal/pitch/control_velocity
+ control: /gimbal/pitch/control_torque
+ kp: 14.11
+ ki: 0.0040
+ kd: 0.0
+ integral_min: -2500.0
+ integral_max: 2500.0
+
+gimbal_player_viewer_controller0:
+ ros__parameters:
+ upper_limit: 0.0101
+ lower_limit: 0.6191
+
+viewer_angle_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/player_viewer/control_angle_error
+ control: /gimbal/player_viewer/control_velocity
+ kp: 17.00
+ ki: 0.00
+ kd: 2.00
+
+friction_wheel_controller:
+ ros__parameters:
+ friction_wheels:
+ - /gimbal/first_left_friction
+ - /gimbal/first_right_friction
+ - /gimbal/second_left_friction
+ - /gimbal/second_right_friction
+ friction_velocities:
+ - 365.00
+ - 365.00
+ - 569.00
+ - 569.00
+ friction_soft_start_stop_time: 1.0
+
+heat_controller:
+ ros__parameters:
+ heat_per_shot: 100000
+ reserved_heat: 20000
+
+shooting_recorder:
+ ros__parameters:
+ friction_wheel_count: 4
+ aim_velocity: 11.8
+ log_mode: 1 # 1: trigger, 2: timing
+
+first_left_friction_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/first_left_friction/velocity
+ setpoint: /gimbal/first_left_friction/control_velocity
+ control: /gimbal/first_left_friction/control_torque
+ kp: 0.0005
+ ki: 0.00
+ kd: 0.00004
+
+first_right_friction_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/first_right_friction/velocity
+ setpoint: /gimbal/first_right_friction/control_velocity
+ control: /gimbal/first_right_friction/control_torque
+ kp: 0.0005
+ ki: 0.00
+ kd: 0.00004
+
+second_left_friction_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/second_left_friction/velocity
+ setpoint: /gimbal/second_left_friction/control_velocity
+ control: /gimbal/second_left_friction/control_torque
+ kp: 0.0009
+ ki: 0.00
+ kd: 0.00008
+
+second_right_friction_velocity_pid_controller:
+ ros__parameters:
+ measurement: /gimbal/second_right_friction/velocity
+ setpoint: /gimbal/second_right_friction/control_velocity
+ control: /gimbal/second_right_friction/control_torque
+ kp: 0.0009
+ ki: 0.00
+ kd: 0.00008
+
+steering_wheel_status:
+ ros__parameters:
+ vehicle_radius: 0.318198
+ wheel_radius: 0.055
+
+steering_wheel_controller:
+ ros__parameters:
+ mess: 22.0
+ moment_of_inertia: 1.08
+ vehicle_radius: 0.318198
+ wheel_radius: 0.055
+ friction_coefficient: 0.6
+ k1: 2.958580e+00
+ k2: 3.082190e-03
+ no_load_power: 11.37
+
+auto_aim_controller:
+ ros__parameters:
+ # capture
+ use_video: false # If true, use video stream instead of camera.
+ video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi"
+ exposure_time: 1
+ invert_image: false
+ # identifier
+ armor_model_path: "/models/mlp.onnx"
+ # pnp
+ fx: 1.722231837421459e+03
+ fy: 1.724876404292754e+03
+ cx: 7.013056440882832e+02
+ cy: 5.645821718351237e+02
+ k1: -0.064232403853946
+ k2: -0.087667493884102
+ k3: 0.792381808294582
+ # tracker
+ armor_predict_duration: 500
+ # controller
+ gimbal_predict_duration: 100
+ yaw_error: 0.
+ pitch_error: 0.
+ shoot_velocity: 28.0
+ predict_sec: 0.095
+ # etc
+ buff_predict_duration: 200
+ buff_model_path: "/models/buff_nocolor_v6.onnx"
+ omni_exposure: 1000.0
+ record_fps: 120
+ debug: false # Setup in actual using.Debug mode is used when referee is not ready
+ debug_color: 0 # 0 For blue while 1 for red. mine
+ debug_robot_id: 4
+ debug_buff_mode: false
+ record: false
+ raw_img_pub: false # Set false in actual use
+ image_viewer_type: 2
+
+hero_auto_aim_bridge:
+ ros__parameters:
+ config_file: "configs/standard3.yaml"
+ bullet_speed_fallback: 11.7
+ result_timeout: 0.1 # 0.08
+ debug: false
+
+pitch_swept_frequency_controller:
+ ros__parameters:
+ target: /gimbal/pitch
+
+ sweep: true
+ logarithmic: true
+ start_freq: 0.1
+ end_freq: 10.0
+ duration: 60.0
+ amplitude: 10.0
+
+ pid: true
+ setpoint: 0.0
+ position_kp: 20.0
+ position_ki: 0.0
+ position_kd: 0.0
+ velocity_kp: 1.65
+ velocity_ki: 0.0
+ velocity_kd: 0.0
+ dc_offset: 0.0
+
+top_yaw_swept_frequency_controller:
+ ros__parameters:
+ target: /gimbal/top_yaw
+
+ sweep: true
+ logarithmic: true
+ start_freq: 0.1
+ end_freq: 10.0
+ duration: 60.0
+ amplitude: 20.0
+
+ pid: true
+ setpoint: 0.0
+ position_kp: 10.0
+ position_ki: 0.0
+ position_kd: 0.0
+ velocity_kp: 3.0
+ velocity_ki: 0.0
+ velocity_kd: 0.0
+ dc_offset: 0.0
+
+bottom_yaw_swept_frequency_controller:
+ ros__parameters:
+ target: /gimbal/bottom_yaw
+
+ sweep: true
+ logarithmic: true
+ start_freq: 0.1
+ end_freq: 4.0
+ duration: 80.0
+ amplitude: 2.0
diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs
new file mode 160000
index 00000000..eff14b71
--- /dev/null
+++ b/rmcs_ws/src/rmcs_core/librmcs
@@ -0,0 +1 @@
+Subproject commit eff14b71e334baaaa462a83c7e6d834886b10168
diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml
index db9ff7c4..18e27e14 100644
--- a/rmcs_ws/src/rmcs_core/plugins.xml
+++ b/rmcs_ws/src/rmcs_core/plugins.xml
@@ -1,31 +1,142 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Steering wheel controller.
+
+
+ Steering wheel status estimator.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Gimbal player viewer
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ the recorder of Hero
+
+
+ Test plugin.
+
+
+ Recorder for friction wheel PID tuning.
+
+
+ Test plugin.
+
+
+ Feedforward pid controller.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+ Test plugin.
+
+
+
diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp
new file mode 100644
index 00000000..acee0afa
--- /dev/null
+++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp
@@ -0,0 +1,570 @@
+#include "controller/pid/matrix_pid_calculator.hpp"
+#include "rmcs_msgs/keyboard.hpp"
+#include "rmcs_msgs/switch.hpp"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace rmcs_core::controller::chassis {
+
+enum class AutoClimbState {
+ IDLE,
+ ALIGN,
+ APPROACH,
+ SUPPORT_DEPLOY,
+ DASH,
+ SUPPORT_RETRACT,
+ FORERAKED
+};
+
+class ChassisClimberController
+ : public rmcs_executor::Component
+ , public rclcpp::Node {
+public:
+ ChassisClimberController()
+ : Node(
+ get_component_name(),
+ rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true))
+ , logger_(get_logger())
+ , front_velocity_pid_calculator_(
+ get_parameter("front_kp").as_double(), get_parameter("front_ki").as_double(),
+ get_parameter("front_kd").as_double())
+ , back_velocity_pid_calculator_(
+ get_parameter("back_kp").as_double(), get_parameter("back_ki").as_double(),
+ get_parameter("back_kd").as_double()) {
+
+ track_velocity_max_ = get_parameter("front_climber_velocity").as_double();
+ climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double();
+ auto_climb_dash_velocity_ = get_parameter("auto_climb_dash_velocity").as_double();
+ auto_climb_support_retract_velocity_abs_ =
+ get_parameter("auto_climb_support_retract_velocity").as_double();
+ sync_coefficient_ = get_parameter("sync_coefficient").as_double();
+ first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double();
+ second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").as_double();
+
+ register_output(
+ "/chassis/climber/left_front_motor/control_torque", climber_front_left_control_torque_,
+ nan_);
+ register_output(
+ "/chassis/climber/right_front_motor/control_torque",
+ climber_front_right_control_torque_, nan_);
+ register_output(
+ "/chassis/climber/left_back_motor/control_torque", climber_back_left_control_torque_,
+ nan_);
+ register_output(
+ "/chassis/climber/right_back_motor/control_torque", climber_back_right_control_torque_,
+ nan_);
+ register_output("/chassis/climbing_forward_velocity", climbing_forward_velocity_, nan_);
+
+ register_input("/chassis/climber/left_front_motor/velocity", climber_front_left_velocity_);
+ register_input(
+ "/chassis/climber/right_front_motor/velocity", climber_front_right_velocity_);
+ register_input("/chassis/climber/left_back_motor/velocity", climber_back_left_velocity_);
+ register_input("/chassis/climber/right_back_motor/velocity", climber_back_right_velocity_);
+
+ register_input("/chassis/climber/left_back_motor/torque", climber_back_left_torque_);
+ register_input("/chassis/climber/right_back_motor/torque", climber_back_right_torque_);
+
+ register_input("/remote/switch/right", switch_right_);
+ register_input("/remote/switch/left", switch_left_);
+ register_input("/remote/keyboard", keyboard_);
+ register_input("/remote/rotary_knob_switch", rotary_knob_switch_);
+ register_input("/chassis/pitch_imu", chassis_pitch_imu_);
+
+ register_input("/gimbal/yaw/angle", gimbal_yaw_angle_);
+ register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_);
+ register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_);
+ }
+
+ void update() override {
+ using namespace rmcs_msgs;
+
+ auto switch_right = *switch_right_;
+ auto switch_left = *switch_left_;
+ auto keyboard = *keyboard_;
+ auto rotary_knob_switch = *rotary_knob_switch_;
+
+ // RCLCPP_INFO(logger_, "%lf", *chassis_pitch_imu_);
+
+ bool rotary_knob_to_down =
+ (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN);
+ bool rotary_knob_from_down =
+ (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN);
+
+ if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN)
+ || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) {
+ reset_all_controls();
+ } else {
+ handle_auto_climb_requests(
+ (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down,
+ rotary_knob_switch);
+
+ if (auto_climb_state_ != AutoClimbState::IDLE) {
+ stop_manual_support();
+ apply_auto_climb_control(update_auto_climb_control());
+ } else {
+ apply_auto_climb_control(update_manual_support_control(keyboard));
+ apply_manual_support_zero_output();
+ }
+ }
+
+ last_switch_left_ = switch_left;
+ last_switch_right_ = switch_right;
+ last_keyboard_ = keyboard;
+ last_rotary_knob_switch_ = rotary_knob_switch;
+ }
+
+private:
+ struct AutoClimbControl {
+ double front_track_velocity = nan_;
+ double back_climber_velocity = nan_;
+ double override_chassis_vx = nan_;
+ };
+
+ void handle_auto_climb_requests(
+ bool start_requested, bool abort_by_rotary, rmcs_msgs::Switch rotary_knob_switch) {
+
+ if (start_requested) {
+ if (auto_climb_state_ == AutoClimbState::IDLE) {
+ start_auto_climb(
+ rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G");
+ } else {
+ abort_auto_climb("toggled again");
+ }
+ } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) {
+ abort_auto_climb("rotary knob left UP");
+ }
+ }
+
+ void start_auto_climb(const char* source) {
+ stop_manual_support();
+ auto_climb_continue_ = true;
+ auto_climb_stair_index_ = 0;
+ auto_climb_align_stable_count_ = 0;
+ auto_climb_support_block_count_ = 0;
+ enter_auto_climb_state(AutoClimbState::ALIGN);
+
+ RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source);
+ }
+
+ void abort_auto_climb(const char* reason) {
+ reset_all_controls();
+ RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason);
+ }
+
+ AutoClimbControl update_auto_climb_control() {
+
+ if (auto_climb_state_ == AutoClimbState::IDLE) {
+ detect_is_foreraked();
+ if (auto_climb_state_ == AutoClimbState::IDLE) {
+ return {};
+ }
+ }
+
+ auto_climb_timer_++;
+
+ switch (auto_climb_state_) {
+ case AutoClimbState::IDLE: return {};
+ case AutoClimbState::ALIGN: return update_auto_climb_align();
+ case AutoClimbState::APPROACH: return update_auto_climb_approach();
+ case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy();
+ case AutoClimbState::DASH: return update_auto_climb_dash();
+ case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract();
+ case AutoClimbState::FORERAKED: return update_forerake_defend();
+ }
+
+ return {};
+ }
+
+ AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) {
+ AutoClimbControl control;
+
+ if (keyboard.b) {
+ stop_manual_support();
+ control.back_climber_velocity = climber_back_control_velocity_abs_;
+ return control;
+ }
+
+ if (last_keyboard_.b) {
+ manual_support_retracting_ = true;
+ manual_support_retract_block_count_ = 0;
+ manual_support_zero_output_ = false;
+ RCLCPP_INFO(logger_, "Manual support retract started.");
+ }
+
+ if (!manual_support_retracting_) {
+ return control;
+ }
+
+ control.back_climber_velocity = -auto_climb_support_retract_velocity_abs_;
+
+ if (is_back_climber_blocked()) {
+ manual_support_retract_block_count_++;
+ } else {
+ manual_support_retract_block_count_ = 0;
+ }
+
+ RCLCPP_INFO_THROTTLE(
+ logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d",
+ manual_support_retract_block_count_);
+
+ if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) {
+ stop_manual_support();
+ manual_support_zero_output_ = true;
+ RCLCPP_INFO(logger_, "Manual support retract completed.");
+ return {};
+ }
+
+ return control;
+ }
+
+ AutoClimbControl update_auto_climb_align() {
+ AutoClimbControl control{
+ .front_track_velocity = 0.0,
+ .back_climber_velocity = 0.0,
+ .override_chassis_vx = 0.0,
+ };
+
+ double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_;
+ if (gimbal_yaw_angle_error < 0)
+ gimbal_yaw_angle_error += 2 * std::numbers::pi;
+
+ double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_;
+ while (err >= std::numbers::pi)
+ err -= 2 * std::numbers::pi;
+ while (err < -std::numbers::pi)
+ err += 2 * std::numbers::pi;
+
+ double yaw_velocity = *gimbal_yaw_velocity_imu_;
+ bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold;
+ bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold;
+
+ if (is_aligned && is_stable) {
+ auto_climb_align_stable_count_++;
+ } else {
+ auto_climb_align_stable_count_ = 0;
+ }
+
+ RCLCPP_INFO_THROTTLE(
+ logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", err,
+ yaw_velocity, auto_climb_align_stable_count_);
+
+ if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) {
+ enter_auto_climb_state(AutoClimbState::APPROACH);
+ RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH.");
+ }
+
+ return control;
+ }
+
+ AutoClimbControl update_auto_climb_approach() {
+ AutoClimbControl control{
+ .front_track_velocity = track_velocity_max_,
+ .back_climber_velocity = 0.0,
+ .override_chassis_vx = kAutoClimbApproachVelocity,
+ };
+
+ double pitch = *chassis_pitch_imu_;
+ double target_pitch = auto_climb_stair_index_ == 0 ? first_stair_approach_pitch_
+ : second_stair_approach_pitch_;
+
+ RCLCPP_INFO_THROTTLE(
+ logger_, *get_clock(), 500, "APPROACH (step %d): pitch=%.3f, target>%.3f",
+ auto_climb_stair_index_ + 1, pitch, target_pitch);
+
+ if (pitch > target_pitch) {
+ enter_auto_climb_state(AutoClimbState::SUPPORT_DEPLOY);
+ RCLCPP_INFO(
+ logger_, "Auto climb entering SUPPORT_DEPLOY (step %d).",
+ auto_climb_stair_index_ + 1);
+ }
+
+ return control;
+ }
+
+ AutoClimbControl update_auto_climb_support_deploy() {
+ AutoClimbControl control{
+ .front_track_velocity = 0.0,
+ .back_climber_velocity = climber_back_control_velocity_abs_,
+ .override_chassis_vx = 0.5,
+ };
+
+ if (is_back_climber_blocked()) {
+ auto_climb_support_block_count_++;
+ } else {
+ auto_climb_support_block_count_ = 0;
+ }
+
+ RCLCPP_INFO_THROTTLE(
+ logger_, *get_clock(), 500, "SUPPORT_DEPLOY (step %d): blocked_ticks=%d",
+ auto_climb_stair_index_ + 1, auto_climb_support_block_count_);
+
+ if (auto_climb_support_block_count_ >= kAutoClimbSupportConfirmTicks) {
+ enter_auto_climb_state(AutoClimbState::DASH);
+ RCLCPP_INFO(
+ logger_, "Auto climb entering DASH (step %d).", auto_climb_stair_index_ + 1);
+ }
+
+ return control;
+ }
+
+ AutoClimbControl update_auto_climb_dash() {
+ AutoClimbControl control{
+ .front_track_velocity = track_velocity_max_,
+ .back_climber_velocity = climber_back_control_velocity_abs_,
+ .override_chassis_vx = auto_climb_dash_velocity_,
+ };
+
+ double pitch = *chassis_pitch_imu_;
+ bool is_leveled = std::abs(pitch) < kAutoClimbLeveledPitchThreshold
+ && auto_climb_timer_ > kAutoClimbDashMinTicks;
+ bool timeout = auto_climb_timer_ > kAutoClimbDashTimeoutTicks;
+
+ RCLCPP_INFO_THROTTLE(
+ logger_, *get_clock(), 500, "DASH (step %d): pitch=%.3f, timer=%d",
+ auto_climb_stair_index_ + 1, pitch, auto_climb_timer_);
+
+ if (is_leveled || timeout) {
+ enter_auto_climb_state(AutoClimbState::SUPPORT_RETRACT);
+
+ if (timeout) {
+ RCLCPP_WARN(
+ logger_, "Auto climb DASH timeout on step %d. Entering SUPPORT_RETRACT.",
+ auto_climb_stair_index_ + 1);
+ } else {
+ RCLCPP_INFO(
+ logger_, "Auto climb reached platform on step %d.",
+ auto_climb_stair_index_ + 1);
+ }
+ }
+
+ return control;
+ }
+
+ AutoClimbControl update_auto_climb_support_retract() {
+ AutoClimbControl control{
+ .front_track_velocity = track_velocity_max_,
+ .back_climber_velocity = -auto_climb_support_retract_velocity_abs_,
+ .override_chassis_vx = auto_climb_dash_velocity_,
+ };
+
+ RCLCPP_INFO_THROTTLE(
+ logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): timer=%d",
+ auto_climb_stair_index_ + 1, auto_climb_timer_);
+
+ if (auto_climb_timer_ > kAutoClimbSupportRetractTicks) {
+ bool has_next_stair =
+ auto_climb_continue_ && (auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs);
+
+ if (has_next_stair) {
+ auto_climb_stair_index_++;
+ enter_auto_climb_state(AutoClimbState::APPROACH);
+ RCLCPP_INFO(
+ logger_, "Auto climb continuing to step %d.", auto_climb_stair_index_ + 1);
+ } else {
+ int finished_steps = auto_climb_stair_index_ + 1;
+ stop_auto_climb();
+ RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", finished_steps);
+ }
+ }
+
+ return control;
+ }
+
+ AutoClimbControl update_forerake_defend() {
+ AutoClimbControl control{
+ .front_track_velocity = track_velocity_max_,
+ .back_climber_velocity = 0.0,
+ .override_chassis_vx = nan_,
+ };
+
+ double pitch = *chassis_pitch_imu_;
+ bool is_leveled = std::abs(pitch) < kAutoClimbLeveledPitchThreshold
+ && auto_climb_timer_ > kAutoClimbDashMinTicks;
+ bool timeout = auto_climb_timer_ > kAutoClimbForerakedTimeoutTicks;
+
+ if (is_leveled || timeout) {
+ enter_auto_climb_state(AutoClimbState::IDLE);
+ }
+
+ return control;
+ }
+
+ void apply_auto_climb_control(const AutoClimbControl& control) {
+ *climbing_forward_velocity_ = control.override_chassis_vx;
+
+ dual_motor_sync_control(
+ control.front_track_velocity, *climber_front_left_velocity_,
+ *climber_front_right_velocity_, front_velocity_pid_calculator_,
+ *climber_front_left_control_torque_, *climber_front_right_control_torque_);
+
+ dual_motor_sync_control(
+ control.back_climber_velocity, *climber_back_left_velocity_,
+ *climber_back_right_velocity_, back_velocity_pid_calculator_,
+ *climber_back_left_control_torque_, *climber_back_right_control_torque_);
+ }
+
+ void reset_all_controls() {
+ *climber_front_left_control_torque_ = nan_;
+ *climber_front_right_control_torque_ = nan_;
+ *climber_back_left_control_torque_ = nan_;
+ *climber_back_right_control_torque_ = nan_;
+ *climbing_forward_velocity_ = nan_;
+ stop_manual_support();
+ stop_auto_climb();
+ }
+
+ void apply_manual_support_zero_output() {
+ if (!manual_support_zero_output_) {
+ return;
+ }
+
+ *climber_back_left_control_torque_ = 0.0;
+ *climber_back_right_control_torque_ = 0.0;
+ }
+
+ void stop_manual_support() {
+ manual_support_retracting_ = false;
+ manual_support_retract_block_count_ = 0;
+ manual_support_zero_output_ = false;
+ }
+
+ void stop_auto_climb() {
+ auto_climb_state_ = AutoClimbState::IDLE;
+ auto_climb_timer_ = 0;
+ auto_climb_stair_index_ = 0;
+ auto_climb_continue_ = false;
+ auto_climb_align_stable_count_ = 0;
+ auto_climb_support_block_count_ = 0;
+ }
+
+ void enter_auto_climb_state(AutoClimbState state) {
+ if (state == auto_climb_state_) {
+ return;
+ }
+ auto_climb_state_ = state;
+ auto_climb_timer_ = 0;
+ auto_climb_align_stable_count_ = 0;
+ auto_climb_support_block_count_ = 0;
+ }
+
+ bool is_back_climber_blocked() const {
+ return (std::abs(*climber_back_left_torque_) > kBackClimberBlockedTorqueThreshold
+ && std::abs(*climber_back_left_velocity_) < kBackClimberBlockedVelocityThreshold)
+ || (std::abs(*climber_back_right_torque_) > kBackClimberBlockedTorqueThreshold
+ && std::abs(*climber_back_right_velocity_) < kBackClimberBlockedVelocityThreshold);
+ }
+
+ void dual_motor_sync_control(
+ double setpoint, double left_velocity, double right_velocity,
+ pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out,
+ double& right_torque_out) {
+
+ if (std::isnan(setpoint)) {
+ left_torque_out = nan_;
+ right_torque_out = nan_;
+ return;
+ }
+
+ Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity};
+ Eigen::Vector2d relative_velocity{
+ left_velocity - right_velocity, right_velocity - left_velocity};
+
+ Eigen::Vector2d control_error = setpoint_error - sync_coefficient_ * relative_velocity;
+ auto control_torques = pid_calculator.update(control_error);
+
+ left_torque_out = control_torques[0];
+ right_torque_out = control_torques[1];
+ }
+
+ void detect_is_foreraked() {
+ if (auto_climb_state_ != AutoClimbState::IDLE) {
+ return;
+ }
+
+ double pitch = *chassis_pitch_imu_;
+ bool is_foreraked = pitch < kForerakedetectPitchThreshold;
+ if (is_foreraked) {
+ RCLCPP_INFO(get_logger(), "Enter foreraked");
+ enter_auto_climb_state(AutoClimbState::FORERAKED);
+ }
+ }
+
+ rclcpp::Logger logger_;
+ static constexpr double nan_ = std::numeric_limits::quiet_NaN();
+ static constexpr double kAutoClimbApproachVelocity = 1.0;
+ static constexpr double kAutoClimbAlignThreshold = 0.10;
+ static constexpr double kAutoClimbAlignVelocityThreshold = 0.2;
+ static constexpr double kAutoClimbLeveledPitchThreshold = 0.1;
+ static constexpr double kForerakedetectPitchThreshold = -0.05;
+ static constexpr double kBackClimberBlockedTorqueThreshold = 0.1;
+ static constexpr double kBackClimberBlockedVelocityThreshold = 0.1;
+ static constexpr int kAutoClimbAlignConfirmTicks = 50;
+ static constexpr int kAutoClimbSupportConfirmTicks = 50;
+ static constexpr int kAutoClimbDashMinTicks = 500;
+ static constexpr int kAutoClimbDashTimeoutTicks = 3000;
+ static constexpr int kAutoClimbForerakedTimeoutTicks = 2000;
+ static constexpr int kAutoClimbSupportRetractTicks = 1000;
+ static constexpr int kAutoClimbMaxStairs = 2;
+ static constexpr int kManualSupportRetractConfirmTicks = 50;
+
+ double sync_coefficient_;
+ double first_stair_approach_pitch_;
+ double second_stair_approach_pitch_;
+
+ double track_velocity_max_;
+ double climber_back_control_velocity_abs_;
+ double auto_climb_dash_velocity_;
+ double auto_climb_support_retract_velocity_abs_;
+
+ AutoClimbState auto_climb_state_ = AutoClimbState::IDLE;
+ int auto_climb_timer_ = 0;
+ int auto_climb_stair_index_ = 0;
+ int auto_climb_align_stable_count_ = 0;
+ int auto_climb_support_block_count_ = 0;
+ bool auto_climb_continue_ = false;
+ bool manual_support_retracting_ = false;
+ int manual_support_retract_block_count_ = 0;
+ bool manual_support_zero_output_ = false;
+
+ OutputInterface climber_front_left_control_torque_;
+ OutputInterface climber_front_right_control_torque_;
+ OutputInterface climber_back_left_control_torque_;
+ OutputInterface climber_back_right_control_torque_;
+ OutputInterface climbing_forward_velocity_;
+
+ InputInterface climber_front_left_velocity_;
+ InputInterface climber_front_right_velocity_;
+ InputInterface climber_back_left_velocity_;
+ InputInterface climber_back_right_velocity_;
+
+ InputInterface climber_back_left_torque_;
+ InputInterface climber_back_right_torque_;
+
+ InputInterface switch_right_;
+ InputInterface switch_left_;
+ InputInterface keyboard_;
+ InputInterface rotary_knob_switch_;
+
+ InputInterface chassis_pitch_imu_;
+ InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_;
+
+ rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN;
+ rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN;
+ rmcs_msgs::Switch last_rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN;
+ rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero();
+
+ pid::MatrixPidCalculator<2> front_velocity_pid_calculator_, back_velocity_pid_calculator_;
+};
+} // namespace rmcs_core::controller::chassis
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(
+ rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component)
diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp
index da70a96f..67114e99 100644
--- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp
+++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp
@@ -1,4 +1,8 @@
+#include
+#include
#include
+#include
+#include
#include
#include
#include
@@ -19,7 +23,7 @@ class ChassisController
: Node(
get_component_name(),
rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true))
- , following_velocity_controller_(7.0, 0.0, 0.0) {
+ , following_velocity_controller_(10.0, 0.0, 2.4) {
following_velocity_controller_.output_max = angular_velocity_max;
following_velocity_controller_.output_min = -angular_velocity_max;
@@ -34,6 +38,8 @@ class ChassisController
register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false);
register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false);
+ register_input("/chassis/velocity", chassis_velocity_, false);
+ register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false);
register_output("/chassis/angle", chassis_angle_, nan);
register_output("/chassis/control_angle", chassis_control_angle_, nan);
@@ -52,14 +58,19 @@ class ChassisController
RCLCPP_WARN(
get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0.");
}
+ chassis_velocity_feedback_ready_ = chassis_velocity_.ready();
+ if (!chassis_velocity_feedback_ready_) {
+ chassis_velocity_.make_and_bind_directly(0.0, 0.0, 0.0);
+ }
}
void update() override {
using namespace rmcs_msgs;
auto switch_right = *switch_right_;
- auto switch_left = *switch_left_;
- auto keyboard = *keyboard_;
+ auto switch_left = *switch_left_;
+ auto keyboard = *keyboard_;
+ auto mode = *mode_;
do {
if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN)
@@ -68,30 +79,31 @@ class ChassisController
break;
}
- auto mode = *mode_;
if (switch_left != Switch::DOWN) {
if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) {
if (mode == rmcs_msgs::ChassisMode::SPIN) {
mode = rmcs_msgs::ChassisMode::STEP_DOWN;
} else {
- mode = rmcs_msgs::ChassisMode::SPIN;
+ mode = rmcs_msgs::ChassisMode::SPIN;
spinning_forward_ = !spinning_forward_;
}
} else if (!last_keyboard_.c && keyboard.c) {
if (mode == rmcs_msgs::ChassisMode::SPIN) {
mode = rmcs_msgs::ChassisMode::AUTO;
} else {
- mode = rmcs_msgs::ChassisMode::SPIN;
+ mode = rmcs_msgs::ChassisMode::SPIN;
spinning_forward_ = !spinning_forward_;
}
} else if (!last_keyboard_.x && keyboard.x) {
- mode = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP
- ? rmcs_msgs::ChassisMode::AUTO
- : rmcs_msgs::ChassisMode::LAUNCH_RAMP;
+ mode = rmcs_msgs::ChassisMode::STEP_DOWN;
+ step_down_facing_ = StepDownFacing::FRONT;
} else if (!last_keyboard_.z && keyboard.z) {
- mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN
- ? rmcs_msgs::ChassisMode::AUTO
- : rmcs_msgs::ChassisMode::STEP_DOWN;
+ if (mode == rmcs_msgs::ChassisMode::STEP_DOWN) {
+ mode = rmcs_msgs::ChassisMode::AUTO;
+ } else {
+ mode = rmcs_msgs::ChassisMode::STEP_DOWN;
+ step_down_facing_ = StepDownFacing::BACK;
+ }
}
*mode_ = mode;
}
@@ -100,24 +112,29 @@ class ChassisController
} while (false);
last_switch_right_ = switch_right;
- last_switch_left_ = switch_left;
- last_keyboard_ = keyboard;
+ last_switch_left_ = switch_left;
+ last_keyboard_ = keyboard;
}
void reset_all_controls() {
*mode_ = rmcs_msgs::ChassisMode::AUTO;
+ step_down_facing_ = StepDownFacing::BACK;
*chassis_control_velocity_ = {nan, nan, nan};
}
void update_velocity_control() {
auto translational_velocity = update_translational_velocity_control();
- auto angular_velocity = update_angular_velocity_control();
+ auto angular_velocity = update_angular_velocity_control(translational_velocity);
chassis_control_velocity_->vector << translational_velocity, angular_velocity;
}
Eigen::Vector2d update_translational_velocity_control() {
+ if (!std::isnan(*climbing_forward_velocity_)) {
+ return Eigen::Vector2d{*climbing_forward_velocity_, 0.0};
+ }
+
auto keyboard = *keyboard_;
Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d};
@@ -132,31 +149,43 @@ class ChassisController
return translational_velocity;
}
- double update_angular_velocity_control() {
- double angular_velocity = 0.0;
+ double update_angular_velocity_control(const Eigen::Vector2d& translational_velocity) {
+ double angular_velocity = 0.0;
double chassis_control_angle = nan;
+ if (!std::isnan(*climbing_forward_velocity_)) {
+ double err = calculate_unsigned_chassis_angle_error(chassis_control_angle);
+
+ constexpr double alignment = 2 * std::numbers::pi;
+ if (err > alignment / 2)
+ err -= alignment;
+
+ angular_velocity = following_velocity_controller_.update(err);
+
+ *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_;
+ *chassis_control_angle_ = chassis_control_angle;
+ return angular_velocity;
+ }
+
switch (*mode_) {
- case rmcs_msgs::ChassisMode::AUTO: break;
+ case rmcs_msgs::ChassisMode::AUTO: {
+ angular_velocity =
+ update_following_angular_velocity(StepDownFacing::BACK, chassis_control_angle);
+
+ // Keep AUTO rear-following gentle at low translation speed and fully enabled at max.
+ const double measured_translational_speed =
+ chassis_velocity_feedback_ready_ ? chassis_velocity_->vector.head<2>().norm()
+ : translational_velocity.norm();
+ angular_velocity *=
+ std::clamp(measured_translational_speed / translational_velocity_max, 0.0, 0.3);
+ } break;
case rmcs_msgs::ChassisMode::SPIN: {
angular_velocity =
0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max);
} break;
case rmcs_msgs::ChassisMode::STEP_DOWN: {
- double err = calculate_unsigned_chassis_angle_error(chassis_control_angle);
-
- // err: [0, 2pi) -> [0, alignment) -> signed.
- // In step-down mode, two sides of the chassis can be used for alignment.
- // TODO: Dynamically determine the split angle based on chassis velocity.
- constexpr double alignment = std::numbers::pi;
- while (err > alignment / 2) {
- chassis_control_angle -= alignment;
- if (chassis_control_angle < 0)
- chassis_control_angle += 2 * std::numbers::pi;
- err -= alignment;
- }
-
- angular_velocity = following_velocity_controller_.update(err);
+ angular_velocity =
+ update_following_angular_velocity(step_down_facing_, chassis_control_angle);
} break;
case rmcs_msgs::ChassisMode::LAUNCH_RAMP: {
double err = calculate_unsigned_chassis_angle_error(chassis_control_angle);
@@ -171,38 +200,62 @@ class ChassisController
angular_velocity = following_velocity_controller_.update(err);
} break;
}
- *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_;
+ *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_;
*chassis_control_angle_ = chassis_control_angle;
return angular_velocity;
}
double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) {
- chassis_control_angle = *gimbal_yaw_angle_error_;
- if (chassis_control_angle < 0)
- chassis_control_angle += 2 * std::numbers::pi;
- // chassis_control_angle: [0, 2pi).
+ chassis_control_angle = normalize_positive_angle(*gimbal_yaw_angle_error_);
// err = setpoint - measurement
// ^ ^
// |gimbal_yaw_angle_error |chassis_angle
// ^
// |(2pi - gimbal_yaw_angle)
- double err = chassis_control_angle + *gimbal_yaw_angle_;
- if (err >= 2 * std::numbers::pi)
- err -= 2 * std::numbers::pi;
- // err: [0, 2pi).
+ double err = normalize_positive_angle(chassis_control_angle + *gimbal_yaw_angle_);
return err;
}
private:
- static constexpr double inf = std::numeric_limits::infinity();
+ enum class StepDownFacing { FRONT, BACK };
+
+ double update_following_angular_velocity(
+ StepDownFacing target_facing, double& chassis_control_angle) {
+ double err = calculate_unsigned_chassis_angle_error(chassis_control_angle);
+ if (target_facing == StepDownFacing::BACK) {
+ chassis_control_angle =
+ normalize_positive_angle(chassis_control_angle + std::numbers::pi);
+ err = normalize_positive_angle(err + std::numbers::pi);
+ }
+
+ err = normalize_signed_angle(err);
+ return following_velocity_controller_.update(err);
+ }
+
+ static double normalize_positive_angle(double angle) {
+ constexpr double full_turn = 2 * std::numbers::pi;
+ while (angle >= full_turn)
+ angle -= full_turn;
+ while (angle < 0.0)
+ angle += full_turn;
+ return angle;
+ }
+
+ static double normalize_signed_angle(double angle) {
+ angle = normalize_positive_angle(angle);
+ if (angle > std::numbers::pi)
+ angle -= 2 * std::numbers::pi;
+ return angle;
+ }
+
static constexpr double nan = std::numeric_limits::quiet_NaN();
// Maximum control velocities
static constexpr double translational_velocity_max = 10.0;
- static constexpr double angular_velocity_max = 16.0;
+ static constexpr double angular_velocity_max = 16.0;
InputInterface joystick_right_;
InputInterface joystick_left_;
@@ -214,14 +267,18 @@ class ChassisController
InputInterface rotary_knob_;
rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN;
- rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN;
- rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero();
+ rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN;
+ rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero();
InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_;
+ InputInterface chassis_velocity_;
+ InputInterface climbing_forward_velocity_;
OutputInterface chassis_angle_, chassis_control_angle_;
OutputInterface mode_;
bool spinning_forward_ = true;
+ bool chassis_velocity_feedback_ready_ = false;
+ StepDownFacing step_down_facing_ = StepDownFacing::BACK;
pid::PidCalculator following_velocity_controller_;
OutputInterface chassis_control_velocity_;
@@ -231,4 +288,4 @@ class ChassisController
#include
-PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::ChassisController, rmcs_executor::Component)
\ No newline at end of file
+PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::ChassisController, rmcs_executor::Component)
diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp
index 2d084f8b..098f17a5 100644
--- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp
+++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp
@@ -1,4 +1,5 @@
#include
+#include
#include
#include
#include
@@ -51,9 +52,9 @@ class ChassisPowerController
using namespace rmcs_msgs;
auto switch_right = *switch_right_;
- auto switch_left = *switch_left_;
- auto keyboard = *keyboard_;
- auto rotary_knob = *rotary_knob_;
+ auto switch_left = *switch_left_;
+ auto keyboard = *keyboard_;
+ auto rotary_knob = *rotary_knob_;
if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN)
|| (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) {
@@ -63,7 +64,7 @@ class ChassisPowerController
update_virtual_buffer_energy();
- boost_mode_ = keyboard.shift || rotary_knob < -0.9;
+ boost_mode_ = keyboard.shift || rotary_knob < -0.9 || rotary_knob > 0.9 || keyboard.g;
update_control_power_limit();
}
@@ -74,8 +75,8 @@ class ChassisPowerController
// charging_power_limit =
constexpr double buffer_energy_control_line = 120; // = referee + excess
- constexpr double buffer_energy_base_line = 30; // = referee
- constexpr double buffer_energy_dead_line = 0; // = 0
+ constexpr double buffer_energy_base_line = 30; // = referee
+ constexpr double buffer_energy_dead_line = 0; // = 0
*supercap_charge_power_limit_ =
*chassis_power_limit_referee_
@@ -91,8 +92,8 @@ class ChassisPowerController
}
void reset_power_control() {
- virtual_buffer_energy_ = virtual_buffer_energy_limit_;
- boost_mode_ = false;
+ virtual_buffer_energy_ = virtual_buffer_energy_limit_;
+ boost_mode_ = false;
*chassis_control_power_limit_ = 0.0;
}
@@ -108,17 +109,18 @@ class ChassisPowerController
double power_limit;
if (boost_mode_ && *supercap_enabled_)
- power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP
- ? inf_
- : *chassis_power_limit_referee_ + 80.0;
+ power_limit = inf_;
+ // power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP
+ // ? inf_
+ // : *chassis_power_limit_referee_ + 80.0;
else
power_limit = *chassis_power_limit_referee_;
chassis_power_limit_expected_ = power_limit;
// chassis_control_power_limit =
constexpr double supercap_voltage_control_line = 12.5; // = supercap
- constexpr double supercap_voltage_base_line = 12.0; // = referee
- power_limit = *chassis_power_limit_referee_
+ constexpr double supercap_voltage_base_line = 12.0; // = referee
+ power_limit = *chassis_power_limit_referee_
+ (power_limit - *chassis_power_limit_referee_)
* std::clamp(
(*supercap_voltage_ - supercap_voltage_base_line)
diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp
new file mode 100644
index 00000000..8450b386
--- /dev/null
+++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp
@@ -0,0 +1,104 @@
+#include
+
+#include
+#include
+#include
+#include
+
+namespace rmcs_core::controller::chassis {
+
+class SteeringWheelStatus
+ : public rmcs_executor::Component
+ , public rclcpp::Node {
+public:
+ explicit SteeringWheelStatus()
+ : Node(
+ get_component_name(),
+ rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true))
+ , vehicle_radius_(get_parameter("vehicle_radius").as_double())
+ , wheel_radius_(get_parameter("wheel_radius").as_double()) {
+ register_input("/chassis/left_front_steering/angle", left_front_steering_angle_);
+ register_input("/chassis/left_back_steering/angle", left_back_steering_angle_);
+ register_input("/chassis/right_back_steering/angle", right_back_steering_angle_);
+ register_input("/chassis/right_front_steering/angle", right_front_steering_angle_);
+
+ register_input("/chassis/left_front_wheel/velocity", left_front_wheel_velocity_);
+ register_input("/chassis/left_back_wheel/velocity", left_back_wheel_velocity_);
+ register_input("/chassis/right_back_wheel/velocity", right_back_wheel_velocity_);
+ register_input("/chassis/right_front_wheel/velocity", right_front_wheel_velocity_);
+
+ register_output("/chassis/velocity", chassis_velocity_, 0.0, 0.0, 0.0);
+ }
+
+ void update() override {
+ auto steering_status = calculate_steering_status();
+ auto wheel_velocities = calculate_wheel_velocities();
+ chassis_velocity_->vector = calculate_chassis_velocity(steering_status, wheel_velocities);
+ }
+
+private:
+ struct SteeringStatus {
+ Eigen::Vector4d angle, cos_angle, sin_angle;
+ };
+
+ SteeringStatus calculate_steering_status() const {
+ SteeringStatus steering_status;
+
+ steering_status.angle = {
+ *left_front_steering_angle_, //
+ *left_back_steering_angle_, //
+ *right_back_steering_angle_, //
+ *right_front_steering_angle_ //
+ };
+ steering_status.angle.array() -= std::numbers::pi / 4;
+ steering_status.cos_angle = steering_status.angle.array().cos();
+ steering_status.sin_angle = steering_status.angle.array().sin();
+
+ return steering_status;
+ }
+
+ Eigen::Vector4d calculate_wheel_velocities() const {
+ return {
+ *left_front_wheel_velocity_, //
+ *left_back_wheel_velocity_, //
+ *right_back_wheel_velocity_, //
+ *right_front_wheel_velocity_ //
+ };
+ }
+
+ Eigen::Vector3d calculate_chassis_velocity(
+ const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const {
+ Eigen::Vector3d velocity;
+ double one_quarter_r = wheel_radius_ / 4.0;
+ velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle);
+ velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle);
+ velocity.z() = -one_quarter_r / vehicle_radius_
+ * (-wheel_velocities[0] * steering_status.sin_angle[0]
+ + wheel_velocities[1] * steering_status.cos_angle[1]
+ + wheel_velocities[2] * steering_status.sin_angle[2]
+ - wheel_velocities[3] * steering_status.cos_angle[3]);
+ return velocity;
+ }
+
+ const double vehicle_radius_;
+ const double wheel_radius_;
+
+ InputInterface left_front_steering_angle_;
+ InputInterface left_back_steering_angle_;
+ InputInterface right_back_steering_angle_;
+ InputInterface right_front_steering_angle_;
+
+ InputInterface left_front_wheel_velocity_;
+ InputInterface left_back_wheel_velocity_;
+ InputInterface right_back_wheel_velocity_;
+ InputInterface right_front_wheel_velocity_;
+
+ OutputInterface chassis_velocity_;
+};
+
+} // namespace rmcs_core::controller::chassis
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(
+ rmcs_core::controller::chassis::SteeringWheelStatus, rmcs_executor::Component)
diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp
index 739b5859..d79d4115 100644
--- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp
+++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp
@@ -2,6 +2,7 @@
#include
#include
+#include
#include
#include
@@ -44,9 +45,9 @@ class DualYawController
register_output("/gimbal/top_yaw/control_torque", top_yaw_control_torque_, 0.0);
register_output("/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque_, 0.0);
-
- register_output("/gimbal/top_yaw/control_angle", top_yaw_control_torque_, 0.0);
- register_output("/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_torque_, 0.0);
+ register_output("/gimbal/top_yaw/control_angle", top_yaw_control_angle_, nan_);
+ register_output(
+ "/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_angle_shift_, nan_);
status_component_ =
create_partner_component(get_component_name() + "_status");
@@ -76,9 +77,13 @@ class DualYawController
if (std::isnan(*control_angle_shift_)) {
*top_yaw_control_angle_ = nan_;
*bottom_yaw_control_angle_shift_ = nan_;
+ top_yaw_encoder_angle_ = *top_yaw_control_angle_;
} else {
- *top_yaw_control_angle_ = 0.0;
+ *top_yaw_control_angle_ = top_yaw_encoder_angle_;
*bottom_yaw_control_angle_shift_ = *control_angle_shift_;
+ // RCLCPP_INFO(
+ // get_logger(), "top:%lf,bottom:%lf", *top_yaw_angle_,
+ // *bottom_yaw_control_angle_shift_);
}
}
@@ -110,6 +115,8 @@ class DualYawController
OutputInterface top_yaw_control_angle_;
OutputInterface bottom_yaw_control_angle_shift_;
+ double top_yaw_encoder_angle_;
+
class DualYawStatus : public rmcs_executor::Component {
public:
explicit DualYawStatus() {
diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp
index f20bd98f..0b781681 100644
--- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp
+++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp
@@ -1,5 +1,8 @@
+#include
+#include
#include
+#include
#include
#include
#include
@@ -8,7 +11,6 @@
#include
#include
-#include "controller/gimbal/precise_two_axis_gimbal_solver.hpp"
#include "controller/gimbal/two_axis_gimbal_solver.hpp"
namespace rmcs_core::controller::gimbal {
@@ -23,12 +25,9 @@ class HeroGimbalController
: Node(
get_component_name(),
rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true))
- , imu_gimbal_solver(
- *this, get_parameter("upper_limit").as_double(),
- get_parameter("lower_limit").as_double())
- , encoder_gimbal_solver(
- *this, get_parameter("upper_limit").as_double(),
- get_parameter("lower_limit").as_double()) {
+ , upper_limit_(get_parameter("upper_limit").as_double())
+ , lower_limit_(get_parameter("lower_limit").as_double())
+ , imu_gimbal_solver(*this, upper_limit_, lower_limit_) {
register_input("/remote/joystick/left", joystick_left_);
register_input("/remote/switch/left", switch_left_);
@@ -38,13 +37,14 @@ class HeroGimbalController
register_input("/remote/keyboard", keyboard_);
register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false);
+ register_input("/gimbal/pitch/angle", gimbal_pitch_angle_);
register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU);
register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_);
register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_);
register_output("/gimbal/yaw/control_angle_shift", yaw_control_angle_shift_, nan_);
- register_output("/gimbal/pitch/control_angle", pitch_control_angle_, nan_);
+ register_output("/gimbal/pitch/control_angle_shift", pitch_control_angle_shift_, nan_);
}
void update() override {
@@ -59,31 +59,30 @@ class HeroGimbalController
break;
}
- if (!last_keyboard_.q && keyboard_->q) {
+ if (!last_keyboard_.e && keyboard_->e) {
if (gimbal_mode_keyboard_ == GimbalMode::IMU)
gimbal_mode_keyboard_ = GimbalMode::ENCODER;
else
gimbal_mode_keyboard_ = GimbalMode::IMU;
}
- *gimbal_mode_ =
- *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_;
+
+ *gimbal_mode_ = gimbal_mode_keyboard_;
if (*gimbal_mode_ == GimbalMode::IMU) {
auto angle_error = update_imu_control();
*yaw_angle_error_ = angle_error.yaw_angle_error;
*pitch_angle_error_ = angle_error.pitch_angle_error;
- encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{});
*yaw_control_angle_shift_ = nan_;
- *pitch_control_angle_ = nan_;
+ *pitch_control_angle_shift_ = nan_;
} else {
imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{});
*yaw_angle_error_ = nan_;
*pitch_angle_error_ = nan_;
- auto control_angle = update_encoder_control();
- *yaw_control_angle_shift_ = control_angle.yaw_shift;
- *pitch_control_angle_ = control_angle.pitch_angle;
+ auto control_shift = update_encoder_control();
+ *yaw_control_angle_shift_ = control_shift.yaw_shift;
+ *pitch_control_angle_shift_ = control_shift.pitch_shift;
}
} while (false);
@@ -92,7 +91,6 @@ class HeroGimbalController
void reset_all_control() {
imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{});
- encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{});
gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU;
*gimbal_mode_ = rmcs_msgs::GimbalMode::IMU;
@@ -100,7 +98,7 @@ class HeroGimbalController
*yaw_angle_error_ = nan_;
*pitch_angle_error_ = nan_;
*yaw_control_angle_shift_ = nan_;
- *pitch_control_angle_ = nan_;
+ *pitch_control_angle_shift_ = nan_;
}
TwoAxisGimbalSolver::AngleError update_imu_control() {
@@ -121,27 +119,42 @@ class HeroGimbalController
double yaw_shift =
joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y();
double pitch_shift =
- -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x();
+ -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x();
return imu_gimbal_solver.update(
TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift});
}
- PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() {
- if (!encoder_gimbal_solver.enabled())
- return encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetControlPitch{-0.31});
+ struct EncoderControlShift {
+ double yaw_shift, pitch_shift;
+ };
- constexpr double joystick_sensitivity = 0.006 * 0.1;
+ EncoderControlShift update_encoder_control() const {
constexpr double mouse_yaw_sensitivity = 0.5 * 0.114;
constexpr double mouse_pitch_sensitivity = 0.5 * 0.095;
- double yaw_shift = joystick_sensitivity * joystick_left_->y()
- + mouse_yaw_sensitivity * mouse_velocity_->y();
- double pitch_shift = -joystick_sensitivity * joystick_left_->x()
- - mouse_pitch_sensitivity * mouse_velocity_->x();
+ EncoderControlShift control_shift;
+ control_shift.yaw_shift = mouse_yaw_sensitivity * mouse_velocity_->y();
+
+ double desired_pitch_shift = mouse_pitch_sensitivity * mouse_velocity_->x();
+ control_shift.pitch_shift = clamp_encoder_pitch_shift(desired_pitch_shift);
+ return control_shift;
+ }
+
+ double clamp_encoder_pitch_shift(double pitch_shift) const {
+ const double current_pitch_angle = *gimbal_pitch_angle_;
+ if (std::isnan(current_pitch_angle))
+ return nan_;
+
+ if (current_pitch_angle < upper_limit_) {
+ return std::clamp(pitch_shift, 0.0, lower_limit_ - current_pitch_angle);
+ }
+ if (current_pitch_angle > lower_limit_) {
+ return std::clamp(pitch_shift, upper_limit_ - current_pitch_angle, 0.0);
+ }
- return encoder_gimbal_solver.update(
- PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift});
+ return std::clamp(
+ pitch_shift, upper_limit_ - current_pitch_angle, lower_limit_ - current_pitch_angle);
}
private:
@@ -157,15 +170,16 @@ class HeroGimbalController
rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero();
InputInterface auto_aim_control_direction_;
+ InputInterface gimbal_pitch_angle_;
rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU;
OutputInterface gimbal_mode_;
+ const double upper_limit_, lower_limit_;
TwoAxisGimbalSolver imu_gimbal_solver;
- PreciseTwoAxisGimbalSolver encoder_gimbal_solver;
OutputInterface yaw_angle_error_, pitch_angle_error_;
- OutputInterface yaw_control_angle_shift_, pitch_control_angle_;
+ OutputInterface yaw_control_angle_shift_, pitch_control_angle_shift_;
};
} // namespace rmcs_core::controller::gimbal
@@ -173,4 +187,4 @@ class HeroGimbalController
#include
PLUGINLIB_EXPORT_CLASS(
- rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component)
\ No newline at end of file
+ rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component)
diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp
index 9a5c2948..3319011d 100644
--- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp
+++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp
@@ -25,8 +25,48 @@ class PreciseTwoAxisGimbalSolver {
: upper_limit_(upper_limit)
, lower_limit_(lower_limit) {
component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_);
+ component.register_input("/tf", tf_);
}
+ struct SetControlDirection : Operation {
+ friend class PreciseTwoAxisGimbalSolver;
+
+ explicit SetControlDirection(OdomImu::DirectionVector target)
+ : target_(std::move(target)) {}
+
+ private:
+ double update(PreciseTwoAxisGimbalSolver& super) const {
+ if (!super.tf_.ready()) {
+ super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_;
+ return PreciseTwoAxisGimbalSolver::nan_;
+ }
+
+ auto dir_yaw_link = fast_tf::cast(target_, *super.tf_);
+ Eigen::Vector3d dir = *dir_yaw_link;
+
+ const double norm = dir.norm();
+ if (norm < kEps) {
+ super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_;
+ return PreciseTwoAxisGimbalSolver::nan_;
+ }
+ dir /= norm;
+
+ const double xy_norm = std::hypot(dir.x(), dir.y());
+
+ // 在 YawLink 中，yaw 是相对当前总 yaw 的增量
+ const double yaw_shift = std::atan2(dir.y(), dir.x());
+
+ // 机械 pitch 约定：水平为 0，低头为正，所以是负号
+ const double pitch_angle = -std::atan2(dir.z(), std::max(xy_norm, kEps));
+
+ super.set_pitch_angle(pitch_angle);
+ return yaw_shift;
+ }
+
+ static constexpr double kEps = 1e-9;
+ OdomImu::DirectionVector target_;
+ };
+
struct SetDisabled : Operation {
friend class PreciseTwoAxisGimbalSolver;
@@ -91,6 +131,7 @@ class PreciseTwoAxisGimbalSolver {
static constexpr double nan_ = std::numeric_limits::quiet_NaN();
rmcs_executor::Component::InputInterface gimbal_pitch_angle_;
+ rmcs_executor::Component::InputInterface tf_;
const double upper_limit_, lower_limit_;
diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp
new file mode 100644
index 00000000..366f66b6
--- /dev/null
+++ b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp
@@ -0,0 +1,166 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+namespace rmcs_core::controller::pid {
+
+class FrictionWheelPidRecorder
+ : public rmcs_executor::Component
+ , public rclcpp::Node {
+public:
+ FrictionWheelPidRecorder()
+ : Node(
+ get_component_name(),
+ rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) {
+ const auto wheels =
+ declare_parameter>("wheels", std::vector{});
+ if (wheels.empty())
+ throw std::runtime_error("Parameter \"wheels\" must not be empty");
+
+ min_setpoint_abs_ = declare_parameter("min_setpoint_abs", 10.0);
+
+ const auto flush_every_n_samples = declare_parameter("flush_every_n_samples", 100);
+ if (flush_every_n_samples <= 0)
+ throw std::runtime_error("Parameter \"flush_every_n_samples\" must be positive");
+ flush_every_n_samples_ = static_cast(flush_every_n_samples);
+
+ const auto output_dir = std::filesystem::path{
+ declare_parameter("output_dir", "/tmp/friction_pid_logs")};
+ const auto output_name = declare_parameter("output_name", "");
+
+ wheel_count_ = wheels.size();
+ wheel_ids_ = std::make_unique(wheel_count_);
+ setpoints_ = std::make_unique[]>(wheel_count_);
+ measurements_ = std::make_unique[]>(wheel_count_);
+ control_torques_ = std::make_unique[]>(wheel_count_);
+ run_ids_ = std::make_unique(wheel_count_);
+ sample_indices_ = std::make_unique(wheel_count_);
+ previously_enabled_ = std::make_unique(wheel_count_);
+
+ for (size_t i = 0; i < wheel_count_; ++i) {
+ wheel_ids_[i] = wheel_id_from_topic(wheels[i]);
+ register_input(wheels[i] + "/control_velocity", setpoints_[i]);
+ register_input(wheels[i] + "/velocity", measurements_[i]);
+ register_input(wheels[i] + "/control_torque", control_torques_[i]);
+
+ run_ids_[i] = 0;
+ sample_indices_[i] = 0;
+ previously_enabled_[i] = false;
+ }
+
+ std::filesystem::create_directories(output_dir);
+ log_file_path_ = output_dir / resolve_output_name(output_name);
+ log_stream_.open(log_file_path_);
+ if (!log_stream_.is_open())
+ throw std::runtime_error(
+ "Failed to open recorder output file: " + log_file_path_.string());
+
+ log_stream_.setf(std::ios::fixed);
+ log_stream_.precision(9);
+ log_stream_
+ << "timestamp_us,wheel_id,run_id,sample_idx,setpoint_velocity,measured_velocity,"
+ "control_torque,enabled\n";
+
+ RCLCPP_INFO(get_logger(), "FrictionWheelPidRecorder writing to %s", log_file_path_.c_str());
+ }
+
+ ~FrictionWheelPidRecorder() override {
+ if (log_stream_.is_open()) {
+ log_stream_.flush();
+ log_stream_.close();
+ }
+ }
+
+ void update() override {
+ const auto timestamp_us = now_timestamp_us();
+
+ for (size_t i = 0; i < wheel_count_; ++i) {
+ const bool enabled = should_record(i);
+ if (enabled && !previously_enabled_[i]) {
+ ++run_ids_[i];
+ sample_indices_[i] = 0;
+ }
+
+ if (enabled) {
+ log_stream_ << timestamp_us << ',' << wheel_ids_[i] << ',' << run_ids_[i] << ','
+ << sample_indices_[i]++ << ',' << *setpoints_[i] << ','
+ << *measurements_[i] << ',' << *control_torques_[i] << ",1\n";
+ if (++unflushed_samples_ >= flush_every_n_samples_) {
+ log_stream_.flush();
+ unflushed_samples_ = 0;
+ }
+ }
+
+ previously_enabled_[i] = enabled;
+ }
+ }
+
+private:
+ static std::string wheel_id_from_topic(std::string_view topic) {
+ auto pos = topic.find_last_of('/');
+ if (pos == std::string_view::npos || pos + 1 >= topic.size())
+ return std::string(topic);
+ return std::string(topic.substr(pos + 1));
+ }
+
+ static uint64_t now_timestamp_us() {
+ const auto now = std::chrono::steady_clock::now().time_since_epoch();
+ return static_cast(
+ std::chrono::duration_cast(now).count());
+ }
+
+ static std::string resolve_output_name(const std::string& configured_name) {
+ if (!configured_name.empty())
+ return configured_name;
+
+ const auto now = std::chrono::system_clock::now().time_since_epoch();
+ const auto ms = std::chrono::duration_cast(now).count();
+ return "friction_wheel_pid_" + std::to_string(ms) + ".csv";
+ }
+
+ bool should_record(size_t index) const {
+ if (!setpoints_[index].ready() || !measurements_[index].ready()
+ || !control_torques_[index].ready())
+ return false;
+
+ const auto setpoint = *setpoints_[index];
+ const auto measurement = *measurements_[index];
+ const auto control_torque = *control_torques_[index];
+
+ return std::isfinite(setpoint) && std::isfinite(measurement)
+ && std::isfinite(control_torque) && std::abs(setpoint) >= min_setpoint_abs_;
+ }
+
+ size_t wheel_count_ = 0;
+ double min_setpoint_abs_ = 10.0;
+ size_t flush_every_n_samples_ = 100;
+ size_t unflushed_samples_ = 0;
+
+ std::filesystem::path log_file_path_;
+ std::ofstream log_stream_;
+
+ std::unique_ptr wheel_ids_;
+ std::unique_ptr[]> setpoints_;
+ std::unique_ptr[]> measurements_;
+ std::unique_ptr[]> control_torques_;
+ std::unique_ptr run_ids_;
+ std::unique_ptr sample_indices_;
+ std::unique_ptr previously_enabled_;
+};
+
+} // namespace rmcs_core::controller::pid
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(
+ rmcs_core::controller::pid::FrictionWheelPidRecorder, rmcs_executor::Component)
diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp
index bf3076e3..95d75cb9 100644
--- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp
+++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp
@@ -31,14 +31,15 @@ class BulletFeederController42mm
register_input(
"/gimbal/control_bullet_allowance/limited_by_heat",
control_bullet_allowance_limited_by_heat_);
+ register_input("/gimbal/bullet_fired", bullet_fired_);
- bullet_feeder_velocity_pid_.kp = 50.0;
- bullet_feeder_velocity_pid_.ki = 10.0;
- bullet_feeder_velocity_pid_.kd = 0.0;
+ bullet_feeder_velocity_pid_.kp = 50.0;
+ bullet_feeder_velocity_pid_.ki = 10.0;
+ bullet_feeder_velocity_pid_.kd = 0.0;
bullet_feeder_velocity_pid_.integral_max = 60.0;
bullet_feeder_velocity_pid_.integral_min = 0.0;
- bullet_feeder_angle_pid_.kp = 50.0;
+ bullet_feeder_angle_pid_.kp = 60.0;
bullet_feeder_angle_pid_.ki = 0.0;
bullet_feeder_angle_pid_.kd = 2.0;
@@ -51,9 +52,9 @@ class BulletFeederController42mm
void update() override {
const auto switch_right = *switch_right_;
- const auto switch_left = *switch_left_;
- const auto mouse = *mouse_;
- const auto keyboard = *keyboard_;
+ const auto switch_left = *switch_left_;
+ const auto mouse = *mouse_;
+ const auto keyboard = *keyboard_;
using namespace rmcs_msgs;
if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN)
@@ -61,7 +62,6 @@ class BulletFeederController42mm
reset_all_controls();
return;
}
-
overdrive_mode_ = keyboard.f;
if (keyboard.ctrl && !last_keyboard_.r && keyboard.r)
low_latency_mode_ = !low_latency_mode_;
@@ -84,8 +84,8 @@ class BulletFeederController42mm
} else {
if (!*friction_ready_ || std::isnan(bullet_feeder_control_angle_)) {
bullet_feeder_control_angle_ = *bullet_feeder_angle_;
- shoot_stage_ = ShootStage::PRELOADED;
- bullet_fed_count_ = static_cast(
+ shoot_stage_ = ShootStage::PRELOADED;
+ bullet_fed_count_ = static_cast(
(*bullet_feeder_angle_ - bullet_feeder_compressed_zero_point_ - 0.1)
/ bullet_feeder_angle_per_bullet_);
}
@@ -135,24 +135,24 @@ class BulletFeederController42mm
}
last_switch_right_ = switch_right;
- last_switch_left_ = switch_left;
- last_mouse_ = mouse;
- last_keyboard_ = keyboard;
+ last_switch_left_ = switch_left;
+ last_mouse_ = mouse;
+ last_keyboard_ = keyboard;
}
private:
void reset_all_controls() {
last_switch_right_ = rmcs_msgs::Switch::UNKNOWN;
- last_switch_left_ = rmcs_msgs::Switch::UNKNOWN;
- last_mouse_ = rmcs_msgs::Mouse::zero();
- last_keyboard_ = rmcs_msgs::Keyboard::zero();
+ last_switch_left_ = rmcs_msgs::Switch::UNKNOWN;
+ last_mouse_ = rmcs_msgs::Mouse::zero();
+ last_keyboard_ = rmcs_msgs::Keyboard::zero();
overdrive_mode_ = low_latency_mode_ = false;
- shoot_stage_ = ShootStage::PRELOADED;
+ shoot_stage_ = ShootStage::PRELOADED;
bullet_fed_count_ = std::numeric_limits::min();
- bullet_feeder_control_angle_ = nan_;
+ bullet_feeder_control_angle_ = nan_;
bullet_feeder_angle_pid_.output_max = inf_;
bullet_feeder_velocity_pid_.reset();
@@ -160,13 +160,13 @@ class BulletFeederController42mm
*bullet_feeder_control_torque_ = nan_;
bullet_feeder_faulty_count_ = 0;
- bullet_feeder_cool_down_ = 0;
+ bullet_feeder_cool_down_ = 0;
}
void set_preloading() {
RCLCPP_INFO(get_logger(), "PRELOADING");
bullet_fed_count_++;
- shoot_stage_ = ShootStage::PRELOADING;
+ shoot_stage_ = ShootStage::PRELOADING;
bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_
+ (bullet_fed_count_ + 0.5) * bullet_feeder_angle_per_bullet_;
bullet_feeder_angle_pid_.output_max = 1.0;
@@ -179,7 +179,7 @@ class BulletFeederController42mm
void set_compressing() {
RCLCPP_INFO(get_logger(), "COMPRESSING");
- shoot_stage_ = ShootStage::COMPRESSING;
+ shoot_stage_ = ShootStage::COMPRESSING;
bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_
+ (bullet_fed_count_ + 1) * bullet_feeder_angle_per_bullet_;
bullet_feeder_angle_pid_.output_max = 0.8;
@@ -192,7 +192,7 @@ class BulletFeederController42mm
void set_shooting() {
RCLCPP_INFO(get_logger(), "SHOOTING");
- shoot_stage_ = ShootStage::SHOOTING;
+ shoot_stage_ = ShootStage::SHOOTING;
bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_
+ (bullet_fed_count_ + 1.2) * bullet_feeder_angle_per_bullet_;
bullet_feeder_angle_pid_.output_max = 1.0;
@@ -215,7 +215,7 @@ class BulletFeederController42mm
void enter_jam_protection() {
bullet_feeder_control_angle_ = nan_;
- bullet_feeder_cool_down_ = 1000;
+ bullet_feeder_cool_down_ = 1000;
bullet_feeder_angle_pid_.reset();
bullet_feeder_velocity_pid_.reset();
RCLCPP_INFO(get_logger(), "Jammed!");
@@ -225,9 +225,10 @@ class BulletFeederController42mm
static constexpr double inf_ = std::numeric_limits::infinity();
static constexpr double bullet_feeder_compressed_zero_point_ = 0.58;
- static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6;
+ static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6;
InputInterface friction_ready_;
+ InputInterface bullet_fired_;
InputInterface switch_right_;
InputInterface switch_left_;
@@ -235,9 +236,9 @@ class BulletFeederController42mm
InputInterface keyboard_;
rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN;
- rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN;
- rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero();
- rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero();
+ rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN;
+ rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero();
+ rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero();
bool overdrive_mode_ = false, low_latency_mode_ = false;
@@ -247,8 +248,8 @@ class BulletFeederController42mm
InputInterface control_bullet_allowance_limited_by_heat_;
enum class ShootStage { PRELOADING, PRELOADED, COMPRESSING, COMPRESSED, SHOOTING };
- ShootStage shoot_stage_ = ShootStage::PRELOADED;
- int bullet_fed_count_ = std::numeric_limits::min();
+ ShootStage shoot_stage_ = ShootStage::PRELOADED;
+ int bullet_fed_count_ = std::numeric_limits::min();
double bullet_feeder_control_angle_ = nan_;
pid::PidCalculator bullet_feeder_velocity_pid_;
@@ -256,7 +257,7 @@ class BulletFeederController42mm
OutputInterface bullet_feeder_control_torque_;
int bullet_feeder_faulty_count_ = 0;
- int bullet_feeder_cool_down_ = 0;
+ int bullet_feeder_cool_down_ = 0;
OutputInterface shoot_mode_;
};
diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp
index a83dc5f4..4ebb69d6 100644
--- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp
+++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp
@@ -90,6 +90,9 @@ class FrictionWheelController
last_switch_left_ = switch_left;
last_keyboard_ = keyboard;
}
+ if (!friction_enabled_) {
+ reset_all_controls();
+ }
}
private:
@@ -150,7 +153,7 @@ class FrictionWheelController
bool detect_friction_faulty() {
for (size_t i = 0; i < friction_count_; i++) {
- if (*friction_velocities_[i] < *friction_control_velocities_[i] * 0.5)
+ if (abs(*friction_velocities_[i]) < abs(*friction_control_velocities_[i] * 0.5))
return true;
}
return false;
@@ -164,18 +167,18 @@ class FrictionWheelController
// The first friction wheel in the list is considered the primary one, meaning we only
// monitor the speed drop of this wheel to detect whether a bullet has been fired.
if (!std::isnan(last_primary_friction_velocity_)) {
- double differential = *friction_velocities_[0] - last_primary_friction_velocity_;
+ double differential = *friction_velocities_[2] - last_primary_friction_velocity_;
if (differential < 0.1)
primary_friction_velocity_decrease_integral_ += differential;
else {
if (primary_friction_velocity_decrease_integral_ < -14.0
- && last_primary_friction_velocity_ < friction_working_velocities_[0] - 20.0)
+ && last_primary_friction_velocity_ < friction_working_velocities_[2] - 25.0)
fired = true;
primary_friction_velocity_decrease_integral_ = 0;
}
}
- last_primary_friction_velocity_ = *friction_velocities_[0];
+ last_primary_friction_velocity_ = *friction_velocities_[2];
return fired;
}
diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp
index 3de0960a..e99b7bc4 100644
--- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp
+++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp
@@ -1,5 +1,7 @@
#include
+#include
+#include
#include
#include
@@ -23,17 +25,25 @@ class HeatController
register_output(
"/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_, 0);
+ register_output("/shoot/heat", shooting_heat_, 0.0);
}
void update() override {
shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_);
if (*bullet_fired_)
- shooter_heat_ += heat_per_shot + 10;
+ shooter_heat_ += heat_per_shot + *shooter_cooling_;
+ // if (count++ == 500) {
+ // RCLCPP_INFO(get_logger(), "limit:%ld,heat:%ld", *shooter_heat_limit_, shooter_heat_);
+ // count = 0;
+ // }
*control_bullet_allowance_ = std::max(
0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot);
+
+ *shooting_heat_ = static_cast(shooter_heat_);
}
+ // int count = 0;
private:
InputInterface shooter_cooling_;
@@ -45,6 +55,7 @@ class HeatController
const int64_t reserved_heat;
int64_t shooter_heat_ = 0;
+ OutputInterface shooting_heat_;
OutputInterface control_bullet_allowance_;
};
diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp
new file mode 100644
index 00000000..e46fcfb2
--- /dev/null
+++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp
@@ -0,0 +1,513 @@
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "controller/pid/pid_calculator.hpp"
+#include
+
+namespace rmcs_core::controller::shooting {
+
+/**
+ * @class PutterController
+ * @brief 推弹机构控制器
+ *
+ * 发射机制说明：
+ * 由于光电门放置于弹舱口，经测试，双中先触发推杆向后复位，然后堵转检测复位完毕，
+ * 默认情况下会给一点点的力保证推杆不会滑下去，全程使用角度环推进，利用灰度检测决定是否推弹
+ * 推杆检测发弹根据两部分来检测，一是摩擦轮，二是推杆的行程
+ * 整套方案以于暑假前完成压力测试
+ */
+class PutterController
+ : public rmcs_executor::Component
+ , public rclcpp::Node {
+public:
+ PutterController()
+ : Node(
+ get_component_name(),
+ rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) {
+
+ register_input("/remote/switch/right", switch_right_);
+ register_input("/remote/switch/left", switch_left_);
+ register_input("/remote/mouse", mouse_);
+ register_input("/remote/keyboard", keyboard_);
+
+ register_input("/gimbal/friction_ready", friction_ready_);
+
+ register_input("/gimbal/bullet_feeder/angle", bullet_feeder_angle_);
+ register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_);
+
+ register_input(
+ "/gimbal/control_bullet_allowance/limited_by_heat",
+ control_bullet_allowance_limited_by_heat_);
+
+ register_input("/gimbal/photoelectric_sensor", photoelectric_sensor_status_);
+ register_input("/gimbal/grayscale_sensor", grayscale_sensor_status_);
+ register_input("/gimbal/bullet_fired", bullet_fired_);
+
+ register_input("/gimbal/putter/angle", putter_angle_);
+ register_input("/gimbal/putter/velocity", putter_velocity_);
+
+ bullet_feeder_velocity_pid_.kp = 5.5;
+ bullet_feeder_velocity_pid_.ki = 1.1;
+ bullet_feeder_velocity_pid_.kd = 0.0;
+ bullet_feeder_velocity_pid_.integral_max = 60.0;
+ bullet_feeder_velocity_pid_.integral_min = 0.0;
+
+ bullet_feeder_angle_pid_.kp = 4.2;
+ bullet_feeder_angle_pid_.ki = 0.0;
+ bullet_feeder_angle_pid_.kd = 1.0;
+
+ putter_return_velocity_pid_.kp = 0.0015;
+ putter_return_velocity_pid_.ki = 0.00005;
+ putter_return_velocity_pid_.kd = 0.;
+ putter_return_velocity_pid_.integral_max = 0.;
+ putter_return_velocity_pid_.integral_min = -0.03;
+
+ putter_velocity_pid_.kp = 0.004;
+ putter_velocity_pid_.ki = 0.0001;
+ putter_velocity_pid_.kd = 0.001;
+ putter_velocity_pid_.integral_max = 0.03;
+ putter_velocity_pid_.integral_min = 0.;
+
+ putter_return_angle_pid.kp = 0.0001;
+ // putter_return_angle_pid.ki = 0.000001;
+ putter_return_angle_pid.kd = 0.;
+
+ register_output(
+ "/gimbal/bullet_feeder/control_torque", bullet_feeder_control_torque_, nan_);
+ register_output("/gimbal/putter/control_torque", putter_control_torque_, nan_);
+
+ register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE);
+ register_output("/gimbal/shooter/condiction", shoot_condiction_);
+
+ register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_);
+
+ // auto_aim
+ register_input("/gimbal/auto_aim/fire_control", fire_control_, false);
+ }
+
+ ~PutterController() {}
+
+ void update() override {
+ const auto switch_right = *switch_right_;
+ const auto switch_left = *switch_left_;
+ const auto mouse = *mouse_;
+ const auto keyboard = *keyboard_;
+
+ using namespace rmcs_msgs;
+
+ if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN)
+ || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) {
+ reset_all_controls();
+ return;
+ }
+
+ if (*friction_ready_) {
+ if (shoot_stage_ == ShootStage::RESETTING || bullet_feeder_cool_down_ > 0) {
+ *shoot_condiction_ = rmcs_msgs::ShootCondiction::JAM;
+ } else if (shoot_stage_ == ShootStage::PRELOADED) {
+ *shoot_condiction_ = rmcs_msgs::ShootCondiction::SHOOT;
+ } else if (shoot_stage_ == ShootStage::SHOOTING) {
+ *shoot_condiction_ = rmcs_msgs::ShootCondiction::FIRED;
+ } else {
+ *shoot_condiction_ = rmcs_msgs::ShootCondiction::PRELOADING;
+ }
+ } else {
+ *shoot_condiction_ = rmcs_msgs::ShootCondiction::FRICTION_WAITING;
+ }
+
+ // 推杆已初始化后的正常控制流程
+ if (putter_initialized) {
+ // 供弹轮卡弹保护冷却期间的处理
+ if (bullet_feeder_cool_down_ > 0) {
+ bullet_feeder_cool_down_--;
+
+ // 使用角度环反转到“后退一格”的位置以解除卡弹
+ double velocity_err = bullet_feeder_angle_pid_.update(
+ bullet_feeder_control_angle_ - *bullet_feeder_angle_)
+ - *bullet_feeder_velocity_;
+ *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_err);
+
+ if (!bullet_feeder_cool_down_) {
+ RCLCPP_INFO(get_logger(), "Jamming Solved, Retrying...");
+ set_resetting();
+ }
+ } else {
+ // 正常运行模式：摩擦轮就绪时才允许发射
+ if (*friction_ready_) {
+ // 发射触发检测
+ // RCLCPP_INFO(get_logger(), "%.2f", *bullet_feeder_angle_);
+ // RCLCPP_INFO(get_logger(), "%.2f", *bullet_feeder_velocity_);
+ if (switch_right != Switch::DOWN) {
+
+ const auto now = std::chrono::steady_clock::now();
+ const bool left_click_edge = (!last_mouse_.left && mouse.left);
+ if (left_click_edge) {
+ if (now - last_click_time_ < std::chrono::milliseconds(500)) {
+ click_count_++;
+ } else {
+ click_count_ = 1;
+ }
+ last_click_time_ = now;
+ }
+
+ const bool manual_trigger =
+ (!last_mouse_.left && mouse.left && !mouse.right)
+ || (last_switch_left_ == rmcs_msgs::Switch::MIDDLE
+ && switch_left == rmcs_msgs::Switch::DOWN);
+
+ // const bool auto_fire_now = (switch_right == Switch::UP) &&
+ // (*fire_control_);
+ const bool auto_fire_now =
+ (switch_right == Switch::UP || (mouse.right && mouse.left))
+ && (*fire_control_);
+
+ const bool auto_trigger_emergence = mouse.right && (click_count_ >= 2);
+
+ const bool auto_trigger =
+ auto_fire_now
+ && (now - last_fire_time_ > std::chrono::milliseconds(1000));
+
+ if (manual_trigger || auto_trigger || auto_trigger_emergence) {
+ if (*control_bullet_allowance_limited_by_heat_ > 0
+ && (shoot_stage_ == ShootStage::PRELOADED || shoot_first)) {
+ set_shooting();
+ last_fire_time_ = now;
+ shoot_first = false;
+ }
+ }
+ if (auto_trigger_emergence) {
+ click_count_ = 0;
+ }
+ }
+
+ if (shoot_stage_ == ShootStage::COMPRESSED) {
+ // 暂存模式：等待灰度传感器状态更新以判断是否完成推弹
+ if (*grayscale_sensor_status_) {
+ set_preloaded();
+ } else {
+ set_preloading();
+ }
+ }
+
+ if (shoot_stage_ == ShootStage::UPDATING) {
+ // 缓冲模式：使推杆和供弹盘不发生运动冲突
+ wait_bullet_ready();
+ }
+
+ if (shoot_stage_ == ShootStage::PRELOADING) {
+ // 盲拨模式：始终执行角度环定位
+ if (std::isnan(bullet_feeder_control_angle_)) {
+ bullet_feeder_control_angle_ =
+ *bullet_feeder_angle_ + bullet_feeder_angle_per_bullet_;
+ }
+
+ const auto angle_err = bullet_feeder_control_angle_ - *bullet_feeder_angle_;
+ if (angle_err < 0.1) {
+ set_compressed();
+ }
+ double velocity_err =
+ bullet_feeder_angle_pid_.update(angle_err) - *bullet_feeder_velocity_;
+ *bullet_feeder_control_torque_ =
+ bullet_feeder_velocity_pid_.update(velocity_err);
+
+ update_jam_detection();
+ } else if (
+ shoot_stage_ == ShootStage::SHOOTING
+ || shoot_stage_ == ShootStage::UPDATING) {
+ // 供弹轮不给力
+ bullet_feeder_velocity_pid_.reset();
+ bullet_feeder_angle_pid_.reset();
+ *bullet_feeder_control_torque_ = 0.;
+
+ } else if (shoot_stage_ == ShootStage::RESETTING) {
+
+ const auto angle_err = bullet_feeder_control_angle_ - *bullet_feeder_angle_;
+ if (angle_err < 0.1) {
+ RCLCPP_INFO(get_logger(), "RESETED");
+ set_compressed();
+ }
+ double velocity_err =
+ bullet_feeder_angle_pid_.update(angle_err) - *bullet_feeder_velocity_;
+ *bullet_feeder_control_torque_ =
+ bullet_feeder_velocity_pid_.update(velocity_err);
+
+ } else {
+ // 其他状态：角度环保持角度不变防止弹链退弹
+ double velocity_err =
+ bullet_feeder_angle_pid_.update(
+ bullet_feeder_control_angle_ - *bullet_feeder_angle_)
+ - *bullet_feeder_velocity_;
+ *bullet_feeder_control_torque_ =
+ bullet_feeder_velocity_pid_.update(velocity_err);
+ }
+
+ if (shoot_stage_ == ShootStage::SHOOTING) {
+ // 发射状态：检测子弹是否发出
+ if (*bullet_fired_
+ || *putter_angle_ - putter_startpoint >= putter_stroke_) {
+ shooted = true;
+ }
+
+ update_putter_jam_detection();
+
+ if (shooted) {
+ // 子弹已发出：推杆复位
+ const auto angle_err = putter_startpoint - *putter_angle_;
+ if (angle_err > -0.1) {
+ *putter_control_torque_ = 0.;
+ set_preloading();
+ shooted = false;
+ } else {
+ *putter_control_torque_ =
+ putter_return_velocity_pid_.update(-80. - *putter_velocity_);
+ putter_timeout_detection();
+ }
+ } else {
+ // 子弹未发出：继续推进
+ *putter_control_torque_ =
+ putter_return_velocity_pid_.update(60. - *putter_velocity_);
+ }
+ }
+
+ } else {
+ // 摩擦轮未就绪：停止供弹轮
+ *bullet_feeder_control_torque_ = 0.;
+ }
+
+ // 非发射状态：推杆给少许力保持位置
+ if (shoot_stage_ != ShootStage::SHOOTING)
+ *putter_control_torque_ = -0.02;
+ }
+ } else {
+ // 推杆未初始化：执行复位操作
+ *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_);
+ update_putter_jam_detection();
+ }
+
+ // 保存当前状态用于下次比较
+ last_switch_right_ = switch_right;
+ last_switch_left_ = switch_left;
+ last_mouse_ = mouse;
+ last_keyboard_ = keyboard;
+ }
+
+private:
+ void reset_all_controls() {
+ last_switch_right_ = rmcs_msgs::Switch::UNKNOWN;
+ last_switch_left_ = rmcs_msgs::Switch::UNKNOWN;
+ last_mouse_ = rmcs_msgs::Mouse::zero();
+ last_keyboard_ = rmcs_msgs::Keyboard::zero();
+
+ bullet_feeder_control_angle_ = nan_;
+ bullet_feeder_angle_pid_.output_max = inf_;
+ bullet_feeder_velocity_pid_.reset();
+ bullet_feeder_angle_pid_.reset();
+ *bullet_feeder_control_torque_ = nan_;
+
+ shoot_stage_ = ShootStage::PRELOADING;
+
+ putter_initialized = false;
+ putter_startpoint = nan_;
+ putter_return_velocity_pid_.reset();
+ putter_velocity_pid_.reset();
+ putter_return_angle_pid.reset();
+ *putter_control_torque_ = nan_;
+
+ bullet_feeder_faulty_count_ = 0;
+ bullet_feeder_cool_down_ = 0;
+
+ *shoot_delay_ms_ = nan_;
+ }
+
+ void set_resetting() {
+ shoot_stage_ = ShootStage::RESETTING;
+ bullet_feeder_reset();
+ }
+
+ void set_preloading() {
+ RCLCPP_INFO(get_logger(), "PRELOADING");
+ shoot_stage_ = ShootStage::PRELOADING;
+ // 盲拨方案：直接增加目标角度
+ if (!std::isnan(bullet_feeder_control_angle_)) {
+ bullet_feeder_control_angle_ += bullet_feeder_angle_per_bullet_;
+ }
+ }
+
+ void set_compressed() {
+ RCLCPP_INFO(get_logger(), "COMPRESSED");
+ shoot_stage_ = ShootStage::COMPRESSED;
+ }
+
+ void set_preloaded() {
+ RCLCPP_INFO(get_logger(), "PRELOADED");
+ shoot_stage_ = ShootStage::PRELOADED;
+ }
+
+ void set_shooting() {
+ RCLCPP_INFO(get_logger(), "SHOOTING");
+ shoot_stage_ = ShootStage::SHOOTING;
+ }
+
+ void set_updating() { shoot_stage_ = ShootStage::UPDATING; }
+
+ void wait_bullet_ready() {
+ if (bullet_ready_count_ >= 1000) {
+ bullet_ready_count_ = 0;
+ set_preloading();
+ } else {
+ bullet_ready_count_++;
+ }
+ }
+
+ void update_jam_detection() {
+ if (*bullet_feeder_control_torque_ < 33.0 || std::isnan(*bullet_feeder_control_torque_)) {
+ bullet_feeder_faulty_count_ = 0;
+ return;
+ }
+
+ // 扭矩持续过大时累计故障计数
+ if (bullet_feeder_faulty_count_ < 1000)
+ bullet_feeder_faulty_count_++;
+ else {
+ bullet_feeder_faulty_count_ = 0;
+ RCLCPP_WARN(get_logger(), "Jam Detected! Reversing 60 degrees...");
+ enter_jam_protection();
+ }
+ }
+
+ void update_putter_jam_detection() {
+ if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING)
+ || (*putter_control_torque_ < 0.05 && shoot_stage_ == ShootStage::SHOOTING)
+ || std::isnan(*putter_control_torque_)) {
+ putter_faulty_count_ = 0;
+ return;
+ }
+
+ // 扭矩异常时累计故障计数
+ if (putter_faulty_count_ < 500)
+ ++putter_faulty_count_;
+ else {
+ putter_faulty_count_ = 0;
+ if (shoot_stage_ != ShootStage::SHOOTING) {
+ // 非发射状态下检测到堵转：推杆已到位，设置为已初始化
+ putter_initialized = true;
+ putter_startpoint = *putter_angle_;
+ } else {
+ // 发射状态下检测到堵转：认为子弹已发出
+ shooted = true;
+ }
+ }
+ }
+
+ void putter_timeout_detection() {
+ // 推杆在发射状态下长时间不推出，认为已经完成推杆，直接进入下一个状态
+ if (shoot_stage_ == ShootStage::SHOOTING) {
+ if (shooted) {
+ if (putter_timeout_count_ < 1600)
+ ++putter_timeout_count_;
+ else {
+ putter_timeout_count_ = 0;
+ set_preloading();
+ shooted = false;
+ }
+ }
+ }
+ }
+
+ void enter_jam_protection() {
+ // 设置目标角度为当前角度后退 60 度
+ bullet_feeder_control_angle_ = *bullet_feeder_angle_ - bullet_feeder_angle_per_bullet_;
+ bullet_feeder_cool_down_ = 1000;
+ bullet_feeder_angle_pid_.reset();
+ bullet_feeder_velocity_pid_.reset();
+ RCLCPP_INFO(get_logger(), "Jammed!");
+ }
+
+ void bullet_feeder_reset() {
+ double pi = std::numbers::pi;
+ double reference_angle = -2 * pi;
+ while (std::abs(reference_angle - *bullet_feeder_angle_) > pi / 6) {
+ reference_angle += (pi / 3);
+ }
+ bullet_feeder_control_angle_ = reference_angle;
+ }
+
+ static constexpr double nan_ = std::numeric_limits::quiet_NaN(); ///< 非数值常量
+ static constexpr double inf_ = std::numeric_limits::infinity(); ///< 无穷大常量
+
+ static constexpr double putter_stroke_ = 11.5; ///< 推杆行程长度
+
+ static constexpr double max_bullet_feeder_control_torque_ = 0.1;
+ static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6;
+
+ InputInterface photoelectric_sensor_status_;
+ InputInterface grayscale_sensor_status_;
+ InputInterface bullet_fired_;
+ bool shooted{false};
+ bool shoot_first{true};
+
+ InputInterface friction_ready_;
+
+ InputInterface switch_right_;
+ InputInterface switch_left_;
+ InputInterface mouse_;
+ InputInterface keyboard_;
+
+ rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN;
+ rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN;
+ rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero();
+ rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero();
+
+ InputInterface bullet_feeder_angle_;
+ InputInterface bullet_feeder_velocity_;
+
+ InputInterface control_bullet_allowance_limited_by_heat_;
+
+ bool putter_initialized = false;
+ int putter_faulty_count_ = 0;
+ int putter_timeout_count_ = 0;
+ double putter_startpoint = nan_;
+ pid::PidCalculator putter_return_velocity_pid_;
+ InputInterface putter_velocity_;
+
+ pid::PidCalculator putter_velocity_pid_;
+
+ enum class ShootStage { RESETTING, PRELOADING, COMPRESSED, PRELOADED, SHOOTING, UPDATING };
+ ShootStage shoot_stage_ = ShootStage::PRELOADING;
+ double bullet_feeder_control_angle_ = nan_;
+
+ pid::PidCalculator bullet_feeder_velocity_pid_;
+ pid::PidCalculator bullet_feeder_angle_pid_;
+ OutputInterface bullet_feeder_control_torque_;
+
+ InputInterface putter_angle_;
+ pid::PidCalculator putter_return_angle_pid;
+ OutputInterface putter_control_torque_;
+
+ int bullet_feeder_faulty_count_ = 0;
+ int bullet_feeder_cool_down_ = 0;
+ int bullet_ready_count_ = 0;
+
+ OutputInterface shoot_mode_;
+ OutputInterface shoot_condiction_;
+
+ OutputInterface shoot_delay_ms_;
+
+ InputInterface fire_control_;
+ std::chrono::steady_clock::time_point last_fire_time_{};
+ std::chrono::steady_clock::time_point last_click_time_{};
+ int click_count_ = 0;
+};
+
+} // namespace rmcs_core::controller::shooting
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component)
\ No newline at end of file
diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp
index 5500b15c..80358510 100644
--- a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp
+++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp
@@ -1,7 +1,10 @@
+#include
+#include
#include
#include
#include
#include
+#include
namespace rmcs_core::controller::shooting {
@@ -18,40 +21,67 @@ class ShootingRecorder
log_mode_ = static_cast(get_parameter("log_mode").as_int());
+ aim_velocity = get_parameter("aim_velocity").as_double();
+
register_input("/referee/shooter/initial_speed", initial_speed_);
register_input("/referee/shooter/shoot_timestamp", shoot_timestamp_);
- register_input("/friction_wheels/temperature", fractional_temperature_);
-
- if (friction_wheel_count_ == 2) {
- const auto topic = std::array{
- "/gimbal/left_friction/control_velocity",
- "/gimbal/right_friction/control_velocity",
- };
- for (int i = 0; i < 2; i++)
- register_input(topic[i], friction_wheels_velocity_[i]);
- } else if (friction_wheel_count_ == 4) {
- const auto topic = std::array{
- "/gimbal/first_left_friction/control_velocity",
- "/gimbal/first_right_friction/control_velocity",
- "/gimbal/second_left_friction/control_velocity",
- "/gimbal/second_right_friction/control_velocity",
- };
- for (int i = 0; i < 4; i++)
- register_input(topic[i], friction_wheels_velocity_[i]);
- }
+
+ // if (friction_wheel_count_ == 4) {
+ // const auto topic = std::array{
+ // "/gimbal/first_left_friction/control_velocity",
+ // "/gimbal/first_right_friction/control_velocity",
+ // };
+ // for (int i = 0; i < 2; i++)
+ // register_input(topic[i], friction_wheels_velocity_[i]);
+ // register_input("/gimbal/first_left_friction/velocity", friction_velocities_[0]);
+ // register_input("/gimbal/first_right_friction/velocity", friction_velocities_[1]);
+ // }
using namespace std::chrono;
auto now = high_resolution_clock::now();
- auto ms = duration_cast(now.time_since_epoch()).count();
+ auto ms = duration_cast(now.time_since_epoch()).count();
auto file = fmt::format("/robot_shoot/{}.log", ms);
log_stream_.open(file);
+
+ std::ofstream outFile("shoot_recorder");
+ RCLCPP_INFO(get_logger(), "ShootingRecorder initialized, log file: %s", file.c_str());
}
~ShootingRecorder() { log_stream_.close(); }
void update() override {
+ // if (*friction_velocities_[0] <= 366.0 && *friction_velocities_[1] <= 366.0)
+ // return;
+ // if (start_time_ == 0.0) {
+ // start_time_ = GetTime();
+ // }
+
+ // int flag = 0;
+
+ // if (GetTime() - start_time_ > 10.0) {
+ // if (flag == 0) {
+ // RCLCPP_INFO(get_logger(), "vv = %f", vv);
+ // flag = 1;
+ // }
+ // return;
+ // }
+
+ // vv = std::max(vv, abs(*friction_velocities_[0] + *friction_velocities_[1]));
+
+ // auto log_text = std::string{};
+ // log_text = fmt::format(
+ // "{:.3f}, {:.3f}, {:.3f}", *friction_velocities_[0], *friction_velocities_[1],
+ // (*friction_velocities_[0] + *friction_velocities_[1]));
+ // log_stream_ << log_text << std::endl;
+ // RCLCPP_INFO(get_logger(), "%s", log_text.c_str());
+
+ // std::ofstream outFile("shoot_recorder", std::ios::app);
+ // if (outFile.is_open()) {
+ // outFile << log_text << std::endl;
+ // outFile.close();
+ // } 评估摩擦轮质量
switch (log_mode_) {
case LogMode::TRIGGER:
// It will be triggered by shooting action
@@ -64,38 +94,50 @@ class ShootingRecorder
return;
break;
}
+ v = *shoot_timestamp_;
+
+ static constexpr size_t max_velocities_size = 1000;
+
+ velocities.push_back(*initial_speed_);
+ if (velocities.size() > max_velocities_size) {
+ velocities.erase(velocities.begin());
+ }
- auto log_text = std::string{};
+ analysis3();
+
+ auto log_text = std::string{};
auto timestamp = timestamp_to_string(*shoot_timestamp_);
- if (friction_wheel_count_ == 2) {
- log_text = fmt::format(
- "{},{},{},{},{}", timestamp, *initial_speed_, //
- *friction_wheels_velocity_[0], *friction_wheels_velocity_[1],
- *fractional_temperature_);
- } else if (friction_wheel_count_ == 4) {
+ if (friction_wheel_count_ == 4) {
log_text = fmt::format(
- "{},{},{},{},{},{},{}", timestamp, *initial_speed_, //
- *friction_wheels_velocity_[0], *friction_wheels_velocity_[1],
- *friction_wheels_velocity_[2], *friction_wheels_velocity_[3],
- *fractional_temperature_);
+ "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_,
+ (int)velocities.size(), //
+ velocity_, excellence_rate_, pass_rate_, range_, range2_, velocity_max,
+ velocity_min);
}
log_stream_ << log_text << std::endl;
- RCLCPP_INFO(get_logger(), "%s", log_text.c_str());
+ RCLCPP_INFO(get_logger(), "%s", log_text.c_str()); // 记录每次射击的初速度和统计数据
+
+ log_velocity = fmt::format("{:.3f}", *initial_speed_);
+ std::ofstream outFile("shoot_recorder", std::ios::app);
+ if (outFile.is_open()) {
+ outFile << log_velocity << std::endl;
+ outFile.close();
+ }
last_shoot_timestamp_ = *shoot_timestamp_;
}
private:
/// @brief Component interface
+ std::array, 2> friction_velocities_;
+
InputInterface initial_speed_;
InputInterface shoot_timestamp_;
- InputInterface fractional_temperature_;
-
std::size_t friction_wheel_count_ = 2;
- std::array, 4> friction_wheels_velocity_;
+ std::array, 2> friction_wheels_velocity_;
/// @brief For log
enum class LogMode { TRIGGER = 1, TIMING = 2 };
@@ -103,25 +145,149 @@ class ShootingRecorder
double last_shoot_timestamp_ = 0;
std::ofstream log_stream_;
+ std::string log_velocity;
std::size_t log_count_ = 0;
+ std::vector velocities;
+
+ double velocity_;
+
+ double excellence_rate_;
+ double pass_rate_;
+
+ double range_;
+ double range2_;
+
+ double velocity_min;
+ double velocity_max;
+
+ double v;
+ double aim_velocity;
+
private:
static std::string timestamp_to_string(double timestamp) {
- auto time = static_cast(timestamp);
+ auto time = static_cast(timestamp);
auto local_time = std::localtime(&time);
char buffer[100];
std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", local_time);
double fractional_seconds = timestamp - std::floor(timestamp);
- int milliseconds = static_cast(fractional_seconds * 1000);
+ int milliseconds = static_cast(fractional_seconds * 1000);
char result[150];
std::snprintf(result, sizeof(result), "%s.%03d", buffer, milliseconds);
return result;
}
+ void analysis1() {
+ double sum = 0.0;
+ for (const auto& v : velocities) {
+ sum += v;
+ }
+ velocity_ = sum / double(velocities.size());
+
+ sort(velocities.begin(), velocities.end());
+
+ range_ = velocities.back() - velocities.front();
+
+ if (velocities.size() >= 3) {
+ range2_ = velocities[int(velocities.size() - 2)] - velocities[1];
+ } else {
+ range2_ = range_;
+ }
+
+ velocity_max = velocities.back();
+ velocity_min = velocities.front();
+
+ int excellence_count = 0;
+ int pass_count = 0;
+ for (int i = 0; i < int(velocities.size()); i++) {
+ if (velocities[i] >= velocity_ - 0.1 && velocities[i] <= velocity_ + 0.1) {
+ pass_count += 1;
+ }
+ if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) {
+ excellence_count += 1;
+ }
+ }
+ excellence_rate_ = double(excellence_count) / double(velocities.size());
+ pass_rate_ = double(pass_count) / double(velocities.size());
+ }
+
+ void analysis2() {
+ double sum = 0.0;
+ for (const auto& v : velocities) {
+ sum += v;
+ }
+
+ sort(velocities.begin(), velocities.end());
+
+ velocity_max = velocities.back();
+ velocity_min = velocities.front();
+
+ int n_adjust = std::max(1, int((int)velocities.size() * 0.05));
+
+ for (int i = 0; i < n_adjust; i++) {
+ sum -= velocities[i];
+ sum -= velocities[velocities.size() - 1 - i];
+ }
+
+ velocity_ = sum / double(velocities.size() - 2 * n_adjust);
+
+ range_ = velocities.back() - velocities.front();
+ range2_ = velocities[int(velocities.size() - 2)] - velocities[1];
+
+ int excellence_count = 0;
+ int pass_count = 0;
+ for (int i = 0; i < int(velocities.size()); i++) {
+ if (velocities[i] >= velocity_ - 0.1 && velocities[i] <= velocity_ + 0.1) {
+ pass_count += 1;
+ }
+ if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) {
+ excellence_count += 1;
+ }
+ }
+ excellence_rate_ = double(excellence_count) / double(velocities.size());
+ pass_rate_ = double(pass_count) / double(velocities.size());
+ }
+
+ void analysis3() {
+ double sum = 0.0;
+ for (const auto& v : velocities) {
+ sum += v;
+ }
+ velocity_ = sum / double(velocities.size());
+
+ int excellence_count = 0;
+ int pass_count = 0;
+
+ for (const auto& v : velocities) {
+ if (v >= aim_velocity - 0.05 && v <= aim_velocity + 0.05) {
+ excellence_count += 1;
+ }
+ if (v >= aim_velocity - 0.1 && v <= aim_velocity + 0.1) {
+ pass_count += 1;
+ }
+ }
+ excellence_rate_ = double(excellence_count) / double(velocities.size());
+ pass_rate_ = double(pass_count) / double(velocities.size());
+
+ sort(velocities.begin(), velocities.end());
+ velocity_max = velocities.back();
+ velocity_min = velocities.front();
+
+ range_ = velocities.back() - velocities.front();
+ range2_ = velocities[int(velocities.size() - 2)] - velocities[1];
+ }
+
+ static double GetTime() {
+ using namespace std::chrono;
+ static auto start = high_resolution_clock::now();
+ auto now = high_resolution_clock::now();
+ duration elapsed = now - start;
+ return elapsed.count();
+ }
};
} // namespace rmcs_core::controller::shooting
diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp
index c11061c2..e02e85eb 100644
--- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp
+++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp
@@ -69,15 +69,13 @@ class LkMotor {
multi_turn_encoder_count_ = 0;
last_raw_angle_ = 0;
- double current_max;
double torque_constant;
double reduction_ratio;
switch (config.motor_type) {
case Type::kMG5010Ei10:
raw_angle_modulus_ = 1 << 16;
- current_max = 33.0;
- torque_constant = 0.90909;
+ torque_constant = 0.1;
reduction_ratio = 10.0;
// Note: max_torque_ should represent the ACTUAL maximum torque of the motor.
@@ -89,21 +87,18 @@ class LkMotor {
break;
case Type::kMG4010Ei10:
raw_angle_modulus_ = 1 << 16;
- current_max = 33.0;
torque_constant = 0.07;
reduction_ratio = 10.0;
max_torque_ = 4.5;
break;
case Type::kMG6012Ei8:
raw_angle_modulus_ = 1 << 16;
- current_max = 33.0;
- torque_constant = 1.09;
+ torque_constant = 1.09 / 8.0;
reduction_ratio = 8.0;
max_torque_ = 16.0;
break;
case Type::kMG4005Ei10:
raw_angle_modulus_ = 1 << 16;
- current_max = 33.0;
torque_constant = 0.06;
reduction_ratio = 10.0;
max_torque_ = 2.5;
@@ -125,7 +120,7 @@ class LkMotor {
velocity_to_command_velocity_coefficient_ = sign * reduction_ratio * kRadToDeg * 100.0;
status_current_to_torque_coefficient_ =
- sign * (current_max / kRawCurrentMax) * torque_constant * reduction_ratio;
+ sign * (kProtocolCurrentMax / kRawCurrentMax) * torque_constant * reduction_ratio;
torque_to_command_current_coefficient_ = 1 / status_current_to_torque_coefficient_;
*max_torque_output_ = max_torque();
@@ -485,7 +480,9 @@ class LkMotor {
// Limits
static constexpr double kNan = std::numeric_limits::quiet_NaN();
+ // LK protocol maps the raw current field range [-2048, 2048] to [-33A, 33A].
static constexpr int kRawCurrentMax = 2048;
+ static constexpr double kProtocolCurrentMax = 33.0;
int raw_angle_modulus_;
// Constants
diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp
new file mode 100644
index 00000000..5a1e461a
--- /dev/null
+++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp
@@ -0,0 +1,799 @@
+#include
+#include
+#include
+#include
+#include
+#include