piper_client/control/
loop_runner.rs

1//! Loop Runner - 控制循环包装器
2//!
3//! 提供高级控制循环接口，处理定时、dt 钳位、异常检测等。
4//!
5//! # 核心功能
6//!
7//! - **精确定时**: 使用 `spin_sleep` 实现低抖动延时
8//! - **dt 钳位**: 限制异常大的时间步长
9//! - **时间跳变处理**: 自动调用 `on_time_jump()`
10//! - **错误传播**: 透明传播控制器和命令错误
11//!
12//! # 使用场景
13//!
14//! ```rust,ignore
15//! use piper_client::control::{run_controller, LoopConfig, Controller};
16//! use piper_client::state::Piper;
17//!
18//! # fn example(piper: Piper<Active<MitMode>>, controller: impl Controller) -> Result<(), Box<dyn std::error::Error>> {
19//! let config = LoopConfig {
20//!     frequency_hz: 100.0,              // 100Hz 控制频率
21//!     dt_clamp_multiplier: 2.0,         // dt 最大为 2x 标称值
22//!     max_iterations: Some(1000),       // 运行 1000 次后停止
23//! };
24//!
25//! run_controller(piper, controller, config)?;
26//! # Ok(())
27//! # }
28//! ```
29
30use super::controller::Controller;
31use crate::Piper;
32use crate::state::{Active, MitMode};
33use crate::types::RobotError;
34use std::time::{Duration, Instant};
35
36/// 控制循环配置
37#[derive(Debug, Clone)]
38pub struct LoopConfig {
39    /// 控制频率（Hz）
40    ///
41    /// 例如：100.0 表示 100Hz（10ms 周期）
42    pub frequency_hz: f64,
43
44    /// dt 钳位倍数
45    ///
46    /// 当实际 dt 超过标称周期的此倍数时，将触发 `on_time_jump()` 并钳位 dt。
47    ///
48    /// 例如：2.0 表示 dt 最大为 2 * (1 / frequency_hz)
49    pub dt_clamp_multiplier: f64,
50
51    /// 最大迭代次数（None 表示无限循环）
52    ///
53    /// 用于测试或定时运行。
54    pub max_iterations: Option<usize>,
55}
56
57impl Default for LoopConfig {
58    fn default() -> Self {
59        LoopConfig {
60            frequency_hz: 100.0,      // 默认 100Hz
61            dt_clamp_multiplier: 2.0, // 默认 2x
62            max_iterations: None,     // 默认无限循环
63        }
64    }
65}
66
67/// 运行控制循环
68///
69/// 这是一个阻塞函数，会持续运行控制循环直到：
70/// - 发生错误
71/// - 达到 `max_iterations`（如果设置）
72/// - 用户中断（Ctrl+C）
73///
74/// # 参数
75///
76/// - `piper`: `Piper<Active<MitMode>>` 实例（Type State 安全保证）
77/// - `controller`: 控制器（实现 `Controller` trait）
78/// - `config`: 循环配置
79///
80/// # 返回
81///
82/// - `Ok(())`: 正常结束（达到 max_iterations）
83/// - `Err(RobotError)`: 发生错误
84///
85/// # 时间处理
86///
87/// - 计算实际 dt
88/// - 如果 dt > max_dt，调用 `controller.on_time_jump(real_dt)`，然后钳位 dt
89/// - 使用钳位后的 dt 调用 `controller.tick()`
90///
91/// # 示例
92///
93/// ```rust,ignore
94/// use piper_client::control::{run_controller, LoopConfig};
95/// # use piper_client::state::Piper;
96/// # use piper_client::control::Controller;
97/// # fn example(
98/// #     piper: Piper<Active<MitMode>>,
99/// #     controller: impl Controller,
100/// # ) -> Result<(), Box<dyn std::error::Error>> {
101/// let config = LoopConfig {
102///     frequency_hz: 200.0,  // 200Hz 高频控制
103///     dt_clamp_multiplier: 1.5,
104///     max_iterations: Some(2000),  // 运行 10 秒后停止
105/// };
106///
107/// run_controller(piper, controller, config)?;
108/// # Ok(())
109/// # }
110/// ```
111pub fn run_controller<C>(
112    piper: Piper<Active<MitMode>>,
113    mut controller: C,
114    config: LoopConfig,
115) -> Result<(), RobotError>
116where
117    C: Controller,
118    RobotError: From<C::Error>,
119{
120    // ✅ 输入验证
121    if config.frequency_hz <= 0.0 {
122        return Err(RobotError::ConfigError(format!(
123            "Invalid frequency_hz: {} (must be > 0)",
124            config.frequency_hz
125        )));
126    }
127    if config.frequency_hz > 10000.0 {
128        tracing::warn!(
129            "Very high control frequency: {} Hz. This may cause performance issues.",
130            config.frequency_hz
131        );
132    }
133    if config.dt_clamp_multiplier <= 0.0 {
134        return Err(RobotError::ConfigError(format!(
135            "Invalid dt_clamp_multiplier: {} (must be > 0)",
136            config.dt_clamp_multiplier
137        )));
138    }
139
140    // 计算标称周期和最大 dt
141    let nominal_period = Duration::from_secs_f64(1.0 / config.frequency_hz);
142    let max_dt = nominal_period.mul_f64(config.dt_clamp_multiplier);
143
144    let mut last_time = Instant::now();
145    let mut iteration = 0;
146
147    loop {
148        // 检查是否达到最大迭代次数
149        if let Some(max_iter) = config.max_iterations
150            && iteration >= max_iter
151        {
152            return Ok(());
153        }
154
155        // 1. 计算 dt
156        let now = Instant::now();
157        let real_dt = now - last_time;
158        let mut dt = real_dt;
159
160        // 2. dt 钳位
161        if real_dt > max_dt {
162            // 调用 on_time_jump 处理异常
163            controller.on_time_jump(real_dt).map_err(RobotError::from)?;
164
165            // 钳位 dt
166            dt = max_dt;
167        }
168
169        // 3. 读取当前状态
170        let current = piper.observer().joint_positions();
171
172        // 4. 调用控制器（只返回力矩）
173        let torques = controller.tick(&current, dt).map_err(RobotError::from)?;
174
175        // 5. 发送命令（使用纯力矩模式，kp/kd=0）
176        let zero_positions = crate::types::JointArray::from([crate::types::Rad(0.0); 6]);
177        let zero_velocities = crate::types::JointArray::from([0.0; 6]);
178        let zero_kp = crate::types::JointArray::from([0.0; 6]);
179        let zero_kd = crate::types::JointArray::from([0.0; 6]);
180        piper.command_torques(
181            &zero_positions,
182            &zero_velocities,
183            &zero_kp,
184            &zero_kd,
185            &torques,
186        )?;
187
188        // 6. 更新时间
189        last_time = now;
190        iteration += 1;
191
192        // 7. 休眠到下一个周期
193        std::thread::sleep(nominal_period);
194    }
195}
196
197/// 使用 spin_sleep 的高精度控制循环
198///
199/// 与 `run_controller()` 类似，但使用 `spin_sleep` 实现更低的延时抖动。
200///
201/// ⚠️ **注意**: `spin_sleep` 会占用更多 CPU，适合对实时性要求极高的场景。
202pub fn run_controller_spin<C>(
203    piper: Piper<Active<MitMode>>,
204    mut controller: C,
205    config: LoopConfig,
206) -> Result<(), RobotError>
207where
208    C: Controller,
209    RobotError: From<C::Error>,
210{
211    use spin_sleep::SpinSleeper;
212
213    // ✅ 输入验证
214    if config.frequency_hz <= 0.0 {
215        return Err(RobotError::ConfigError(format!(
216            "Invalid frequency_hz: {} (must be > 0)",
217            config.frequency_hz
218        )));
219    }
220    if config.frequency_hz > 10000.0 {
221        tracing::warn!(
222            "Very high control frequency: {} Hz. This may cause performance issues.",
223            config.frequency_hz
224        );
225    }
226    if config.dt_clamp_multiplier <= 0.0 {
227        return Err(RobotError::ConfigError(format!(
228            "Invalid dt_clamp_multiplier: {} (must be > 0)",
229            config.dt_clamp_multiplier
230        )));
231    }
232
233    let nominal_period = Duration::from_secs_f64(1.0 / config.frequency_hz);
234    let max_dt = nominal_period.mul_f64(config.dt_clamp_multiplier);
235    let sleeper = SpinSleeper::default();
236
237    let mut last_time = Instant::now();
238    let mut iteration = 0;
239
240    loop {
241        if let Some(max_iter) = config.max_iterations
242            && iteration >= max_iter
243        {
244            return Ok(());
245        }
246
247        let now = Instant::now();
248        let real_dt = now - last_time;
249        let mut dt = real_dt;
250
251        if real_dt > max_dt {
252            controller.on_time_jump(real_dt).map_err(RobotError::from)?;
253            dt = max_dt;
254        }
255
256        let current = piper.observer().joint_positions();
257        let torques = controller.tick(&current, dt).map_err(RobotError::from)?;
258
259        let zero_positions = crate::types::JointArray::from([crate::types::Rad(0.0); 6]);
260        let zero_velocities = crate::types::JointArray::from([0.0; 6]);
261        let zero_kp = crate::types::JointArray::from([0.0; 6]);
262        let zero_kd = crate::types::JointArray::from([0.0; 6]);
263        piper.command_torques(
264            &zero_positions,
265            &zero_velocities,
266            &zero_kp,
267            &zero_kd,
268            &torques,
269        )?;
270
271        last_time = now;
272        iteration += 1;
273
274        sleeper.sleep(nominal_period);
275    }
276}
277
278#[cfg(test)]
279mod tests {
280    use super::*;
281
282    #[test]
283    fn test_loop_config_default() {
284        let config = LoopConfig::default();
285        assert_eq!(config.frequency_hz, 100.0);
286        assert_eq!(config.dt_clamp_multiplier, 2.0);
287        assert_eq!(config.max_iterations, None);
288    }
289
290    #[test]
291    fn test_loop_config_custom() {
292        let config = LoopConfig {
293            frequency_hz: 200.0,
294            dt_clamp_multiplier: 1.5,
295            max_iterations: Some(1000),
296        };
297        assert_eq!(config.frequency_hz, 200.0);
298        assert_eq!(config.dt_clamp_multiplier, 1.5);
299        assert_eq!(config.max_iterations, Some(1000));
300    }
301
302    #[test]
303    fn test_invalid_frequency() {
304        let config = LoopConfig {
305            frequency_hz: -1.0,
306            ..Default::default()
307        };
308        // 注意：此测试需要实际的 robot 实例，在单元测试中无法完成
309        // 只验证配置构造
310        assert_eq!(config.frequency_hz, -1.0);
311    }
312}
piper_client/control/loop_runner.rs

piper_client/control/
loop_runner.rs
