From 2c622fd0d17c1294f77808adb30dd24075f57bb6 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Sun, 28 Apr 2024 19:50:05 +0200 Subject: [PATCH] Added Fossen PID tuning --- .../pid_controller/pid_controller_node.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py index 6c7d0123..4c3e187c 100644 --- a/motion/pid_controller/pid_controller/pid_controller_node.py +++ b/motion/pid_controller/pid_controller/pid_controller_node.py @@ -23,7 +23,8 @@ def __init__(self): parameters=[ ('pid_controller.Kp', [1.0, 1.0, 1.0]), ('pid_controller.Ki', [1.0, 1.0, 1.0]), - ('pid_controller.Kd', [1.0, 1.0, 1.0]) + ('pid_controller.Kd', [1.0, 1.0, 1.0]), + ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65]) ]) self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) @@ -33,6 +34,17 @@ def __init__(self): Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value + M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value + + M = np.reshape(M, (3, 3)) + M_diag = np.diag(M) + + ## PID TUNING VALUES ## (OVERWRITES YAML FILE VALUES) + omega_n = 1.2 + zeta = 0.75 + Kp = M_diag * omega_n**2 + Kd = M_diag * 2 * zeta * omega_n #- D_diag + Ki = omega_n/10 * Kp self.pid = PID(Kp, Ki, Kd)