From 3d02b45bee3b75197646ca582c75256777aaa738 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Wed, 2 Sep 2026 20:04:45 -0700 Subject: [PATCH 1/4] fix(sim): fix asset masses --- assets/cameras/zed_mini/zed_mini.xml | 5 +++-- assets/objects/droid_wrist_mount/droid_wrist_mount.xml | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/assets/cameras/zed_mini/zed_mini.xml b/assets/cameras/zed_mini/zed_mini.xml index d755585b..078e0637 100644 --- a/assets/cameras/zed_mini/zed_mini.xml +++ b/assets/cameras/zed_mini/zed_mini.xml @@ -23,8 +23,9 @@ - - + + + - + + From 89a4ceb6a513407cdc6fb589ae56c6bdde257b5b Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Wed, 2 Sep 2026 20:47:00 -0700 Subject: [PATCH 2/4] feat(composer): add gravcomp ignore --- python/rcs/envs/configs.py | 1 + python/rcs/envs/scenes.py | 9 +++++++++ python/rcs/sim/composer.py | 11 +++++++++-- 3 files changed, 19 insertions(+), 2 deletions(-) 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 16ca88f9..ae8ba871 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 From 0bc87b28210d876dcef1601a761ae5173dd59b15 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Thu, 3 Sep 2026 13:43:51 -0700 Subject: [PATCH 3/4] feat(sim): fr3 sysid --- assets/robots/fr3/fr3.xml | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/assets/robots/fr3/fr3.xml b/assets/robots/fr3/fr3.xml index 409bb62b..50652d50 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 @@ - + From e220433240d4ce52fe3320e685b87f8d160610e5 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Thu, 3 Sep 2026 20:51:32 -0700 Subject: [PATCH 4/4] fix(assets): zed intrinsics --- assets/cameras/zed2i/zed2i.xml | 15 ++++++++++---- assets/cameras/zed_mini/zed_mini.xml | 29 +++++++++++++++++++--------- 2 files changed, 31 insertions(+), 13 deletions(-) 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 078e0637..e73b5c50 100644 --- a/assets/cameras/zed_mini/zed_mini.xml +++ b/assets/cameras/zed_mini/zed_mini.xml @@ -23,18 +23,29 @@ - + + - - - + - + - - +