invariant-robotics-core 0.0.2

Core types, physics checks, authority validation, and cryptography for Invariant.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70

// P2: Joint velocity limits check

use std::collections::HashMap;

use crate::models::command::JointState;
use crate::models::profile::{JointDefinition, RealWorldMargins};
use crate::models::verdict::CheckResult;

/// Check that every joint's velocity magnitude does not exceed
/// `max_velocity * global_velocity_scale * (1 - velocity_margin)`.
///
/// When `margins` is `Some`, the limit is tightened by `velocity_margin`.
///
/// Each [`JointState`] is matched to a [`JointDefinition`] by name.
/// A joint state with no matching definition is treated as a violation.
/// If `joints` is empty the check passes trivially.
pub fn check_velocity_limits(
    joints: &[JointState],
    definitions: &[JointDefinition],
    global_velocity_scale: f64,
    margins: Option<&RealWorldMargins>,
) -> CheckResult {
    let def_map: HashMap<&str, &JointDefinition> =
        definitions.iter().map(|d| (d.name.as_str(), d)).collect();

    let mut violations: Vec<String> = Vec::new();

    for state in joints {
        match def_map.get(state.name.as_str()) {
            None => {
                violations.push(format!(
                    "'{}': unknown joint (no definition found)",
                    state.name
                ));
            }
            Some(def) => {
                let margin_factor = 1.0 - margins.map(|m| m.velocity_margin).unwrap_or(0.0);
                let limit = def.max_velocity * global_velocity_scale * margin_factor;
                if !state.velocity.is_finite() {
                    violations.push(format!("'{}': velocity is NaN or infinite", state.name));
                } else if state.velocity.abs() > limit {
                    violations.push(format!(
                        "'{}': |velocity| {:.6} exceeds limit {:.6} (max_velocity {:.6} * scale {:.6})",
                        state.name,
                        state.velocity.abs(),
                        limit,
                        def.max_velocity,
                        global_velocity_scale
                    ));
                }
            }
        }
    }

    if violations.is_empty() {
        CheckResult {
            name: "velocity_limits".to_string(),
            category: "physics".to_string(),
            passed: true,
            details: "all joints within velocity limits".to_string(),
        }
    } else {
        CheckResult {
            name: "velocity_limits".to_string(),
            category: "physics".to_string(),
            passed: false,
            details: violations.join("; "),
        }
    }
}