"""Headless dynamics smoke test for the canonical slave URDF."""

from __future__ import annotations

import unittest
from pathlib import Path

import numpy as np
import pinocchio as pin


CONFIG_ROOT = Path(__file__).resolve().parents[1] / "config"
SLAVE_URDF = CONFIG_ROOT / "real_slave_7dof.urdf"
SLAVE_JOINT_NAMES = (
    "R_SHOULDER_P",
    "R_SHOULDER_R",
    "R_SHOULDER_Y",
    "R_ELBOW_R",
    "R_WRIST_P",
    "R_WRIST_Y",
    "R_WRIST_R",
)


class SlavePinocchioModelTest(unittest.TestCase):
    def test_canonical_model_dynamics_are_finite(self) -> None:
        model = pin.buildModelFromUrdf(str(SLAVE_URDF))
        data = model.createData()
        self.assertEqual(model.nq, 7)
        self.assertEqual(model.nv, 7)
        for name in SLAVE_JOINT_NAMES:
            self.assertGreater(int(model.getJointId(name)), 0, name)

        q = pin.neutral(model)
        qd = np.zeros(model.nv)
        mass = pin.crba(model, data, q)
        nonlinear = pin.nonLinearEffects(model, data, q, qd)
        frame_id = int(model.getFrameId("R_WRIST_R_S"))
        jacobian = pin.computeFrameJacobian(
            model,
            data,
            q,
            frame_id,
            pin.ReferenceFrame.WORLD,
        )

        self.assertEqual(mass.shape, (7, 7))
        self.assertEqual(nonlinear.shape, (7,))
        self.assertEqual(jacobian.shape, (6, 7))
        self.assertTrue(np.all(np.isfinite(mass)))
        self.assertTrue(np.all(np.isfinite(nonlinear)))
        self.assertTrue(np.all(np.isfinite(jacobian)))


if __name__ == "__main__":
    unittest.main()