diff --git a/assets/cameras/zed2i/zed2i.xml b/assets/cameras/zed2i/zed2i.xml
index 80cc0abd..e8a4d8ff 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 d755585b..e73b5c50 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 9bfc7eb2..2205cb0d 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 1e7d892d..50652d50 100644
--- a/assets/robots/fr3/fr3.xml
+++ b/assets/robots/fr3/fr3.xml
@@ -5,7 +5,7 @@
-
+
@@ -80,26 +80,26 @@
-
+
-
+
-
+
-
+
@@ -107,7 +107,7 @@
+ frictionloss="1.679" damping="0.910" armature="0.254"/>
@@ -116,7 +116,7 @@
+ frictionloss="1.840" damping="0.910" armature="0.254"/>
@@ -130,7 +130,7 @@
+ frictionloss="1.448" damping="0.910" armature="0.254"/>
diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py
index da792e64..24a1aecd 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 6cedeae3..8a1d506b 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 48a42155..6f7488fd 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