nabled_model/
fixture.rs

1//! JSON fixture loader for Physical AI integration tests.
2
3use nabled_core::scalar::NabledReal;
4use nabled_kinematics::chain::{ChainSpec, DhConvention, JointType as KinJointType};
5use ndarray::{Array1, Array2};
6use serde::Deserialize;
7
8use crate::ModelError;
9use crate::joint::JointType;
10use crate::link::InertialSpec;
11use crate::origin::joint_origin_from_dh_scalars;
12use crate::robot::{BodySpec, DhParams, RobotModel};
13
14#[derive(Debug, Deserialize)]
15pub struct Planar2rFixture {
16    pub description:   String,
17    pub link_lengths:  Vec<f64>,
18    pub dh_convention: String,
19    pub dh_params:     Vec<[f64; 4]>,
20    pub gravity:       Option<[f64; 3]>,
21    pub links:         Option<Vec<LinkFixture>>,
22    pub cases:         Vec<Planar2rCase>,
23}
24
25#[derive(Debug, Deserialize, Clone, Copy)]
26pub struct LinkFixture {
27    pub mass:    f64,
28    pub com:     [f64; 3],
29    pub inertia: [[f64; 3]; 3],
30}
31
32#[derive(Debug, Deserialize)]
33pub struct Planar2rCase {
34    pub name:                 String,
35    pub q:                    Vec<f64>,
36    pub qd:                   Option<Vec<f64>>,
37    pub qdd:                  Option<Vec<f64>>,
38    pub tau:                  Option<Vec<f64>>,
39    pub tau_gravity:          Option<Vec<f64>>,
40    pub ee_translation:       Option<Vec<f64>>,
41    pub jacobian_translation: Option<Vec<Vec<f64>>>,
42}
43
44impl Planar2rFixture {
45    /// Build a `RobotModel` from fixture DH and optional link inertials.
46    pub fn to_robot_model<T: NabledReal + Default>(&self) -> Result<RobotModel<T>, ModelError> {
47        let mut model = RobotModel::new();
48        let mut parent = None;
49        for (i, params) in self.dh_params.iter().enumerate() {
50            let inertial =
51                self.links.as_ref().and_then(|links| links.get(i)).map(|link| InertialSpec {
52                    mass:    parse_scalar::<T>(link.mass).unwrap_or(T::one()),
53                    com:     [
54                        parse_scalar::<T>(link.com[0]).unwrap_or(T::zero()),
55                        parse_scalar::<T>(link.com[1]).unwrap_or(T::zero()),
56                        parse_scalar::<T>(link.com[2]).unwrap_or(T::zero()),
57                    ],
58                    inertia: Array2::from_shape_fn((3, 3), |(r, c)| {
59                        parse_scalar::<T>(link.inertia[r][c]).unwrap_or(T::zero())
60                    }),
61                });
62            let body = BodySpec {
63                link: crate::link::LinkSpec { name: format!("link{i}") },
64                parent_link: if i == 0 { "base".to_string() } else { format!("link{}", i - 1) },
65                joint_type: JointType::Revolute,
66                axis: crate::joint::JointAxis::Z,
67                limits: None,
68                inertial,
69                joint_origin: joint_origin_from_dh_scalars(
70                    parse_scalar::<T>(params[0])?,
71                    parse_scalar::<T>(params[1])?,
72                    parse_scalar::<T>(params[2])?,
73                    parse_scalar::<T>(params[3])?,
74                )?,
75                dh_params: Some(DhParams {
76                    a:            parse_scalar::<T>(params[0])?,
77                    alpha:        parse_scalar::<T>(params[1])?,
78                    d:            parse_scalar::<T>(params[2])?,
79                    theta_offset: parse_scalar::<T>(params[3])?,
80                }),
81            };
82            let index = model.add_body(parent, body);
83            parent = Some(index);
84        }
85        model.validate()?;
86        Ok(model)
87    }
88
89    /// Build a kinematic `ChainSpec` from fixture DH parameters.
90    pub fn to_chain_spec<T: NabledReal>(&self) -> Result<ChainSpec<T>, ModelError> {
91        let convention = match self.dh_convention.as_str() {
92            "standard" => DhConvention::Standard,
93            "modified" => DhConvention::Modified,
94            other => {
95                return Err(ModelError::InvalidInput(format!("unknown DH convention {other}")));
96            }
97        };
98        let n = self.dh_params.len();
99        let joint_types = vec![KinJointType::Revolute; n];
100        let a = Array1::from_iter(self.dh_params.iter().map(|p| parse_scalar::<T>(p[0]).unwrap()));
101        let alpha =
102            Array1::from_iter(self.dh_params.iter().map(|p| parse_scalar::<T>(p[1]).unwrap()));
103        let d = Array1::from_iter(self.dh_params.iter().map(|p| parse_scalar::<T>(p[2]).unwrap()));
104        let theta_offset =
105            Array1::from_iter(self.dh_params.iter().map(|p| parse_scalar::<T>(p[3]).unwrap()));
106        ChainSpec::from_dh(convention, joint_types, a, alpha, d, theta_offset)
107            .map_err(|_| ModelError::DimensionMismatch)
108    }
109
110    /// Load fixture from JSON file path.
111    pub fn from_file(path: &str) -> Result<Self, ModelError> {
112        let content = std::fs::read_to_string(path)
113            .map_err(|err| ModelError::ParseError(format!("failed to read {path}: {err}")))?;
114        serde_json::from_str(&content)
115            .map_err(|err| ModelError::ParseError(format!("invalid JSON: {err}")))
116    }
117}
118
119fn parse_scalar<T: NabledReal>(value: f64) -> Result<T, ModelError> {
120    T::from_f64(value).ok_or_else(|| ModelError::ParseError(format!("invalid scalar {value}")))
121}
122
123/// Load the canonical planar 2R JSON fixture used by integration tests.
124pub fn load_planar2r_json() -> Result<Planar2rFixture, ModelError> {
125    let path =
126        concat!(env!("CARGO_MANIFEST_DIR"), "/../nabled/tests/fixtures/physical_ai/2r_planar.json");
127    Planar2rFixture::from_file(path)
128}
129
130#[derive(Debug, Deserialize)]
131pub struct SixDofDhFixture {
132    pub description:   String,
133    pub dh_convention: String,
134    pub dh_params:     Vec<[f64; 4]>,
135    pub cases:         Vec<SixDofCase>,
136}
137
138#[derive(Debug, Deserialize)]
139pub struct SixDofCase {
140    pub name:           String,
141    pub q:              Vec<f64>,
142    pub ee_translation: Vec<f64>,
143    #[serde(default)]
144    pub tolerance:      f64,
145}
146
147impl SixDofDhFixture {
148    /// Build a kinematic `ChainSpec` from fixture DH parameters.
149    pub fn to_chain_spec<T: NabledReal>(&self) -> Result<ChainSpec<T>, ModelError> {
150        let convention = match self.dh_convention.as_str() {
151            "standard" => DhConvention::Standard,
152            "modified" => DhConvention::Modified,
153            other => {
154                return Err(ModelError::InvalidInput(format!("unknown DH convention {other}")));
155            }
156        };
157        let n = self.dh_params.len();
158        let joint_types = vec![KinJointType::Revolute; n];
159        let a = Array1::from_iter(self.dh_params.iter().map(|p| parse_scalar::<T>(p[0]).unwrap()));
160        let alpha =
161            Array1::from_iter(self.dh_params.iter().map(|p| parse_scalar::<T>(p[1]).unwrap()));
162        let d = Array1::from_iter(self.dh_params.iter().map(|p| parse_scalar::<T>(p[2]).unwrap()));
163        let theta_offset =
164            Array1::from_iter(self.dh_params.iter().map(|p| parse_scalar::<T>(p[3]).unwrap()));
165        ChainSpec::from_dh(convention, joint_types, a, alpha, d, theta_offset)
166            .map_err(|_| ModelError::DimensionMismatch)
167    }
168
169    /// Load fixture from JSON file path.
170    pub fn from_file(path: &str) -> Result<Self, ModelError> {
171        let content = std::fs::read_to_string(path)
172            .map_err(|err| ModelError::ParseError(format!("failed to read {path}: {err}")))?;
173        serde_json::from_str(&content)
174            .map_err(|err| ModelError::ParseError(format!("invalid JSON: {err}")))
175    }
176}
177
178/// Load the canonical 6-DOF DH JSON fixture used by integration tests.
179pub fn load_six_dof_dh_json() -> Result<SixDofDhFixture, ModelError> {
180    let path = concat!(
181        env!("CARGO_MANIFEST_DIR"),
182        "/../nabled/tests/fixtures/physical_ai/six_dof_dh.json"
183    );
184    SixDofDhFixture::from_file(path)
185}
186
187#[derive(Debug, Deserialize)]
188pub struct YBranchFixture {
189    pub description: String,
190    pub cases:       Vec<YBranchCase>,
191}
192
193#[derive(Debug, Deserialize)]
194pub struct YBranchCase {
195    pub name:                 String,
196    pub q:                    Vec<f64>,
197    pub left_ee_translation:  Vec<f64>,
198    pub right_ee_translation: Vec<f64>,
199}
200
201impl YBranchFixture {
202    /// Load fixture from JSON file path.
203    pub fn from_file(path: &str) -> Result<Self, ModelError> {
204        let content = std::fs::read_to_string(path)
205            .map_err(|err| ModelError::ParseError(format!("failed to read {path}: {err}")))?;
206        serde_json::from_str(&content)
207            .map_err(|err| ModelError::ParseError(format!("invalid JSON: {err}")))
208    }
209}
210
211/// Load the Y-branch tree fixture used by integration test S22.
212pub fn load_y_branch_json() -> Result<YBranchFixture, ModelError> {
213    let path =
214        concat!(env!("CARGO_MANIFEST_DIR"), "/../nabled/tests/fixtures/physical_ai/y_branch.json");
215    YBranchFixture::from_file(path)
216}
217
218#[cfg(test)]
219mod tests {
220    use super::*;
221
222    #[test]
223    fn load_planar2r_fixture_from_repo() {
224        let fixture = load_planar2r_json().unwrap();
225        assert_eq!(fixture.dh_params.len(), 2);
226        let chain = fixture.to_chain_spec::<f64>().unwrap();
227        assert_eq!(chain.num_joints(), 2);
228    }
229
230    #[test]
231    fn load_six_dof_fixture_from_repo() {
232        let fixture = load_six_dof_dh_json().unwrap();
233        assert_eq!(fixture.dh_params.len(), 6);
234        let chain = fixture.to_chain_spec::<f64>().unwrap();
235        assert_eq!(chain.num_joints(), 6);
236    }
237
238    #[test]
239    fn load_y_branch_fixture_from_repo() {
240        let fixture = load_y_branch_json().unwrap();
241        assert_eq!(fixture.cases.len(), 2);
242        assert_eq!(fixture.cases[0].q.len(), 3);
243        assert_eq!(fixture.cases[0].left_ee_translation.len(), 3);
244        assert_eq!(fixture.cases[0].right_ee_translation.len(), 3);
245    }
246
247    #[test]
248    fn planar2r_to_robot_model_has_inertials() {
249        let fixture = load_planar2r_json().unwrap();
250        let model = fixture.to_robot_model::<f64>().unwrap();
251        assert_eq!(model.dof(), 2);
252        let body = model.joint(0).unwrap();
253        assert!(body.inertial.is_some());
254    }
255
256    #[test]
257    fn from_file_reports_missing_path() {
258        let err = Planar2rFixture::from_file("/no/such/fixture.json").unwrap_err();
259        assert!(
260            matches!(err, ModelError::ParseError(message) if message.contains("failed to read"))
261        );
262    }
263
264    #[test]
265    fn to_chain_spec_rejects_unknown_dh_convention() {
266        let mut fixture = load_planar2r_json().unwrap();
267        fixture.dh_convention = "bogus".to_string();
268        assert!(matches!(fixture.to_chain_spec::<f64>(), Err(ModelError::InvalidInput(_))));
269    }
270}
nabled_model/fixture.rs

nabled_model/
fixture.rs
