piper_client/types/
joint.rs

1//! 关节索引和数组
2//!
3//! 提供编译期安全的关节索引，防止越界和索引错误。
4//!
5//! # 设计目标
6//!
7//! - **编译期安全**: 使用枚举防止无效索引
8//! - **零开销**: 编译后与直接数组访问性能相同
9//! - **类型友好**: 支持泛型和迭代器
10//!
11//! # 示例
12//!
13//! ```rust
14//! use piper_client::types::{Joint, JointArray, Rad};
15//!
16//! let positions = JointArray::new([
17//!     Rad(0.0), Rad(0.1), Rad(0.2),
18//!     Rad(0.3), Rad(0.4), Rad(0.5),
19//! ]);
20//!
21//! // 类型安全的索引访问
22//! let j1_pos = positions[Joint::J1];
23//! assert_eq!(j1_pos, Rad(0.0));
24//!
25//! // 迭代器
26//! for (joint, pos) in Joint::ALL.iter().zip(positions.iter()) {
27//!     println!("{:?}: {}", joint, pos);
28//! }
29//!
30//! // 映射转换
31//! let deg_positions = positions.map(|r| r.to_deg());
32//! ```
33
34use super::units::Rad;
35use std::fmt;
36use std::ops::{Index, IndexMut};
37
38/// 关节枚举
39///
40/// 表示 Piper 机械臂的 6 个关节。使用枚举提供编译期类型安全。
41#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
42#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
43pub enum Joint {
44    /// 关节 1（基座旋转）
45    J1 = 0,
46    /// 关节 2（肩部俯仰）
47    J2 = 1,
48    /// 关节 3（肘部俯仰）
49    J3 = 2,
50    /// 关节 4（腕部旋转）
51    J4 = 3,
52    /// 关节 5（腕部俯仰）
53    J5 = 4,
54    /// 关节 6（末端旋转）
55    J6 = 5,
56}
57
58impl Joint {
59    /// 所有关节的数组
60    pub const ALL: [Joint; 6] = [
61        Joint::J1,
62        Joint::J2,
63        Joint::J3,
64        Joint::J4,
65        Joint::J5,
66        Joint::J6,
67    ];
68
69    /// 获取关节索引（0-5）
70    #[inline]
71    pub const fn index(self) -> usize {
72        self as usize
73    }
74
75    /// 从索引创建关节（范围检查）
76    pub fn from_index(index: usize) -> Option<Self> {
77        match index {
78            0 => Some(Joint::J1),
79            1 => Some(Joint::J2),
80            2 => Some(Joint::J3),
81            3 => Some(Joint::J4),
82            4 => Some(Joint::J5),
83            5 => Some(Joint::J6),
84            _ => None,
85        }
86    }
87
88    /// 获取关节名称
89    pub const fn name(self) -> &'static str {
90        match self {
91            Joint::J1 => "J1",
92            Joint::J2 => "J2",
93            Joint::J3 => "J3",
94            Joint::J4 => "J4",
95            Joint::J5 => "J5",
96            Joint::J6 => "J6",
97        }
98    }
99}
100
101impl fmt::Display for Joint {
102    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
103        write!(f, "{}", self.name())
104    }
105}
106
107/// 关节数组
108///
109/// 类型安全的 6 关节数组容器，支持索引、迭代和映射操作。
110#[derive(Debug, Clone, PartialEq)]
111#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
112pub struct JointArray<T> {
113    data: [T; 6],
114}
115
116// 如果 T 实现了 Copy，则 JointArray<T> 也实现 Copy
117impl<T: Copy> Copy for JointArray<T> {}
118
119impl<T> JointArray<T> {
120    /// 创建新的关节数组
121    #[inline]
122    pub const fn new(data: [T; 6]) -> Self {
123        JointArray { data }
124    }
125
126    /// 获取内部数组的引用
127    #[inline]
128    pub fn as_array(&self) -> &[T; 6] {
129        &self.data
130    }
131
132    /// 获取内部数组的可变引用
133    #[inline]
134    pub fn as_array_mut(&mut self) -> &mut [T; 6] {
135        &mut self.data
136    }
137
138    /// 获取内部数组（消耗 self）
139    #[inline]
140    pub fn into_array(self) -> [T; 6] {
141        self.data
142    }
143
144    /// 迭代器
145    pub fn iter(&self) -> std::slice::Iter<'_, T> {
146        self.data.iter()
147    }
148
149    /// 可变迭代器
150    pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, T> {
151        self.data.iter_mut()
152    }
153
154    /// 映射转换
155    pub fn map<U, F>(self, mut f: F) -> JointArray<U>
156    where
157        F: FnMut(T) -> U,
158    {
159        let [a, b, c, d, e, g] = self.data;
160        JointArray::new([f(a), f(b), f(c), f(d), f(e), f(g)])
161    }
162
163    /// 带索引的映射转换
164    pub fn map_with_joint<U, F>(self, mut f: F) -> JointArray<U>
165    where
166        F: FnMut(Joint, T) -> U,
167    {
168        let [a, b, c, d, e, g] = self.data;
169        JointArray::new([
170            f(Joint::J1, a),
171            f(Joint::J2, b),
172            f(Joint::J3, c),
173            f(Joint::J4, d),
174            f(Joint::J5, e),
175            f(Joint::J6, g),
176        ])
177    }
178
179    /// 按关节和另一个数组的元素执行映射
180    pub fn map_with<U, V, F>(self, other: JointArray<U>, mut f: F) -> JointArray<V>
181    where
182        F: FnMut(T, U) -> V,
183    {
184        let [a1, b1, c1, d1, e1, f1] = self.data;
185        let [a2, b2, c2, d2, e2, f2] = other.data;
186        JointArray::new([
187            f(a1, a2),
188            f(b1, b2),
189            f(c1, c2),
190            f(d1, d2),
191            f(e1, e2),
192            f(f1, f2),
193        ])
194    }
195}
196
197impl<T: Copy> JointArray<T> {
198    /// 创建所有元素相同的数组
199    #[inline]
200    pub const fn splat(value: T) -> Self {
201        JointArray::new([value, value, value, value, value, value])
202    }
203}
204
205impl<T: Default> Default for JointArray<T> {
206    fn default() -> Self {
207        JointArray::new([
208            T::default(),
209            T::default(),
210            T::default(),
211            T::default(),
212            T::default(),
213            T::default(),
214        ])
215    }
216}
217
218// 索引访问
219impl<T> Index<Joint> for JointArray<T> {
220    type Output = T;
221
222    #[inline]
223    fn index(&self, joint: Joint) -> &T {
224        &self.data[joint.index()]
225    }
226}
227
228impl<T> IndexMut<Joint> for JointArray<T> {
229    #[inline]
230    fn index_mut(&mut self, joint: Joint) -> &mut T {
231        &mut self.data[joint.index()]
232    }
233}
234
235impl<T> Index<usize> for JointArray<T> {
236    type Output = T;
237
238    #[inline]
239    fn index(&self, index: usize) -> &T {
240        &self.data[index]
241    }
242}
243
244impl<T> IndexMut<usize> for JointArray<T> {
245    #[inline]
246    fn index_mut(&mut self, index: usize) -> &mut T {
247        &mut self.data[index]
248    }
249}
250
251// From/Into 转换
252impl<T> From<[T; 6]> for JointArray<T> {
253    #[inline]
254    fn from(data: [T; 6]) -> Self {
255        JointArray::new(data)
256    }
257}
258
259impl<T> From<JointArray<T>> for [T; 6] {
260    #[inline]
261    fn from(arr: JointArray<T>) -> Self {
262        arr.data
263    }
264}
265
266// IntoIterator 实现
267impl<T> IntoIterator for JointArray<T> {
268    type Item = T;
269    type IntoIter = std::array::IntoIter<T, 6>;
270
271    fn into_iter(self) -> Self::IntoIter {
272        self.data.into_iter()
273    }
274}
275
276impl<'a, T> IntoIterator for &'a JointArray<T> {
277    type Item = &'a T;
278    type IntoIter = std::slice::Iter<'a, T>;
279
280    fn into_iter(self) -> Self::IntoIter {
281        self.data.iter()
282    }
283}
284
285impl<'a, T> IntoIterator for &'a mut JointArray<T> {
286    type Item = &'a mut T;
287    type IntoIter = std::slice::IterMut<'a, T>;
288
289    fn into_iter(self) -> Self::IntoIter {
290        self.data.iter_mut()
291    }
292}
293
294/// 关节位置（弧度）
295pub type JointPositions = JointArray<Rad>;
296
297/// 关节速度（弧度/秒）
298pub type JointVelocities = JointArray<Rad>;
299
300/// 关节加速度（弧度/秒²）
301pub type JointAccelerations = JointArray<Rad>;
302
303#[cfg(test)]
304mod tests {
305    use super::*;
306
307    #[test]
308    fn test_joint_index() {
309        assert_eq!(Joint::J1.index(), 0);
310        assert_eq!(Joint::J6.index(), 5);
311    }
312
313    #[test]
314    fn test_joint_from_index() {
315        assert_eq!(Joint::from_index(0), Some(Joint::J1));
316        assert_eq!(Joint::from_index(5), Some(Joint::J6));
317        assert_eq!(Joint::from_index(6), None);
318    }
319
320    #[test]
321    fn test_joint_name() {
322        assert_eq!(Joint::J1.name(), "J1");
323        assert_eq!(format!("{}", Joint::J3), "J3");
324    }
325
326    #[test]
327    fn test_joint_all() {
328        assert_eq!(Joint::ALL.len(), 6);
329        assert_eq!(Joint::ALL[0], Joint::J1);
330        assert_eq!(Joint::ALL[5], Joint::J6);
331    }
332
333    #[test]
334    fn test_joint_array_creation() {
335        let arr = JointArray::new([1, 2, 3, 4, 5, 6]);
336        assert_eq!(arr[Joint::J1], 1);
337        assert_eq!(arr[Joint::J6], 6);
338    }
339
340    #[test]
341    fn test_joint_array_indexing() {
342        let positions =
343            JointArray::new([Rad(0.0), Rad(0.1), Rad(0.2), Rad(0.3), Rad(0.4), Rad(0.5)]);
344
345        assert_eq!(positions[Joint::J1], Rad(0.0));
346        assert_eq!(positions[Joint::J6], Rad(0.5));
347
348        // 使用 usize 索引
349        assert_eq!(positions[0], Rad(0.0));
350        assert_eq!(positions[5], Rad(0.5));
351    }
352
353    #[test]
354    fn test_joint_array_iteration() {
355        let positions = JointArray::new([Rad(1.0); 6]);
356        let sum: f64 = positions.iter().map(|r| r.0).sum();
357        assert!((sum - 6.0).abs() < 1e-10);
358    }
359
360    #[test]
361    fn test_joint_array_map() {
362        let rad = JointArray::new([Rad(std::f64::consts::PI); 6]);
363        let deg = rad.map(|r| r.to_deg());
364        assert!((deg[Joint::J1].0 - 180.0).abs() < 1e-6);
365    }
366
367    #[test]
368    fn test_joint_array_map_with_joint() {
369        let positions = JointArray::new([Rad(1.0); 6]);
370        let scaled = positions.map_with_joint(|joint, rad| Rad(rad.0 * (joint.index() + 1) as f64));
371
372        assert_eq!(scaled[Joint::J1], Rad(1.0));
373        assert_eq!(scaled[Joint::J2], Rad(2.0));
374        assert_eq!(scaled[Joint::J6], Rad(6.0));
375    }
376
377    #[test]
378    fn test_joint_array_map_with() {
379        let a = JointArray::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
380        let b = JointArray::new([0.5, 0.5, 0.5, 0.5, 0.5, 0.5]);
381        let c = a.map_with(b, |x, y| x * y);
382
383        assert_eq!(c[Joint::J1], 0.5);
384        assert_eq!(c[Joint::J2], 1.0);
385    }
386
387    #[test]
388    fn test_joint_array_splat() {
389        let arr = JointArray::splat(Rad(0.5));
390        for joint in Joint::ALL {
391            assert_eq!(arr[joint], Rad(0.5));
392        }
393    }
394
395    #[test]
396    fn test_joint_array_mut() {
397        let mut positions = JointArray::new([Rad(0.0); 6]);
398        positions[Joint::J3] = Rad(1.5);
399        assert_eq!(positions[Joint::J3], Rad(1.5));
400    }
401
402    #[test]
403    fn test_joint_array_into_iter() {
404        let arr = JointArray::new([1, 2, 3, 4, 5, 6]);
405        let vec: Vec<_> = arr.into_iter().collect();
406        assert_eq!(vec, vec![1, 2, 3, 4, 5, 6]);
407    }
408
409    #[test]
410    fn test_joint_array_default() {
411        let arr: JointArray<i32> = JointArray::default();
412        for i in 0..6 {
413            assert_eq!(arr[i], 0);
414        }
415    }
416
417    #[test]
418    fn test_joint_positions_type() {
419        let positions: JointPositions = JointArray::new([Rad(0.0); 6]);
420        assert_eq!(positions[Joint::J1], Rad(0.0));
421    }
422
423    #[test]
424    fn test_from_into_array() {
425        let data = [1, 2, 3, 4, 5, 6];
426        let joint_array = JointArray::from(data);
427        let back: [i32; 6] = joint_array.into();
428        assert_eq!(data, back);
429    }
430}
piper_client/types/joint.rs

piper_client/types/
joint.rs
