diff --git a/assets/cameras/zed2i/zed2i.xml b/assets/cameras/zed2i/zed2i.xml
index 80cc0abdb..e8a4d8ffa 100644
--- a/assets/cameras/zed2i/zed2i.xml
+++ b/assets/cameras/zed2i/zed2i.xml
@@ -25,14 +25,21 @@
-
+ Full intrinsics (not just fovy) so the sim matches the real off-center principal
+ point (cx=723.64, cy=406.80 vs image centre 640,360 -- a ~84px/47px offset that a
+ fovy-only camera ignores and the extrinsic cannot absorb). MuJoCo intrinsic model:
+ sensorsize (arbitrary 16:9 -> cancels) + resolution + focalpixel(fx,fy) +
+ principalpixel. NOTE MuJoCo's principalpixel is +X-LEFT, +Y-UP, so:
+ principalpixel = [-(cx - W/2), -(cy - H/2)] = [-83.64, -46.80]
+ (verified by rendering: a point on the optical axis lands at (723,407)). -->
-
+
diff --git a/assets/cameras/zed_mini/zed_mini.xml b/assets/cameras/zed_mini/zed_mini.xml
index d755585b5..e73b5c504 100644
--- a/assets/cameras/zed_mini/zed_mini.xml
+++ b/assets/cameras/zed_mini/zed_mini.xml
@@ -23,17 +23,29 @@
-
-
-
-
+
+
+
+
+
-
+
-
-
+
diff --git a/assets/objects/droid_wrist_mount/droid_wrist_mount.xml b/assets/objects/droid_wrist_mount/droid_wrist_mount.xml
index 9bfc7eb26..2205cb0d9 100644
--- a/assets/objects/droid_wrist_mount/droid_wrist_mount.xml
+++ b/assets/objects/droid_wrist_mount/droid_wrist_mount.xml
@@ -22,9 +22,10 @@
-
+
-
+
+
diff --git a/assets/robots/fr3/fr3.xml b/assets/robots/fr3/fr3.xml
index 409bb62b7..50652d50d 100644
--- a/assets/robots/fr3/fr3.xml
+++ b/assets/robots/fr3/fr3.xml
@@ -5,7 +5,7 @@
-
+
@@ -80,34 +80,34 @@
-
+
-
+
-
+
-
+
-
+
@@ -115,8 +115,8 @@
-
+
@@ -129,8 +129,8 @@
-
+
diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py
index da792e64d..24a1aecd2 100644
--- a/python/rcs/envs/configs.py
+++ b/python/rcs/envs/configs.py
@@ -214,6 +214,7 @@ def config(self) -> SimEnvCreatorConfig:
)
},
}
+ cfg.gravcomp_ignore = {"wrist", "zed_mount"}
cfg.camera_adds = {
"wrist": CameraAdderConfig(
xml_path=CAMERA_PATHS["zed_mini"],
diff --git a/python/rcs/envs/scenes.py b/python/rcs/envs/scenes.py
index 16ca88f9d..ae8ba8718 100644
--- a/python/rcs/envs/scenes.py
+++ b/python/rcs/envs/scenes.py
@@ -106,6 +106,11 @@ class SimEnvCreatorConfig(typing.Generic[TaskConfig]):
"""shared base frame is a common reference frame for all robots in the scene and the origin for all actions and observations, e.g. the middle of franka duo
thus this transformation defines the offset of each robot's base to this shared base frame."""
add_gravcomp: bool = False
+ gravcomp_ignore: set[str] = field(default_factory=set)
+ """ids of attached cameras (``camera_adds`` keys) and robot-frame objects (``robot_frame_objects`` ids)
+ that should NOT be gravity-compensated even when ``add_gravcomp`` is True, e.g. a wrist camera and its
+ mount that the real robot's controller does not compensate. The simulated arm then carries them as an
+ uncompensated load, matching the hardware."""
wrapper_cfg: WrapperConfig = field(default_factory=WrapperConfig)
headless: bool = False
shared_base_frame_to_root_frame: rcs.common.Pose = field(default_factory=rcs.common.Pose)
@@ -322,6 +327,10 @@ def create_model(self, cfg: SimEnvCreatorConfig) -> MjModel:
),
)
+ # attachments the real robot's controller does not gravity-compensate (prefix = id + "_")
+ if cfg.gravcomp_ignore:
+ composer.ignore_gravcomp({f"{object_id}_" for object_id in cfg.gravcomp_ignore})
+
return composer
def create_env_from_model(self, cfg: SimEnvCreatorConfig, mjmodel: MjModel) -> gym.Env:
diff --git a/python/rcs/sim/composer.py b/python/rcs/sim/composer.py
index 48a42155b..6f7488fdc 100644
--- a/python/rcs/sim/composer.py
+++ b/python/rcs/sim/composer.py
@@ -20,6 +20,7 @@ def __init__(
self.spec.compiler.autolimits = True
self.add_gravcomp = add_gravcomp
self._gravcomp_prefixes: set[str] = set()
+ self._gravcomp_ignore_prefixes: set[str] = set()
self._root_relative_replay_free_joints: set[str] = set()
def _resolve_asset_paths(self, spec: mujoco.MjSpec, xml_path: str):
@@ -72,6 +73,9 @@ def _prefixed_free_joint_names(self, spec: mujoco.MjSpec, prefix: str) -> list[s
free_joint_type = int(mujoco.mjtJoint.mjJNT_FREE)
return [f"{prefix}{joint.name}" for joint in spec.joints if joint.name and int(joint.type) == free_joint_type]
+ def ignore_gravcomp(self, prefixes: set[str]):
+ self._gravcomp_ignore_prefixes.update(prefixes)
+
def register_root_relative_replay_free_joints(self, joint_names: list[str]):
self._root_relative_replay_free_joints.update(joint_names)
@@ -329,10 +333,13 @@ def _apply_gravcomp(self):
if not self.add_gravcomp or not self._gravcomp_prefixes:
return
+ def _ignored(name: str) -> bool:
+ return any(name.startswith(prefix) for prefix in self._gravcomp_ignore_prefixes)
+
for body in self.spec.bodies:
- if body.name and any(body.name.startswith(prefix) for prefix in self._gravcomp_prefixes):
+ if body.name and any(body.name.startswith(prefix) for prefix in self._gravcomp_prefixes) and not _ignored(body.name):
body.gravcomp = 1
for joint in self.spec.joints:
- if joint.name and any(joint.name.startswith(prefix) for prefix in self._gravcomp_prefixes):
+ if joint.name and any(joint.name.startswith(prefix) for prefix in self._gravcomp_prefixes) and not _ignored(joint.name):
joint.actgravcomp = True