piper_client/control/controller.rs
1//! Controller trait - 控制器通用接口
2//!
3//! 定义了所有控制器必须实现的接口。
4//!
5//! # 设计理念
6//!
7//! - **Tick 模式**: 用户控制循环，控制器只负责计算
8//! - **时间感知**: 显式传入 `dt`，便于单元测试
9//! - **类型安全**: 使用强类型单位（`Rad`, `NewtonMeter`）
10//! - **错误处理**: 关联类型 `Error` 允许自定义错误
11//!
12//! # 时间跳变处理
13//!
14//! 当检测到异常的 `dt` 时（如系统卡顿、线程调度延迟），
15//! `on_time_jump()` 方法会被调用。
16//!
17//! ⚠️ **重要**: 对于时间敏感的控制器（如 PID），**必须**覆盖此方法：
18//!
19//! - ✅ **必须重置**: 微分项（D term），防止计算出巨大的导数
20//! - ❌ **不要清零**: 积分项（I term），否则机械臂会瞬间失去抗重力能力
21//!
22//! # 示例
23//!
24//! ```rust,no_run
25//! use piper_client::control::Controller;
26//! use piper_client::types::{JointArray, Rad, NewtonMeter};
27//! use std::time::Duration;
28//!
29//! struct MyController {
30//! target: JointArray<Rad>,
31//! last_error: JointArray<f64>,
32//! }
33//!
34//! impl Controller for MyController {
35//! type Error = std::io::Error;
36//!
37//! fn tick(
38//! &mut self,
39//! current: &JointArray<Rad>,
40//! dt: Duration,
41//! ) -> Result<JointArray<NewtonMeter>, Self::Error> {
42//! // 计算控制输出
43//! let error = self.target.map_with(*current, |t, c| t - c);
44//! let output = error.map(|e| NewtonMeter(e.0 * 10.0)); // 简单 P 控制
45//! self.last_error = error.map(|e| e.0);
46//! Ok(output)
47//! }
48//!
49//! fn on_time_jump(&mut self, _dt: Duration) -> Result<(), Self::Error> {
50//! // ✅ 重置微分项相关状态
51//! self.last_error = JointArray::from([0.0; 6]);
52//! // ❌ 不要清零积分项！
53//! Ok(())
54//! }
55//! }
56//! ```
57
58use crate::types::{JointArray, NewtonMeter, Rad};
59use std::time::Duration;
60
61/// 控制器通用接口
62///
63/// 所有控制器都必须实现此 trait。
64///
65/// # 生命周期
66///
67/// - **初始化**: 在控制器构造时设置目标、参数
68/// - **运行**: 循环调用 `tick()`，传入当前状态和时间步长
69/// - **异常**: 当 `dt` 异常时，调用 `on_time_jump()`
70/// - **清理**: 控制器 `Drop` 时自动清理
71///
72/// # 线程安全
73///
74/// `Controller` 本身不要求 `Send` 或 `Sync`。
75/// 如果需要在多线程中使用，请将其包装在 `Mutex` 中。
76pub trait Controller {
77 /// 控制器错误类型
78 ///
79 /// 允许每个控制器定义自己的错误类型。
80 type Error: std::error::Error + Send + 'static;
81
82 /// 计算一步控制输出
83 ///
84 /// # 参数
85 ///
86 /// - `current`: 当前关节位置
87 /// - `dt`: 时间步长（自上次 `tick` 以来的时间）
88 ///
89 /// # 返回
90 ///
91 /// - `Ok(output)`: 关节力矩命令
92 /// - `Err(e)`: 控制器内部错误
93 ///
94 /// # 注意
95 ///
96 /// - `dt` 可能会被钳位（clamped），不一定等于实际时间
97 /// - 如果 `dt` 被钳位，`on_time_jump()` 会先被调用
98 /// - 输出力矩应该被钳位到安全范围内
99 ///
100 /// # 示例
101 ///
102 /// ```rust,no_run
103 /// # use piper_client::control::Controller;
104 /// # use piper_client::types::{JointArray, Rad, NewtonMeter};
105 /// # use std::time::Duration;
106 /// # struct MyController;
107 /// # impl Controller for MyController {
108 /// # type Error = std::io::Error;
109 /// fn tick(
110 /// &mut self,
111 /// current: &JointArray<Rad>,
112 /// dt: Duration,
113 /// ) -> Result<JointArray<NewtonMeter>, Self::Error> {
114 /// // 1. 计算误差
115 /// let error = self.compute_error(current);
116 ///
117 /// // 2. 更新内部状态（积分、微分等）
118 /// self.update_state(&error, dt);
119 ///
120 /// // 3. 计算输出
121 /// let output = self.compute_output(&error, dt);
122 ///
123 /// // 4. 钳位输出到安全范围
124 /// Ok(output.map(|t| t.clamp(NewtonMeter(-50.0), NewtonMeter(50.0))))
125 /// }
126 /// # fn on_time_jump(&mut self, _dt: Duration) -> Result<(), Self::Error> { Ok(()) }
127 /// # }
128 /// # impl MyController {
129 /// # fn compute_error(&self, _: &JointArray<Rad>) -> JointArray<f64> { JointArray::from([0.0; 6]) }
130 /// # fn update_state(&mut self, _: &JointArray<f64>, _: Duration) {}
131 /// # fn compute_output(&self, _: &JointArray<f64>, _: Duration) -> JointArray<NewtonMeter> { JointArray::from([NewtonMeter(0.0); 6]) }
132 /// # }
133 /// ```
134 fn tick(
135 &mut self,
136 current: &JointArray<Rad>,
137 dt: Duration,
138 ) -> Result<JointArray<NewtonMeter>, Self::Error>;
139
140 /// 处理时间跳变
141 ///
142 /// 当检测到 `dt` 超过预期（通常是由于系统卡顿、线程调度延迟等），
143 /// 此方法会被调用，允许控制器重置或调整内部状态。
144 ///
145 /// # 参数
146 ///
147 /// - `dt`: 实际经过的时间（未钳位前）
148 ///
149 /// # 默认实现
150 ///
151 /// 默认实现不做任何事情（`Ok(())`）。
152 ///
153 /// # ⚠️ 重要提示
154 ///
155 /// 对于 **时间敏感** 的控制器（如 PID），**强烈建议** 覆盖此方法：
156 ///
157 /// ## ✅ 应该重置的状态
158 ///
159 /// - **微分项（D term）**: `last_error` 等用于计算导数的状态
160 /// - 原因：大的 `dt` 会导致 `(error - last_error) / dt` 计算错误
161 ///
162 /// ## ❌ 不应该清零的状态
163 ///
164 /// - **积分项（I term）**: 累积误差
165 /// - 原因：机械臂可能依赖积分项对抗重力
166 /// - 后果：清零会导致机械臂瞬间下坠（Sagging）
167 ///
168 /// # 示例
169 ///
170 /// ```rust,no_run
171 /// # use piper_client::control::Controller;
172 /// # use piper_client::types::{JointArray, Rad, NewtonMeter};
173 /// # use std::time::Duration;
174 /// struct PidController {
175 /// integral: JointArray<f64>, // 积分项
176 /// last_error: JointArray<f64>, // 用于计算微分
177 /// }
178 ///
179 /// impl Controller for PidController {
180 /// type Error = std::io::Error;
181 ///
182 /// fn tick(&mut self, current: &JointArray<Rad>, dt: Duration)
183 /// -> Result<JointArray<NewtonMeter>, Self::Error> {
184 /// // ... 实现 ...
185 /// Ok(JointArray::from([NewtonMeter(0.0); 6]))
186 /// }
187 ///
188 /// fn on_time_jump(&mut self, _dt: Duration) -> Result<(), Self::Error> {
189 /// // ✅ 重置微分项
190 /// self.last_error = JointArray::from([0.0; 6]);
191 ///
192 /// // ❌ 不要清零积分项！
193 /// // self.integral = JointArray::from([0.0; 6]); // 会导致机械臂下坠
194 ///
195 /// Ok(())
196 /// }
197 /// }
198 /// ```
199 ///
200 /// # 何时调用
201 ///
202 /// 通常在 `run_controller()` 中，当检测到 `dt` 超过阈值时：
203 ///
204 /// ```rust,ignore
205 /// if real_dt > max_dt {
206 /// controller.on_time_jump(real_dt)?;
207 /// dt = max_dt; // 钳位后传入 tick()
208 /// }
209 /// ```
210 fn on_time_jump(&mut self, _dt: Duration) -> Result<(), Self::Error> {
211 // 默认：什么都不做
212 // 时间敏感的控制器（如 PID）应该覆盖此方法
213 Ok(())
214 }
215
216 /// 重置控制器到初始状态（可选）
217 ///
218 /// 用于在不销毁控制器的情况下重新开始。
219 ///
220 /// # 默认实现
221 ///
222 /// 默认不实现（返回错误），控制器可以选择性实现。
223 ///
224 /// # 示例
225 ///
226 /// ```rust,no_run
227 /// # use piper_client::control::Controller;
228 /// # use piper_client::types::{JointArray, Rad, NewtonMeter};
229 /// # use std::time::Duration;
230 /// # struct MyController { integral: JointArray<f64>, last_error: JointArray<f64> }
231 /// # impl Controller for MyController {
232 /// # type Error = std::io::Error;
233 /// # fn tick(&mut self, _: &JointArray<Rad>, _: Duration) -> Result<JointArray<NewtonMeter>, Self::Error> { Ok(JointArray::from([NewtonMeter(0.0); 6])) }
234 /// fn reset(&mut self) -> Result<(), Self::Error> {
235 /// // 重置所有内部状态
236 /// self.integral = JointArray::from([0.0; 6]);
237 /// self.last_error = JointArray::from([0.0; 6]);
238 /// Ok(())
239 /// }
240 /// # }
241 /// ```
242 fn reset(&mut self) -> Result<(), Self::Error> {
243 // 默认不支持重置
244 // 控制器可以选择性实现此方法
245 Ok(())
246 }
247}
248
249#[cfg(test)]
250mod tests {
251 use super::*;
252
253 /// 简单的测试控制器（比例控制）
254 struct TestController {
255 target: JointArray<Rad>,
256 kp: f64,
257 }
258
259 impl Controller for TestController {
260 type Error = std::io::Error;
261
262 fn tick(
263 &mut self,
264 current: &JointArray<Rad>,
265 _dt: Duration,
266 ) -> Result<JointArray<NewtonMeter>, Self::Error> {
267 let error = self.target.map_with(*current, |t, c| t - c);
268 let output = error.map(|e| NewtonMeter(self.kp * e.0));
269 Ok(output)
270 }
271 }
272
273 #[test]
274 fn test_controller_trait_basic() {
275 let target = JointArray::from([Rad(1.0); 6]);
276 let mut controller = TestController { target, kp: 10.0 };
277
278 let current = JointArray::from([Rad(0.5); 6]);
279 let dt = Duration::from_millis(10);
280
281 let output = controller.tick(¤t, dt).unwrap();
282
283 // 误差 = 1.0 - 0.5 = 0.5
284 // 输出 = 10.0 * 0.5 = 5.0
285 assert!((output[0].0 - 5.0).abs() < 1e-10);
286 }
287
288 #[test]
289 fn test_on_time_jump_default() {
290 let target = JointArray::from([Rad(1.0); 6]);
291 let mut controller = TestController { target, kp: 10.0 };
292
293 // 默认实现应该什么都不做，不报错
294 let result = controller.on_time_jump(Duration::from_secs(1));
295 assert!(result.is_ok());
296 }
297
298 #[test]
299 fn test_reset_default() {
300 let target = JointArray::from([Rad(1.0); 6]);
301 let mut controller = TestController { target, kp: 10.0 };
302
303 // 默认实现应该什么都不做，不报错
304 let result = controller.reset();
305 assert!(result.is_ok());
306 }
307}