From 8c78b898611652af4badb2db025f85c0a501deb2 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 9 Jul 2026 17:11:29 +0000 Subject: [PATCH 1/4] Initial plan From 215b9d440faf0fa0cb16288e17814ba96b8cf749 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 9 Jul 2026 17:15:20 +0000 Subject: [PATCH 2/4] Rewrite exc_ff_pid as velocity controller --- config/vgoswec_0_exc_ff_pid.yaml | 22 +++++------ config/vgoswec_45_exc_ff_pid.yaml | 24 ++++++----- docs/CONTROLLERS.md | 19 +++++---- docs/MPC_TODO.md | 4 +- src/active_pto.cpp | 28 ++++++------- src/active_pto.h | 28 +++++++------ src/config_loader.cpp | 12 +++--- src/config_loader.h | 12 +++--- src/demo_vgoswec.cpp | 20 +++++----- src/excitation_force_provider.h | 2 +- src/pid_controller.h | 4 +- src/rsda_pto_functor.cpp | 2 +- tests/smoke_test.cpp | 66 ++++++++++++++++++++++++++++--- 13 files changed, 155 insertions(+), 88 deletions(-) diff --git a/config/vgoswec_0_exc_ff_pid.yaml b/config/vgoswec_0_exc_ff_pid.yaml index 2ebe866..483fbc1 100644 --- a/config/vgoswec_0_exc_ff_pid.yaml +++ b/config/vgoswec_0_exc_ff_pid.yaml @@ -1,9 +1,9 @@ # vgoswec_0_exc_ff_pid.yaml # Source: Ogden et al., ASME JOMAE 145(3):030905 (VGOSWEC-0 / VGM-0). # VGM-0 free-decay reference: ωn = 1.07 rad/s, Ts = 5.86 s. -# Active control: Excitation-force feedforward + PID trajectory tracking. +# Active control: excitation-force feedforward + inner velocity PID. # -# τ = α · F_exc,pitch(t) + PID( θ_ref(t) - θ(t) ) +# τ = -(ff_gain · F_exc,pitch(t) + PID_vel(alpha · F_exc,pitch(t) - θ̇(t))) simulation: duration: 60.0 @@ -40,12 +40,12 @@ wave: controller: type: exc_ff_pid exc_ff_pid: - alpha: 1.0 # feedforward gain on F_exc TODO: tune with tank-test data - theta_ref: 0.0 # [rad] reference angle (upright equilibrium) - pid: - kp: 0.5 # [N·m/rad] TODO: tune with tank-test data - ki: 0.05 # [N·m/(rad·s)] TODO: tune with tank-test data - kd: 0.05 # [N·m·s/rad] TODO: tune with tank-test data - tau_d: 0.02 # [s] derivative filter time constant (≈ 3× dt) - u_min: -5.0 # [N·m] output clamp - u_max: 5.0 # [N·m] output clamp + alpha: 0.05 # vel_ref = alpha * F_exc [(rad/s)/(N·m)] (~1/(2·B_eff)); TODO tune + ff_gain: 0.5 # feedforward torque = ff_gain * F_exc [-] (Korde F_exc/2); TODO tune + vel_pid: + kp: 1.0 # [N·m per (rad/s)] TODO tune + ki: 0.0 # TODO tune + kd: 0.0 # TODO tune + tau_d: 0.02 # [s] derivative filter time constant + u_min: -5.0 # [N·m] output clamp + u_max: 5.0 # [N·m] output clamp diff --git a/config/vgoswec_45_exc_ff_pid.yaml b/config/vgoswec_45_exc_ff_pid.yaml index 3ec6d19..811d790 100644 --- a/config/vgoswec_45_exc_ff_pid.yaml +++ b/config/vgoswec_45_exc_ff_pid.yaml @@ -1,15 +1,13 @@ # vgoswec_45_exc_ff_pid.yaml # Source: Ogden et al., ASME JOMAE 145(3):030905 (VGOSWEC-45 / VGM-45). # VGM-45 free-decay reference: ωn = 1.84 rad/s, Ts = 3.42 s. -# Active control: Excitation-force feedforward + PID trajectory tracking. +# Active control: excitation-force feedforward + inner velocity PID. # -# τ = α · F_exc,pitch(t) + PID( θ_ref(t) - θ(t) ) +# τ = -(ff_gain · F_exc,pitch(t) + PID_vel(alpha · F_exc,pitch(t) - θ̇(t))) # # The feedforward term uses the actual wave excitation torque (pitch DOF) # extracted from HydroForces::Evaluate per-component at each timestep. # This implements active WEC phase-anticipation. -# -# Default θ_ref = 0 (regulate flap to upright; feedforward does anticipation). simulation: duration: 60.0 @@ -54,12 +52,12 @@ wave: controller: type: exc_ff_pid exc_ff_pid: - alpha: 1.0 # feedforward gain on F_exc TODO: tune with tank-test data - theta_ref: 0.0 # [rad] reference angle (upright equilibrium) - pid: - kp: 0.5 # [N·m/rad] TODO: tune with tank-test data - ki: 0.05 # [N·m/(rad·s)] TODO: tune with tank-test data - kd: 0.05 # [N·m·s/rad] TODO: tune with tank-test data - tau_d: 0.02 # [s] derivative filter time constant (≈ 3× dt) - u_min: -5.0 # [N·m] output clamp - u_max: 5.0 # [N·m] output clamp + alpha: 0.05 # vel_ref = alpha * F_exc [(rad/s)/(N·m)] (~1/(2·B_eff)); TODO tune + ff_gain: 0.5 # feedforward torque = ff_gain * F_exc [-] (Korde F_exc/2); TODO tune + vel_pid: + kp: 1.0 # [N·m per (rad/s)] TODO tune + ki: 0.0 # TODO tune + kd: 0.0 # TODO tune + tau_d: 0.02 # [s] derivative filter time constant + u_min: -5.0 # [N·m] output clamp + u_max: 5.0 # [N·m] output clamp diff --git a/docs/CONTROLLERS.md b/docs/CONTROLLERS.md index 8236423..e55b423 100644 --- a/docs/CONTROLLERS.md +++ b/docs/CONTROLLERS.md @@ -79,23 +79,28 @@ CC control requires bidirectional power flow. A physical PTO must support reacti --- -## (D) ExcitationFeedforwardPID +## (D) ExcitationVelocityController (`exc_ff_pid`) ### Formula ``` -τ = α · F_exc,pitch(t) + PID(θ_ref − θ) +vel_ref = α · F_exc,pitch(t) +τ_cmd = ff_gain · F_exc,pitch(t) + PID_vel(vel_ref − θ̇) +τ_pto = −τ_cmd ``` ### Sub-components -- **Feedforward** (`α · F_exc`): uses real-time wave excitation torque from `ExcitationForceProvider`. Requires `HydroSystem::SetPerComponentCaptureEnabled(true)`. -- **PID** (`PID(θ_ref − θ)`): full PID with filtered derivative (time constant τ_d) and anti-windup back-calculation. Keeps flap near `θ_ref` (default: 0 = upright). +- **Velocity reference** (`α · F_exc`): sets a phase-aligned target velocity from real-time wave excitation torque. +- **Feedforward torque** (`ff_gain · F_exc`): uses real-time wave excitation torque from `ExcitationForceProvider`. Requires `HydroSystem::SetPerComponentCaptureEnabled(true)`. +- **Velocity PID** (`PID_vel(vel_ref − θ̇)`): full PID with filtered derivative (time constant τ_d) and anti-windup back-calculation. Regulates flap velocity, not flap position. ### PID parameters | Name | Default | Notes | |------|---------|-------| -| `kp` | 0.5 | N·m/rad | -| `ki` | 0.05 | N·m/(rad·s) | -| `kd` | 0.05 | N·m·s/rad | +| `alpha` | 0.05 | (rad/s)/(N·m) | +| `ff_gain` | 0.5 | direct feedforward torque scale | +| `kp` | 1.0 | N·m per (rad/s) | +| `ki` | 0.0 | N·m/rad | +| `kd` | 0.0 | N·m·s/(rad/s) | | `tau_d` | 0.02 s | ≈ 4× timestep | | `u_min/u_max` | ±5 N·m | Saturation clamp | diff --git a/docs/MPC_TODO.md b/docs/MPC_TODO.md index 877347d..cbc177f 100644 --- a/docs/MPC_TODO.md +++ b/docs/MPC_TODO.md @@ -4,7 +4,7 @@ Future extension: Model Predictive Control (MPC) for the VGOSWEC-45. ## Why MPC -The current ExcitationFeedforwardPID assumes the excitation force is known +The current ExcitationVelocityController assumes the excitation force is known perfectly at each instant (from HydroForces::Evaluate) but uses it reactively. True optimal WEC control requires **preview** of future excitation — MPC provides this while respecting torque, angle, and power constraints. @@ -50,7 +50,7 @@ Add `--controller mpc` to CLI and `vgoswec_45_mpc.yaml` config. - [ ] Implement linear state-space discretization - [ ] Implement causal excitation predictor (Kalman filter on wave elevation) - [ ] Implement QP formulation and OSQP wrapper -- [ ] Validate against ExcitationFeedforwardPID in simulation +- [ ] Validate against ExcitationVelocityController in simulation - [ ] Tank-test comparison ## References diff --git a/src/active_pto.cpp b/src/active_pto.cpp index 985bf7f..4bca77b 100644 --- a/src/active_pto.cpp +++ b/src/active_pto.cpp @@ -33,25 +33,25 @@ double ComplexConjugateControl::ComputeForce(double disp, double vel, double /*t return std::clamp(-K_r_ * disp - B_r_ * vel, -clip_, clip_); } -// ─── (D) ExcitationFeedforwardPID ──────────────────────────────────────────── +// ─── (D) ExcitationVelocityController ──────────────────────────────────────── -ExcitationFeedforwardPID::ExcitationFeedforwardPID( +ExcitationVelocityController::ExcitationVelocityController( std::shared_ptr src, double alpha, - std::unique_ptr pid, - double theta_ref) + double ff_gain, + std::unique_ptr pid) : f_exc_source_(std::move(src)), alpha_(alpha), - pid_(std::move(pid)), - theta_ref_(theta_ref) { - pid_->SetSetpoint(theta_ref_); -} - -double ExcitationFeedforwardPID::ComputeForce(double disp, double vel, double t) { - (void)vel; - const double tau_ff = alpha_ * f_exc_source_->GetLatestExcitationTorque(); - const double tau_pid = pid_->Compute(disp, t); - return tau_ff + tau_pid; + ff_gain_(ff_gain), + pid_(std::move(pid)) {} + +double ExcitationVelocityController::ComputeForce(double disp, double vel, double t) { + (void)disp; + const double f_exc = f_exc_source_->GetLatestExcitationTorque(); + pid_->SetSetpoint(alpha_ * f_exc); + const double tau_ff = ff_gain_ * f_exc; + const double tau_pid = pid_->Compute(vel, t); + return -(tau_ff + tau_pid); } } // namespace vgoswec diff --git a/src/active_pto.h b/src/active_pto.h index 84c4d2e..e2dc35d 100644 --- a/src/active_pto.h +++ b/src/active_pto.h @@ -8,6 +8,8 @@ // │ returned value = PTO torque about hinge Y-axis [N·m] │ // │ Positive torque OPPOSES positive θ (restoring convention) │ // │ Absorbed power: P_abs = −τ_pto · ω (positive = extracted from waves)│ +// │ Active vel control demand: ff_gain·F_exc + PID(alpha·F_exc − θ̇) │ +// │ Returned active-control torque: τ_pto = −(demand) │ // └──────────────────────────────────────────────────────────────────────────┘ // ============================================================================= #pragma once @@ -69,30 +71,34 @@ class ComplexConjugateControl : public seastack::pto::IPTOModel { }; // ============================================================================= -// (D) ExcitationFeedforwardPID — active WEC control -// τ = α · F_exc,pitch(t) + PID( θ_ref − θ ) +// (D) ExcitationVelocityController — active WEC control +// vel_ref = α · F_exc,pitch(t) +// τ_cmd = ff_gain · F_exc,pitch(t) + PID( vel_ref − θ̇ ) +// τ_pto = −τ_cmd // // The feedforward term uses the actual wave excitation torque broadcast by // ExcitationForceProvider (updated from HydroForces::Evaluate per_component). -// The PID term regulates the flap toward θ_ref (default = 0, upright). +// The PID closes an inner velocity loop, driving flap velocity to track a +// phase-aligned reference proportional to excitation torque. // -// This implements phase anticipation: α·F_exc advances the flap to absorb -// energy while the PID damps deviations from the reference trajectory. +// Under this file's sign convention, positive returned torque opposes +// positive θ, so the applied PTO torque is the negative of the active control +// effort above. // ============================================================================= -class ExcitationFeedforwardPID : public seastack::pto::IPTOModel { +class ExcitationVelocityController : public seastack::pto::IPTOModel { public: - ExcitationFeedforwardPID(std::shared_ptr src, - double alpha, - std::unique_ptr pid, - double theta_ref = 0.0); + ExcitationVelocityController(std::shared_ptr src, + double alpha, + double ff_gain, + std::unique_ptr pid); double ComputeForce(double disp, double vel, double t) override; private: std::shared_ptr f_exc_source_; double alpha_; + double ff_gain_; std::unique_ptr pid_; - double theta_ref_; }; } // namespace vgoswec diff --git a/src/config_loader.cpp b/src/config_loader.cpp index d269fb9..b707a0f 100644 --- a/src/config_loader.cpp +++ b/src/config_loader.cpp @@ -46,9 +46,9 @@ BodyConfig ParseBody(const YAML::Node& n) { PIDConfig ParsePID(const YAML::Node& n) { PIDConfig p; - p.kp = ReadOpt(n, "kp", 0.5); - p.ki = ReadOpt(n, "ki", 0.05); - p.kd = ReadOpt(n, "kd", 0.05); + p.kp = ReadOpt(n, "kp", 1.0); + p.ki = ReadOpt(n, "ki", 0.0); + p.kd = ReadOpt(n, "kd", 0.0); p.tau_d = ReadOpt(n, "tau_d", 0.02); p.u_min = ReadOpt(n, "u_min", -5.0); p.u_max = ReadOpt(n, "u_max", 5.0); @@ -77,9 +77,9 @@ ControllerConfig ParseController(const YAML::Node& n) { } if (n["exc_ff_pid"]) { const auto& p = n["exc_ff_pid"]; - ctrl.exc_ff_pid.alpha = ReadOpt(p, "alpha", 1.0); - ctrl.exc_ff_pid.theta_ref = ReadOpt(p, "theta_ref", 0.0); - if (p["pid"]) ctrl.exc_ff_pid.pid = ParsePID(p["pid"]); + ctrl.exc_ff_pid.alpha = ReadOpt(p, "alpha", 0.05); + ctrl.exc_ff_pid.ff_gain = ReadOpt(p, "ff_gain", 0.5); + if (p["vel_pid"]) ctrl.exc_ff_pid.vel_pid = ParsePID(p["vel_pid"]); } return ctrl; } diff --git a/src/config_loader.h b/src/config_loader.h index e76e19f..22126e0 100644 --- a/src/config_loader.h +++ b/src/config_loader.h @@ -42,18 +42,18 @@ struct CCConfig { }; struct PIDConfig { - double kp{0.5}; - double ki{0.05}; - double kd{0.05}; + double kp{1.0}; + double ki{0.0}; + double kd{0.0}; double tau_d{0.02}; ///< [s] derivative filter time constant double u_min{-5.0}; ///< [N·m] double u_max{5.0}; ///< [N·m] }; struct ExcFFPIDConfig { - double alpha{1.0}; ///< feedforward gain on F_exc - double theta_ref{0.0}; ///< [rad] reference angle - PIDConfig pid; + double alpha{0.05}; ///< [(rad/s)/(N·m)] velocity reference gain, vel_ref = alpha * F_exc + double ff_gain{0.5}; ///< [-] direct feedforward torque gain, tau_ff = ff_gain * F_exc + PIDConfig vel_pid; ///< Velocity-loop PID gains/output clamp }; struct ControllerConfig { diff --git a/src/demo_vgoswec.cpp b/src/demo_vgoswec.cpp index 373cf57..cd0cc99 100644 --- a/src/demo_vgoswec.cpp +++ b/src/demo_vgoswec.cpp @@ -191,18 +191,20 @@ static std::shared_ptr BuildController( if (type == "exc_ff_pid") { vgoswec::PIDParams p{}; - p.kp = cfg.controller.exc_ff_pid.pid.kp; - p.ki = cfg.controller.exc_ff_pid.pid.ki; - p.kd = cfg.controller.exc_ff_pid.pid.kd; - p.tau_d = cfg.controller.exc_ff_pid.pid.tau_d; - p.u_min = cfg.controller.exc_ff_pid.pid.u_min; - p.u_max = cfg.controller.exc_ff_pid.pid.u_max; + p.kp = cfg.controller.exc_ff_pid.vel_pid.kp; + p.ki = cfg.controller.exc_ff_pid.vel_pid.ki; + p.kd = cfg.controller.exc_ff_pid.vel_pid.kd; + p.tau_d = cfg.controller.exc_ff_pid.vel_pid.tau_d; + p.u_min = cfg.controller.exc_ff_pid.vel_pid.u_min; + p.u_max = cfg.controller.exc_ff_pid.vel_pid.u_max; p.dt_expected = cfg.timestep; auto pid = std::make_unique(p); - return std::make_shared( - exc_provider, cfg.controller.exc_ff_pid.alpha, std::move(pid), - cfg.controller.exc_ff_pid.theta_ref); + return std::make_shared( + exc_provider, + cfg.controller.exc_ff_pid.alpha, + cfg.controller.exc_ff_pid.ff_gain, + std::move(pid)); } throw std::runtime_error("Unknown controller type: " + type); diff --git a/src/excitation_force_provider.h b/src/excitation_force_provider.h index 0d9b57e..6fd8d00 100644 --- a/src/excitation_force_provider.h +++ b/src/excitation_force_provider.h @@ -7,7 +7,7 @@ // std::vector per_comp; // hydro_forces.Evaluate(state, time, &per_comp); // exc_provider.Update(per_comp); -// // ... ExcitationFeedforwardPID reads GetLatestExcitationTorque() +// // ... ExcitationVelocityController reads GetLatestExcitationTorque() // // DOF convention (BEMIO / WEC-Sim): // 0 = surge (Fx), 1 = sway (Fy), 2 = heave (Fz), diff --git a/src/pid_controller.h b/src/pid_controller.h index fd988eb..8c77e12 100644 --- a/src/pid_controller.h +++ b/src/pid_controller.h @@ -32,7 +32,7 @@ struct PIDParams { /// - Anti-windup via back-calculation (integrator only advances when unsaturated) /// - Output clamp [u_min, u_max] /// -/// Units: measurement [rad], output [N·m] (rotational PTO use case). +/// Units: measurement/setpoint are application-defined, output is [N·m]. class PIDController : public seastack::control::IController { public: explicit PIDController(const PIDParams& params); @@ -42,7 +42,7 @@ class PIDController : public seastack::control::IController { double GetSetpoint() const { return setpoint_; } /// Compute control output. - /// @param measurement Current flap angle θ [rad] + /// @param measurement Current measured process variable /// @param time Current simulation time [s] (used to compute dt) /// @return PTO torque command [N·m] double Compute(double measurement, double time) override; diff --git a/src/rsda_pto_functor.cpp b/src/rsda_pto_functor.cpp index f4f6160..dee1c6d 100644 --- a/src/rsda_pto_functor.cpp +++ b/src/rsda_pto_functor.cpp @@ -10,7 +10,7 @@ double RsdaPtoFunctor::evaluate(double time, double /*rest_angle*/, double angle const ::chrono::ChLinkRSDA& /*link*/) { // angle -> flap deviation theta [rad] (displacement); vel -> theta_dot [rad/s]. // rest_angle is unused: the controllers reference theta from the initialized-at-rest - // configuration (rest_angle = 0), matching theta_ref = 0 in the exc_ff_pid controller. + // configuration (rest_angle = 0); active controllers use angle/velocity directly. return model_->ComputeForce(angle, vel, time); } diff --git a/tests/smoke_test.cpp b/tests/smoke_test.cpp index da28843..2a00ac5 100644 --- a/tests/smoke_test.cpp +++ b/tests/smoke_test.cpp @@ -6,6 +6,8 @@ #include #include +#include +#include #include "config_loader.h" #include "pid_controller.h" @@ -88,9 +90,9 @@ TEST(OptimalPassive, BasicDamping) { EXPECT_NEAR(op.ComputeForce(0.0, 3.0, 0.0), -6.0, 1e-12); } -// ─── ExcitationFeedforwardPID tests ────────────────────────────────────────── +// ─── ExcitationVelocityController tests ────────────────────────────────────── -TEST(ExcitationFeedforwardPID, FeedforwardOnly) { +TEST(ExcitationVelocityController, FeedforwardOnlyUsesActiveSignConvention) { auto exc = std::make_shared(0, 4); exc->UpdateDirect(2.0, 0.0); // F_exc = 2.0 N·m @@ -98,10 +100,64 @@ TEST(ExcitationFeedforwardPID, FeedforwardOnly) { p.kp = 0.0; p.ki = 0.0; p.kd = 0.0; p.tau_d = 0.01; p.u_min = -100.0; p.u_max = 100.0; auto pid = std::make_unique(p); - vgoswec::ExcitationFeedforwardPID ff_pid(exc, /*alpha=*/0.5, std::move(pid)); + vgoswec::ExcitationVelocityController controller( + exc, /*alpha=*/0.5, /*ff_gain=*/0.5, std::move(pid)); - // τ = alpha * F_exc + PID(0) = 0.5 * 2.0 + 0 = 1.0 - EXPECT_NEAR(ff_pid.ComputeForce(0.0, 0.0, 0.0), 1.0, 1e-9); + // Applied PTO torque follows the repo's restoring sign convention: + // τ_pto = -(ff_gain * F_exc + 0) = -1.0 N·m. + EXPECT_NEAR(controller.ComputeForce(0.0, 0.0, 0.0), -1.0, 1e-9); +} + +TEST(ExcitationVelocityController, TracksVelocityErrorNotPosition) { + auto exc = std::make_shared(0, 4); + exc->UpdateDirect(2.0, 0.0); // vel_ref = alpha * F_exc = 1.0 rad/s + + vgoswec::PIDParams p; + p.kp = 2.0; p.ki = 0.0; p.kd = 0.0; + p.tau_d = 0.01; p.u_min = -100.0; p.u_max = 100.0; + auto pid = std::make_unique(p); + vgoswec::ExcitationVelocityController controller( + exc, /*alpha=*/0.5, /*ff_gain=*/0.5, std::move(pid)); + + // error = vel_ref - vel = 1.0 - 0.25 = 0.75 + // tau_cmd = ff_gain * F_exc + kp * error = 1.0 + 1.5 = 2.5 + // tau_pto = -tau_cmd = -2.5 + EXPECT_NEAR(controller.ComputeForce(/*disp=*/99.0, /*vel=*/0.25, 0.0), -2.5, 1e-9); +} + +TEST(ConfigLoader, ExcitationVelocityControllerSchema) { + const auto cfg_path = + (std::filesystem::temp_directory_path() / "vgoswec_exc_ff_pid_test.yaml").string(); + std::ofstream cfg(cfg_path); + ASSERT_TRUE(cfg.is_open()); + cfg << "hydro:\n" + " h5_file: hydroData/test.h5\n" + "controller:\n" + " type: exc_ff_pid\n" + " exc_ff_pid:\n" + " alpha: 0.05\n" + " ff_gain: 0.5\n" + " vel_pid:\n" + " kp: 1.25\n" + " ki: 0.1\n" + " kd: 0.2\n" + " tau_d: 0.03\n" + " u_min: -4.0\n" + " u_max: 6.0\n"; + cfg.close(); + + const auto loaded = vgoswec::LoadConfig(cfg_path); + EXPECT_EQ(loaded.controller.type, "exc_ff_pid"); + EXPECT_DOUBLE_EQ(loaded.controller.exc_ff_pid.alpha, 0.05); + EXPECT_DOUBLE_EQ(loaded.controller.exc_ff_pid.ff_gain, 0.5); + EXPECT_DOUBLE_EQ(loaded.controller.exc_ff_pid.vel_pid.kp, 1.25); + EXPECT_DOUBLE_EQ(loaded.controller.exc_ff_pid.vel_pid.ki, 0.1); + EXPECT_DOUBLE_EQ(loaded.controller.exc_ff_pid.vel_pid.kd, 0.2); + EXPECT_DOUBLE_EQ(loaded.controller.exc_ff_pid.vel_pid.tau_d, 0.03); + EXPECT_DOUBLE_EQ(loaded.controller.exc_ff_pid.vel_pid.u_min, -4.0); + EXPECT_DOUBLE_EQ(loaded.controller.exc_ff_pid.vel_pid.u_max, 6.0); + + std::filesystem::remove(cfg_path); } int main(int argc, char** argv) { From a7d18d5bd26c15ef1501a2b142fb3678a90bd9c9 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 9 Jul 2026 17:17:17 +0000 Subject: [PATCH 3/4] Clarify velocity controller intent --- src/active_pto.cpp | 2 +- tests/smoke_test.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/active_pto.cpp b/src/active_pto.cpp index 4bca77b..2a0536e 100644 --- a/src/active_pto.cpp +++ b/src/active_pto.cpp @@ -46,7 +46,7 @@ ExcitationVelocityController::ExcitationVelocityController( pid_(std::move(pid)) {} double ExcitationVelocityController::ComputeForce(double disp, double vel, double t) { - (void)disp; + (void)disp; // Inner velocity loop only: no outer position/displacement regulation. const double f_exc = f_exc_source_->GetLatestExcitationTorque(); pid_->SetSetpoint(alpha_ * f_exc); const double tau_ff = ff_gain_ * f_exc; diff --git a/tests/smoke_test.cpp b/tests/smoke_test.cpp index 2a00ac5..d3f9bcd 100644 --- a/tests/smoke_test.cpp +++ b/tests/smoke_test.cpp @@ -122,6 +122,7 @@ TEST(ExcitationVelocityController, TracksVelocityErrorNotPosition) { // error = vel_ref - vel = 1.0 - 0.25 = 0.75 // tau_cmd = ff_gain * F_exc + kp * error = 1.0 + 1.5 = 2.5 // tau_pto = -tau_cmd = -2.5 + // Use a nonzero displacement sentinel to confirm the velocity controller ignores it. EXPECT_NEAR(controller.ComputeForce(/*disp=*/99.0, /*vel=*/0.25, 0.0), -2.5, 1e-9); } From 970ea619f67e100ac2591332eda5f37fe824e106 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 9 Jul 2026 17:18:46 +0000 Subject: [PATCH 4/4] Polish velocity controller docs --- config/vgoswec_0_exc_ff_pid.yaml | 4 ++-- config/vgoswec_45_exc_ff_pid.yaml | 4 ++-- docs/CONTROLLERS.md | 4 ++-- tests/smoke_test.cpp | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/config/vgoswec_0_exc_ff_pid.yaml b/config/vgoswec_0_exc_ff_pid.yaml index 483fbc1..e8a389b 100644 --- a/config/vgoswec_0_exc_ff_pid.yaml +++ b/config/vgoswec_0_exc_ff_pid.yaml @@ -40,8 +40,8 @@ wave: controller: type: exc_ff_pid exc_ff_pid: - alpha: 0.05 # vel_ref = alpha * F_exc [(rad/s)/(N·m)] (~1/(2·B_eff)); TODO tune - ff_gain: 0.5 # feedforward torque = ff_gain * F_exc [-] (Korde F_exc/2); TODO tune + alpha: 0.05 # vel_ref = alpha * F_exc [(rad/s)/(N·m)] (~1/(2·B_eff), B_eff = effective hinge radiation damping); TODO tune + ff_gain: 0.5 # feedforward torque = ff_gain * F_exc [-] (default half-excitation feedforward, i.e. the Korde F_exc/2 term); TODO tune vel_pid: kp: 1.0 # [N·m per (rad/s)] TODO tune ki: 0.0 # TODO tune diff --git a/config/vgoswec_45_exc_ff_pid.yaml b/config/vgoswec_45_exc_ff_pid.yaml index 811d790..57dc79f 100644 --- a/config/vgoswec_45_exc_ff_pid.yaml +++ b/config/vgoswec_45_exc_ff_pid.yaml @@ -52,8 +52,8 @@ wave: controller: type: exc_ff_pid exc_ff_pid: - alpha: 0.05 # vel_ref = alpha * F_exc [(rad/s)/(N·m)] (~1/(2·B_eff)); TODO tune - ff_gain: 0.5 # feedforward torque = ff_gain * F_exc [-] (Korde F_exc/2); TODO tune + alpha: 0.05 # vel_ref = alpha * F_exc [(rad/s)/(N·m)] (~1/(2·B_eff), B_eff = effective hinge radiation damping); TODO tune + ff_gain: 0.5 # feedforward torque = ff_gain * F_exc [-] (default half-excitation feedforward, i.e. the Korde F_exc/2 term); TODO tune vel_pid: kp: 1.0 # [N·m per (rad/s)] TODO tune ki: 0.0 # TODO tune diff --git a/docs/CONTROLLERS.md b/docs/CONTROLLERS.md index e55b423..2b849fa 100644 --- a/docs/CONTROLLERS.md +++ b/docs/CONTROLLERS.md @@ -99,8 +99,8 @@ vel_ref = α · F_exc,pitch(t) | `alpha` | 0.05 | (rad/s)/(N·m) | | `ff_gain` | 0.5 | direct feedforward torque scale | | `kp` | 1.0 | N·m per (rad/s) | -| `ki` | 0.0 | N·m/rad | -| `kd` | 0.0 | N·m·s/(rad/s) | +| `ki` | 0.0 | integral gain on velocity error | +| `kd` | 0.0 | derivative gain on velocity error | | `tau_d` | 0.02 s | ≈ 4× timestep | | `u_min/u_max` | ±5 N·m | Saturation clamp | diff --git a/tests/smoke_test.cpp b/tests/smoke_test.cpp index d3f9bcd..49dcb14 100644 --- a/tests/smoke_test.cpp +++ b/tests/smoke_test.cpp @@ -104,7 +104,7 @@ TEST(ExcitationVelocityController, FeedforwardOnlyUsesActiveSignConvention) { exc, /*alpha=*/0.5, /*ff_gain=*/0.5, std::move(pid)); // Applied PTO torque follows the repo's restoring sign convention: - // τ_pto = -(ff_gain * F_exc + 0) = -1.0 N·m. + // τ_pto = -(ff_gain * F_exc + 0) = -1.0 N·m (PID disabled via zero gains). EXPECT_NEAR(controller.ComputeForce(0.0, 0.0, 0.0), -1.0, 1e-9); } @@ -120,7 +120,7 @@ TEST(ExcitationVelocityController, TracksVelocityErrorNotPosition) { exc, /*alpha=*/0.5, /*ff_gain=*/0.5, std::move(pid)); // error = vel_ref - vel = 1.0 - 0.25 = 0.75 - // tau_cmd = ff_gain * F_exc + kp * error = 1.0 + 1.5 = 2.5 + // tau_cmd = (0.5 * 2.0) + (2.0 * 0.75) = 1.0 + 1.5 = 2.5 // tau_pto = -tau_cmd = -2.5 // Use a nonzero displacement sentinel to confirm the velocity controller ignores it. EXPECT_NEAR(controller.ComputeForce(/*disp=*/99.0, /*vel=*/0.25, 0.0), -2.5, 1e-9);