phytrace_sdk/models/domains/
manipulation.rs

1//! Manipulation Domain - Arm state, end effector, and grasping.
2
3use serde::{Deserialize, Serialize};
4use validator::Validate;
5
6use super::common::{ObjectRef, Pose, Position3D};
7use crate::models::enums::{ArmState, GraspPhase, ManipulationControlMode};
8
9/// Manipulation domain containing arm and grasping information.
10#[derive(Debug, Clone, Default, Serialize, Deserialize)]
11pub struct ManipulationDomain {
12    /// Arm state
13    #[serde(skip_serializing_if = "Option::is_none")]
14    pub arm_state: Option<ArmState>,
15
16    /// End effector information
17    #[serde(skip_serializing_if = "Option::is_none")]
18    pub end_effector: Option<EndEffector>,
19
20    /// Workspace information
21    #[serde(skip_serializing_if = "Option::is_none")]
22    pub workspace: Option<Workspace>,
23
24    /// Current grasp operation
25    #[serde(skip_serializing_if = "Option::is_none")]
26    pub grasp: Option<GraspOperation>,
27
28    /// Hand guiding mode (for cobots)
29    #[serde(skip_serializing_if = "Option::is_none")]
30    pub hand_guiding: Option<HandGuiding>,
31
32    /// Control mode
33    #[serde(skip_serializing_if = "Option::is_none")]
34    pub control_mode: Option<ManipulationControlMode>,
35
36    /// Whether in collision avoidance
37    #[serde(skip_serializing_if = "Option::is_none")]
38    pub collision_avoidance_active: Option<bool>,
39}
40
41/// End effector information.
42#[derive(Debug, Clone, Default, Serialize, Deserialize)]
43pub struct EndEffector {
44    /// End effector type
45    #[serde(skip_serializing_if = "Option::is_none")]
46    pub effector_type: Option<String>,
47
48    /// Current pose
49    #[serde(skip_serializing_if = "Option::is_none")]
50    pub pose: Option<Pose>,
51
52    /// Target pose
53    #[serde(skip_serializing_if = "Option::is_none")]
54    pub target_pose: Option<Pose>,
55
56    /// Velocity (m/s)
57    #[serde(skip_serializing_if = "Option::is_none")]
58    pub velocity_mps: Option<f64>,
59
60    /// Force applied (N)
61    #[serde(skip_serializing_if = "Option::is_none")]
62    pub force_n: Option<f64>,
63
64    /// Torque applied (Nm)
65    #[serde(skip_serializing_if = "Option::is_none")]
66    pub torque_nm: Option<f64>,
67
68    /// Tool center point offset
69    #[serde(skip_serializing_if = "Option::is_none")]
70    pub tcp_offset: Option<Position3D>,
71}
72
73/// Workspace information.
74#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
75pub struct Workspace {
76    /// Workspace ID
77    #[serde(skip_serializing_if = "Option::is_none")]
78    pub workspace_id: Option<String>,
79
80    /// Workspace type
81    #[serde(skip_serializing_if = "Option::is_none")]
82    pub workspace_type: Option<String>,
83
84    /// Whether arm is within workspace bounds
85    #[serde(skip_serializing_if = "Option::is_none")]
86    pub within_bounds: Option<bool>,
87
88    /// Distance to workspace boundary (m)
89    #[serde(skip_serializing_if = "Option::is_none")]
90    #[validate(range(min = 0.0))]
91    pub distance_to_boundary_m: Option<f64>,
92
93    /// Reachability to target (0-1)
94    #[serde(skip_serializing_if = "Option::is_none")]
95    #[validate(range(min = 0.0, max = 1.0))]
96    pub reachability: Option<f64>,
97}
98
99/// Grasp operation information.
100#[derive(Debug, Clone, Default, Serialize, Deserialize, Validate)]
101pub struct GraspOperation {
102    /// Grasp phase
103    #[serde(skip_serializing_if = "Option::is_none")]
104    pub phase: Option<GraspPhase>,
105
106    /// Target object
107    #[serde(skip_serializing_if = "Option::is_none")]
108    pub target_object: Option<ObjectRef>,
109
110    /// Grasp type (e.g., "pinch", "power", "precision")
111    #[serde(skip_serializing_if = "Option::is_none")]
112    pub grasp_type: Option<String>,
113
114    /// Grasp confidence (0-1)
115    #[serde(skip_serializing_if = "Option::is_none")]
116    #[validate(range(min = 0.0, max = 1.0))]
117    pub confidence: Option<f64>,
118
119    /// Grasp pose
120    #[serde(skip_serializing_if = "Option::is_none")]
121    pub grasp_pose: Option<Pose>,
122
123    /// Pre-grasp pose
124    #[serde(skip_serializing_if = "Option::is_none")]
125    pub pre_grasp_pose: Option<Pose>,
126
127    /// Whether grasp is stable
128    #[serde(skip_serializing_if = "Option::is_none")]
129    pub is_stable: Option<bool>,
130
131    /// Slip detected
132    #[serde(skip_serializing_if = "Option::is_none")]
133    pub slip_detected: Option<bool>,
134
135    /// Force at grasp (N)
136    #[serde(skip_serializing_if = "Option::is_none")]
137    pub force_n: Option<f64>,
138}
139
140/// Hand guiding mode information (for collaborative robots).
141#[derive(Debug, Clone, Default, Serialize, Deserialize)]
142pub struct HandGuiding {
143    /// Whether hand guiding is enabled
144    #[serde(skip_serializing_if = "Option::is_none")]
145    pub enabled: Option<bool>,
146
147    /// Whether currently being guided
148    #[serde(skip_serializing_if = "Option::is_none")]
149    pub is_active: Option<bool>,
150
151    /// Detected force from human (N)
152    #[serde(skip_serializing_if = "Option::is_none")]
153    pub detected_force_n: Option<f64>,
154
155    /// Compliance level
156    #[serde(skip_serializing_if = "Option::is_none")]
157    pub compliance_level: Option<String>,
158
159    /// Whether recording path
160    #[serde(skip_serializing_if = "Option::is_none")]
161    pub recording_path: Option<bool>,
162}
163
164impl ManipulationDomain {
165    /// Create with arm state.
166    pub fn new(state: ArmState) -> Self {
167        Self {
168            arm_state: Some(state),
169            ..Default::default()
170        }
171    }
172
173    /// Builder to add end effector.
174    pub fn with_end_effector(mut self, end_effector: EndEffector) -> Self {
175        self.end_effector = Some(end_effector);
176        self
177    }
178
179    /// Builder to add grasp operation.
180    pub fn with_grasp(mut self, grasp: GraspOperation) -> Self {
181        self.grasp = Some(grasp);
182        self
183    }
184
185    /// Builder to add hand guiding.
186    pub fn with_hand_guiding(mut self, hand_guiding: HandGuiding) -> Self {
187        self.hand_guiding = Some(hand_guiding);
188        self
189    }
190}
191
192impl GraspOperation {
193    /// Create a grasp operation.
194    pub fn new(phase: GraspPhase) -> Self {
195        Self {
196            phase: Some(phase),
197            ..Default::default()
198        }
199    }
200
201    /// Set target object.
202    pub fn with_target(mut self, object: ObjectRef) -> Self {
203        self.target_object = Some(object);
204        self
205    }
206
207    /// Set confidence.
208    pub fn with_confidence(mut self, confidence: f64) -> Self {
209        self.confidence = Some(confidence);
210        self
211    }
212}
213
214#[cfg(test)]
215mod tests {
216    use super::*;
217
218    #[test]
219    fn test_manipulation_domain() {
220        let manip = ManipulationDomain::new(ArmState::Moving)
221            .with_grasp(GraspOperation::new(GraspPhase::Approaching));
222
223        assert_eq!(manip.arm_state, Some(ArmState::Moving));
224        assert!(manip.grasp.is_some());
225    }
226
227    // ==========================================================================
228    // ManipulationDomain Tests
229    // ==========================================================================
230
231    #[test]
232    fn test_manipulation_domain_default() {
233        let manip = ManipulationDomain::default();
234        assert!(manip.arm_state.is_none());
235        assert!(manip.end_effector.is_none());
236        assert!(manip.workspace.is_none());
237        assert!(manip.grasp.is_none());
238        assert!(manip.hand_guiding.is_none());
239        assert!(manip.control_mode.is_none());
240        assert!(manip.collision_avoidance_active.is_none());
241    }
242
243    #[test]
244    fn test_manipulation_domain_new() {
245        let manip = ManipulationDomain::new(ArmState::Home);
246        assert_eq!(manip.arm_state, Some(ArmState::Home));
247    }
248
249    #[test]
250    fn test_manipulation_domain_with_end_effector() {
251        let end_effector = EndEffector {
252            effector_type: Some("parallel_gripper".to_string()),
253            velocity_mps: Some(0.5),
254            force_n: Some(10.0),
255            ..Default::default()
256        };
257
258        let manip = ManipulationDomain::new(ArmState::Moving).with_end_effector(end_effector);
259
260        assert!(manip.end_effector.is_some());
261        assert_eq!(manip.end_effector.as_ref().unwrap().force_n, Some(10.0));
262    }
263
264    #[test]
265    fn test_manipulation_domain_with_grasp() {
266        let grasp = GraspOperation::new(GraspPhase::Closing).with_confidence(0.95);
267
268        let manip = ManipulationDomain::new(ArmState::Grasping).with_grasp(grasp);
269
270        assert!(manip.grasp.is_some());
271        assert_eq!(manip.grasp.as_ref().unwrap().confidence, Some(0.95));
272    }
273
274    #[test]
275    fn test_manipulation_domain_with_hand_guiding() {
276        let hand_guiding = HandGuiding {
277            enabled: Some(true),
278            is_active: Some(true),
279            detected_force_n: Some(5.0),
280            ..Default::default()
281        };
282
283        let manip = ManipulationDomain::default().with_hand_guiding(hand_guiding);
284
285        assert!(manip.hand_guiding.is_some());
286        assert_eq!(manip.hand_guiding.as_ref().unwrap().is_active, Some(true));
287    }
288
289    #[test]
290    fn test_manipulation_domain_chained_builders() {
291        let manip = ManipulationDomain::new(ArmState::Moving)
292            .with_end_effector(EndEffector::default())
293            .with_grasp(GraspOperation::new(GraspPhase::PreGrasp))
294            .with_hand_guiding(HandGuiding::default());
295
296        assert!(manip.end_effector.is_some());
297        assert!(manip.grasp.is_some());
298        assert!(manip.hand_guiding.is_some());
299    }
300
301    // ==========================================================================
302    // EndEffector Tests
303    // ==========================================================================
304
305    #[test]
306    fn test_end_effector_default() {
307        let ee = EndEffector::default();
308        assert!(ee.effector_type.is_none());
309        assert!(ee.pose.is_none());
310        assert!(ee.target_pose.is_none());
311        assert!(ee.velocity_mps.is_none());
312        assert!(ee.force_n.is_none());
313        assert!(ee.torque_nm.is_none());
314        assert!(ee.tcp_offset.is_none());
315    }
316
317    #[test]
318    fn test_end_effector_full() {
319        let ee = EndEffector {
320            effector_type: Some("vacuum_gripper".to_string()),
321            pose: Some(Pose::from_position(0.5, 0.2, 0.8)),
322            target_pose: Some(Pose::from_position(0.6, 0.2, 0.5)),
323            velocity_mps: Some(0.3),
324            force_n: Some(15.0),
325            torque_nm: Some(2.5),
326            tcp_offset: Some(Position3D::new(0.0, 0.0, 0.1)),
327        };
328
329        assert_eq!(ee.effector_type, Some("vacuum_gripper".to_string()));
330        assert_eq!(ee.velocity_mps, Some(0.3));
331        assert_eq!(ee.torque_nm, Some(2.5));
332    }
333
334    #[test]
335    fn test_end_effector_serialization() {
336        let ee = EndEffector {
337            effector_type: Some("magnetic".to_string()),
338            force_n: Some(50.0),
339            ..Default::default()
340        };
341
342        let json = serde_json::to_string(&ee).unwrap();
343        let deserialized: EndEffector = serde_json::from_str(&json).unwrap();
344        assert_eq!(deserialized.force_n, Some(50.0));
345    }
346
347    // ==========================================================================
348    // Workspace Tests
349    // ==========================================================================
350
351    #[test]
352    fn test_workspace_default() {
353        let ws = Workspace::default();
354        assert!(ws.workspace_id.is_none());
355        assert!(ws.workspace_type.is_none());
356        assert!(ws.within_bounds.is_none());
357        assert!(ws.distance_to_boundary_m.is_none());
358        assert!(ws.reachability.is_none());
359    }
360
361    #[test]
362    fn test_workspace_full() {
363        let ws = Workspace {
364            workspace_id: Some("ws-001".to_string()),
365            workspace_type: Some("picking_station".to_string()),
366            within_bounds: Some(true),
367            distance_to_boundary_m: Some(0.15),
368            reachability: Some(0.95),
369        };
370
371        assert_eq!(ws.within_bounds, Some(true));
372        assert_eq!(ws.reachability, Some(0.95));
373    }
374
375    #[test]
376    fn test_workspace_serialization() {
377        let ws = Workspace {
378            workspace_type: Some("assembly".to_string()),
379            ..Default::default()
380        };
381
382        let json = serde_json::to_string(&ws).unwrap();
383        let deserialized: Workspace = serde_json::from_str(&json).unwrap();
384        assert_eq!(deserialized.workspace_type, Some("assembly".to_string()));
385    }
386
387    // ==========================================================================
388    // GraspOperation Tests
389    // ==========================================================================
390
391    #[test]
392    fn test_grasp_operation_new() {
393        let grasp = GraspOperation::new(GraspPhase::Failed);
394        assert_eq!(grasp.phase, Some(GraspPhase::Failed));
395    }
396
397    #[test]
398    fn test_grasp_operation_with_target() {
399        let obj = ObjectRef {
400            object_id: Some("obj-001".to_string()),
401            object_type: Some("box".to_string()),
402            ..Default::default()
403        };
404
405        let grasp = GraspOperation::new(GraspPhase::Approaching).with_target(obj);
406
407        assert!(grasp.target_object.is_some());
408        assert_eq!(
409            grasp.target_object.as_ref().unwrap().object_id,
410            Some("obj-001".to_string())
411        );
412    }
413
414    #[test]
415    fn test_grasp_operation_with_confidence() {
416        let grasp = GraspOperation::new(GraspPhase::Closing).with_confidence(0.88);
417
418        assert_eq!(grasp.confidence, Some(0.88));
419    }
420
421    #[test]
422    fn test_grasp_operation_default() {
423        let grasp = GraspOperation::default();
424        assert!(grasp.phase.is_none());
425        assert!(grasp.target_object.is_none());
426        assert!(grasp.grasp_type.is_none());
427        assert!(grasp.confidence.is_none());
428        assert!(grasp.grasp_pose.is_none());
429        assert!(grasp.pre_grasp_pose.is_none());
430        assert!(grasp.is_stable.is_none());
431        assert!(grasp.slip_detected.is_none());
432        assert!(grasp.force_n.is_none());
433    }
434
435    #[test]
436    fn test_grasp_operation_full() {
437        let grasp = GraspOperation {
438            phase: Some(GraspPhase::Secured),
439            target_object: Some(ObjectRef::default()),
440            grasp_type: Some("pinch".to_string()),
441            confidence: Some(0.92),
442            grasp_pose: Some(Pose::from_position(0.1, 0.2, 0.3)),
443            pre_grasp_pose: Some(Pose::from_position(0.1, 0.2, 0.5)),
444            is_stable: Some(true),
445            slip_detected: Some(false),
446            force_n: Some(8.5),
447        };
448
449        assert_eq!(grasp.grasp_type, Some("pinch".to_string()));
450        assert_eq!(grasp.is_stable, Some(true));
451        assert_eq!(grasp.force_n, Some(8.5));
452    }
453
454    #[test]
455    fn test_grasp_operation_chained_builders() {
456        let grasp = GraspOperation::new(GraspPhase::Closing)
457            .with_target(ObjectRef::default())
458            .with_confidence(0.9);
459
460        assert!(grasp.target_object.is_some());
461        assert_eq!(grasp.confidence, Some(0.9));
462    }
463
464    // ==========================================================================
465    // HandGuiding Tests
466    // ==========================================================================
467
468    #[test]
469    fn test_hand_guiding_default() {
470        let hg = HandGuiding::default();
471        assert!(hg.enabled.is_none());
472        assert!(hg.is_active.is_none());
473        assert!(hg.detected_force_n.is_none());
474        assert!(hg.compliance_level.is_none());
475        assert!(hg.recording_path.is_none());
476    }
477
478    #[test]
479    fn test_hand_guiding_full() {
480        let hg = HandGuiding {
481            enabled: Some(true),
482            is_active: Some(true),
483            detected_force_n: Some(3.5),
484            compliance_level: Some("medium".to_string()),
485            recording_path: Some(true),
486        };
487
488        assert_eq!(hg.enabled, Some(true));
489        assert_eq!(hg.detected_force_n, Some(3.5));
490        assert_eq!(hg.recording_path, Some(true));
491    }
492
493    #[test]
494    fn test_hand_guiding_serialization() {
495        let hg = HandGuiding {
496            enabled: Some(true),
497            compliance_level: Some("high".to_string()),
498            ..Default::default()
499        };
500
501        let json = serde_json::to_string(&hg).unwrap();
502        let deserialized: HandGuiding = serde_json::from_str(&json).unwrap();
503        assert_eq!(deserialized.compliance_level, Some("high".to_string()));
504    }
505
506    // ==========================================================================
507    // Serialization Roundtrip Tests
508    // ==========================================================================
509
510    #[test]
511    fn test_manipulation_domain_serialization_roundtrip() {
512        let manip = ManipulationDomain::new(ArmState::Moving)
513            .with_end_effector(EndEffector {
514                force_n: Some(10.0),
515                ..Default::default()
516            })
517            .with_grasp(GraspOperation::new(GraspPhase::Approaching));
518
519        let json = serde_json::to_string(&manip).unwrap();
520        let deserialized: ManipulationDomain = serde_json::from_str(&json).unwrap();
521
522        assert_eq!(deserialized.arm_state, Some(ArmState::Moving));
523        assert!(deserialized.end_effector.is_some());
524        assert!(deserialized.grasp.is_some());
525    }
526}
phytrace_sdk/models/domains/manipulation.rs

phytrace_sdk/models/domains/
manipulation.rs
