From c41fa342f46a9977a982440c97a05ff4f1a30341 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 10 Jan 2025 16:29:18 +0100 Subject: [PATCH 1/2] Fix typo in `MujocoCamera` parameter name from `azimut` to `azimuth` --- examples/jaxsim_for_robot_controllers.ipynb | 2 +- src/jaxsim/mujoco/utils.py | 8 ++++---- src/jaxsim/mujoco/visualizer.py | 10 +++++----- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/examples/jaxsim_for_robot_controllers.ipynb b/examples/jaxsim_for_robot_controllers.ipynb index eb2a03c23..45321010c 100644 --- a/examples/jaxsim_for_robot_controllers.ipynb +++ b/examples/jaxsim_for_robot_controllers.ipynb @@ -187,7 +187,7 @@ " in_link_frame=False,\n", " ),\n", " distance=3,\n", - " azimut=150,\n", + " azimuth=150,\n", " elevation=-10,\n", " ),\n", ")\n", diff --git a/src/jaxsim/mujoco/utils.py b/src/jaxsim/mujoco/utils.py index cb0645a76..2bd6921f3 100644 --- a/src/jaxsim/mujoco/utils.py +++ b/src/jaxsim/mujoco/utils.py @@ -147,7 +147,7 @@ def build_from_target_view( camera_name: str, lookat: Sequence[float | int] | npt.NDArray = (0, 0, 0), distance: float | int | npt.NDArray = 3, - azimut: float | int | npt.NDArray = 90, + azimuth: float | int | npt.NDArray = 90, elevation: float | int | npt.NDArray = -45, fovy: float | int | npt.NDArray = 45, degrees: bool = True, @@ -170,7 +170,7 @@ def build_from_target_view( distance: The distance from the target point (displacement between the origins of `T` and `C`). - azimut: + azimuth: The rotation around z of the camera. With an angle of 0, the camera would loot at the target point towards the positive x-axis of `T`. elevation: @@ -193,8 +193,8 @@ def build_from_target_view( seq="ZX", angles=[-90, 90], degrees=True ).as_matrix() - # Process the azimut. - R_az = Rotation.from_euler(seq="Y", angles=azimut, degrees=degrees).as_matrix() + # Process the azimuth. + R_az = Rotation.from_euler(seq="Y", angles=azimuth, degrees=degrees).as_matrix() W_H_C[0:3, 0:3] = W_H_C[0:3, 0:3] @ R_az # Process elevation. diff --git a/src/jaxsim/mujoco/visualizer.py b/src/jaxsim/mujoco/visualizer.py index 5ea83786d..46bff8482 100644 --- a/src/jaxsim/mujoco/visualizer.py +++ b/src/jaxsim/mujoco/visualizer.py @@ -178,7 +178,7 @@ def open( close_on_exit: bool = True, lookat: Sequence[float | int] | npt.NDArray | None = None, distance: float | int | npt.NDArray | None = None, - azimut: float | int | npt.NDArray | None = None, + azimuth: float | int | npt.NDArray | None = None, elevation: float | int | npt.NDArray | None = None, ) -> contextlib.AbstractContextManager[mujoco.viewer.Handle]: """ @@ -195,7 +195,7 @@ def open( viewer=handle, lookat=lookat, distance=distance, - azimut=azimut, + azimuth=azimuth, elevation=elevation, ) @@ -210,7 +210,7 @@ def setup_viewer_camera( *, lookat: Sequence[float | int] | npt.NDArray | None, distance: float | int | npt.NDArray | None = None, - azimut: float | int | npt.NDArray | None = None, + azimuth: float | int | npt.NDArray | None = None, elevation: float | int | npt.NDArray | None = None, ) -> mj.viewer.Handle: """ @@ -236,8 +236,8 @@ def setup_viewer_camera( if distance is not None: viewer.cam.distance = float(distance) - if azimut is not None: - viewer.cam.azimuth = float(azimut) % 360 + if azimuth is not None: + viewer.cam.azimuth = float(azimuth) % 360 if elevation is not None: viewer.cam.elevation = float(elevation) From 4965926f44d08871a4c3feffc4fc32205f988b72 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 10 Jan 2025 16:29:53 +0100 Subject: [PATCH 2/2] Add unit tests for `ModelToMjcf` conversion --- tests/test_visualizer.py | 64 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 tests/test_visualizer.py diff --git a/tests/test_visualizer.py b/tests/test_visualizer.py new file mode 100644 index 000000000..6643bb6b4 --- /dev/null +++ b/tests/test_visualizer.py @@ -0,0 +1,64 @@ +import pytest +import rod + +from jaxsim.mujoco import ModelToMjcf +from jaxsim.mujoco.loaders import MujocoCamera + + +@pytest.fixture +def mujoco_camera(): + + return MujocoCamera.build_from_target_view( + camera_name="test_camera", + lookat=(0, 0, 0), + distance=1, + azimuth=0, + elevation=0, + fovy=45, + degrees=True, + ) + + +def test_urdf_loading(jaxsim_model_single_pendulum, mujoco_camera): + model = jaxsim_model_single_pendulum.built_from + + _ = ModelToMjcf.convert(model=model, cameras=mujoco_camera) + + +def test_sdf_loading(jaxsim_model_single_pendulum, mujoco_camera): + + model = rod.Sdf.load(sdf=jaxsim_model_single_pendulum.built_from).serialize( + pretty=True + ) + + _ = ModelToMjcf.convert(model=model, cameras=mujoco_camera) + + +def test_rod_loading(jaxsim_model_single_pendulum, mujoco_camera): + + model = rod.Sdf.load(sdf=jaxsim_model_single_pendulum.built_from).models()[0] + + _ = ModelToMjcf.convert(model=model, cameras=mujoco_camera) + + +def test_heightmap(jaxsim_model_single_pendulum, mujoco_camera): + + model = rod.Sdf.load(sdf=jaxsim_model_single_pendulum.built_from).models()[0] + + _ = ModelToMjcf.convert( + model=model, + cameras=mujoco_camera, + heightmap=True, + heightmap_samples_xy=(51, 51), + ) + + +def test_inclined_plane(jaxsim_model_single_pendulum, mujoco_camera): + + model = rod.Sdf.load(sdf=jaxsim_model_single_pendulum.built_from).models()[0] + + _ = ModelToMjcf.convert( + model=model, + cameras=mujoco_camera, + plane_normal=(0.3, 0.3, 0.3), + )