57 lines
1.5 KiB
Python
57 lines
1.5 KiB
Python
|
"""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()
|