nabled_sim/
context.rs

1//! Validated robot context for serial-chain orchestration.
2
3use nabled_core::scalar::NabledReal;
4use nabled_dynamics::DynamicsConfig;
5use nabled_kinematics::chain::ChainSpec;
6use nabled_model::dh::{DynamicsBranchSpec, extract_chain_spec_for_dynamics};
7use nabled_model::robot::RobotModel;
8
9use crate::SimError;
10
11/// Shared context binding model, serial chain, and dynamics configuration.
12#[derive(Debug, Clone, PartialEq)]
13pub struct RobotContext<T> {
14    pub model:           RobotModel<T>,
15    pub chain:           ChainSpec<T>,
16    pub dynamics_config: DynamicsConfig<T>,
17}
18
19impl<T: NabledReal + Default> RobotContext<T> {
20    /// Build context without validation; call [`Self::validate`] before use.
21    #[must_use]
22    pub fn new(
23        model: RobotModel<T>,
24        chain: ChainSpec<T>,
25        dynamics_config: DynamicsConfig<T>,
26    ) -> Self {
27        Self { model, chain, dynamics_config }
28    }
29
30    /// Ensure model DOF matches serial chain joint count.
31    pub fn validate(&self) -> Result<(), SimError> {
32        if self.model.dof() != self.chain.num_joints() {
33            return Err(SimError::DimensionMismatch);
34        }
35        self.chain.validate()?;
36        self.model.validate()?;
37        Ok(())
38    }
39
40    /// Extract a serial branch for branch dynamics (compose-down to `nabled-model`).
41    pub fn extract_chain_for_dynamics(
42        &self,
43        base_link: &str,
44        ee_link: &str,
45    ) -> Result<DynamicsBranchSpec<T>, SimError> {
46        extract_chain_spec_for_dynamics(&self.model, base_link, ee_link).map_err(SimError::from)
47    }
48}
49
50#[cfg(test)]
51mod tests {
52    use nabled_kinematics::chain::ChainSpec;
53    use nabled_model::fixture::load_planar2r_json;
54    use ndarray::arr1;
55
56    use super::*;
57
58    #[test]
59    fn validates_matching_dof() {
60        let fixture = load_planar2r_json().expect("fixture");
61        let model = fixture.to_robot_model::<f64>().expect("model");
62        let chain = fixture.to_chain_spec::<f64>().expect("chain");
63        let config = DynamicsConfig {
64            gravity: fixture.gravity.unwrap_or([0.0, -9.81, 0.0]),
65            ..DynamicsConfig::default()
66        };
67        let ctx = RobotContext::new(model, chain, config);
68        ctx.validate().expect("valid context");
69    }
70
71    #[test]
72    fn rejects_dof_mismatch() {
73        let fixture = load_planar2r_json().expect("fixture");
74        let model = fixture.to_robot_model::<f64>().expect("model");
75        let chain = ChainSpec::from_dh(
76            nabled_kinematics::chain::DhConvention::Standard,
77            vec![
78                nabled_kinematics::chain::JointType::Revolute,
79                nabled_kinematics::chain::JointType::Revolute,
80                nabled_kinematics::chain::JointType::Revolute,
81            ],
82            arr1(&[1.0, 1.0, 1.0]),
83            arr1(&[0.0, 0.0, 0.0]),
84            arr1(&[0.0, 0.0, 0.0]),
85            arr1(&[0.0, 0.0, 0.0]),
86        )
87        .expect("3-dof chain");
88        let config = DynamicsConfig { gravity: [0.0, -9.81, 0.0], ..DynamicsConfig::default() };
89        let ctx = RobotContext::new(model, chain, config);
90        assert_eq!(ctx.validate(), Err(SimError::DimensionMismatch));
91    }
92
93    #[test]
94    fn rejects_malformed_chain() {
95        let fixture = load_planar2r_json().expect("fixture");
96        let model = fixture.to_robot_model::<f64>().expect("model");
97        let chain = ChainSpec {
98            convention:   nabled_kinematics::chain::DhConvention::Standard,
99            joint_types:  vec![
100                nabled_kinematics::chain::JointType::Revolute,
101                nabled_kinematics::chain::JointType::Revolute,
102            ],
103            a:            arr1(&[1.0]),
104            alpha:        arr1(&[0.0, 0.0]),
105            d:            arr1(&[0.0, 0.0]),
106            theta_offset: arr1(&[0.0, 0.0]),
107            ee_index:     None,
108        };
109        let config = DynamicsConfig { gravity: [0.0, -9.81, 0.0], ..DynamicsConfig::default() };
110        let ctx = RobotContext::new(model, chain, config);
111        assert!(matches!(
112            ctx.validate(),
113            Err(SimError::Kinematics(nabled_kinematics::KinematicsError::DimensionMismatch))
114        ));
115    }
116
117    #[test]
118    fn rejects_invalid_model() {
119        use nabled_model::joint::JointAxis;
120        use nabled_model::link::LinkSpec;
121        use nabled_model::robot::{BodySpec, DhParams, RobotModel};
122
123        let fixture = load_planar2r_json().expect("fixture");
124        let chain = fixture.to_chain_spec::<f64>().expect("chain");
125        let body = |name: &str, parent: &str| BodySpec {
126            link:         LinkSpec { name: name.to_string() },
127            parent_link:  parent.to_string(),
128            joint_type:   nabled_model::joint::JointType::Revolute,
129            axis:         JointAxis::Z,
130            limits:       None,
131            inertial:     None,
132            joint_origin: nabled_model::origin::transform_from_xyz_rpy([1.0, 0.0, 0.0], [
133                0.0, 0.0, 0.0,
134            ])
135            .expect("origin"),
136            dh_params:    Some(DhParams {
137                a:            1.0,
138                alpha:        0.0,
139                d:            0.0,
140                theta_offset: 0.0,
141            }),
142        };
143        let mut model = RobotModel::new();
144        let _ = model.add_body(None, body("link0", "base"));
145        let _ = model.add_body(Some(99), body("link1", "link0"));
146        let config = DynamicsConfig { gravity: [0.0, -9.81, 0.0], ..DynamicsConfig::default() };
147        let ctx = RobotContext::new(model, chain, config);
148        assert!(matches!(
149            ctx.validate(),
150            Err(SimError::Model(nabled_model::ModelError::InvalidInput(_)))
151        ));
152    }
153}
nabled_sim/context.rs

nabled_sim/
context.rs
