Revamped collision checking

jrl/robot.py

@@ -19,7 +19,6 @@
     quaternion_product,
     quaternion_to_rpy,
     rotation_matrix_to_quaternion,
-    quaternion_norm,
     DEFAULT_TORCH_DTYPE,
 )
 from jrl.config import DEVICE, PT_NP_TYPE
@@ -40,13 +39,6 @@ def _assert_is_2d(x: Union[torch.Tensor, np.ndarray]):
     assert isinstance(x, (torch.Tensor, np.ndarray)), f"Expected x to be a torch.Tensor or np.ndarray but got {type(x)}"
 
 
-def _assert_is_pose_matrix(poses: Union[torch.Tensor, np.ndarray]):
-    _assert_is_2d(poses)
-    assert poses.shape[1] == 7, f"Expected poses matrix to be [n x 7] but got {poses.shape}"
-    norms = quaternion_norm(poses[:, 3:7])
-    assert max(norms) < 1.01 and min(norms) > 0.99, "quaternion(s) are not unit quaternion(s)"
-
-
 def _assert_is_joint_angle_matrix(xs: Union[torch.Tensor, np.ndarray], ndof: int):
     _assert_is_2d(xs)
     assert xs.shape[1] == ndof, f"Expected matrix to be [n x ndof] ([{xs.shape[0]} x {ndof}]) but got {xs.shape}"

pyproject.toml

@@ -14,8 +14,10 @@ dependencies = [
     "pylint>=3.3.7",
     "pyqt5>=5.15.11",
     "pytest>=8.4.0",
+    "robot-descriptions>=1.20.0",
     "torch>=2.3.0",
     "tqdm>=4.67.1",
+    "viser>=1.0.6",
 ]
 
 [tool.setuptools.packages]