Skip to content

fix(sim): gravcomp ignore and fix masses #337

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
juelg wants to merge 5 commits into
base: master
Choose a base branch
from
Open

fix(sim): gravcomp ignore and fix masses #337

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
af29a57
fix(sim): fix asset masses
juelg Sep 3, 2026
202ae0f
feat(composer): add gravcomp ignore
juelg Sep 3, 2026
d2449a4
feat(sim): fr3 sysid
juelg Sep 3, 2026
4431375
fix(assets): zed intrinsics
juelg Sep 4, 2026
8197725
Merge branch 'master' into juelg/gravcomp-fix
juelg Sep 8, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 11 additions & 4 deletions assets/cameras/zed2i/zed2i.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,14 +25,21 @@
<body name="zed2i" childclass="zed2i">
<geom mesh="zed2i" quat="0 0 1 0" material="black" class="visual"/>
<geom mesh="zed2i_coll" quat="0 0 1 0" material="black" class="collision"/>
<!-- from real camera after calibration
<!-- from real camera after calibration (720p / 1280x720)
[786.14367676 0. 723.64068604 0. ]
[ 0. 786.14367676 406.79891968 0. ]
[ 0. 0. 1. 0. ]]
>>> np.arctan(720/(2*786.14367676 ))*360/np.pi
np.float64(49.209076483949794) -->
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)). -->

<camera name="camera" pos="0 -0.02 0.005" euler="-1.5707963267948966 0 0" fovy="49"/>
<camera name="camera" pos="0 -0.02 0.005" euler="-1.5707963267948966 0 0"
resolution="1280 720" sensorsize="0.005325 0.0029953"
focalpixel="786.14367676 786.14367676" principalpixel="-83.6406860 -46.7989197"/>
</body>
</worldbody>

Expand Down
30 changes: 21 additions & 9 deletions assets/cameras/zed_mini/zed_mini.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,17 +23,29 @@

<worldbody>
<body name="zed_mini" childclass="zed_mini">
<geom mesh="zed_mini" quat="1 0 0 0" material="black" class="visual"/>
<geom mesh="zed_mini_coll" quat="1 0 0 0" material="black" class="collision"/>
<!-- intrinsic matrix from real camera -->
<!-- [[733.30859375 0. 645.03240967 0. ]
[ 0. 733.30859375 358.78286743 0. ]
<!-- Housing geoms shifted by G = inv(_WRIST_CAM_OFFSET) @ O_orig, where O_orig is the
ORIGINAL manual wrist placement (single_arm.py, pre-hand-eye: translation
[-0.077,0.009,-0.008], rpy deg [0,-70,0]) at which the mesh sat correctly in the
mount. This puts the housing back where it belongs while the <camera> below stays
at the new calibrated optical pose. Purely visual/collision -- does NOT affect the
camera image or _WRIST_CAM_OFFSET (those depend only on the <camera> pose). -->
<geom mesh="zed_mini" pos="-0.029645 0.002464 -0.004295" quat="0.999847 0.011973 0.010415 -0.00733" material="black" class="visual" mass="0"/>
<!-- Real measured mass 56.9 g; COM/inertia derived from the mesh at this mass. -->
<geom mesh="zed_mini_coll" pos="-0.029645 0.002464 -0.004295" quat="0.999847 0.011973 0.010415 -0.00733" material="black" class="collision" mass="0.0569"/>
<!-- intrinsic matrix from real camera (serial 14943057, ZED_M, 1280x720) -->
<!-- [[740.23132324 0. 602.53009033 0. ]
[ 0. 740.23132324 357.18844604 0. ]
[ 0. 0. 1. 0. ]] -->
<!-- >>> np.arctan(720/(2*733.30859375))*360/np.pi
np.float64(52.295211753153616) -->
<!-- Full intrinsics (not just fovy) so the sim matches the real off-center principal
point (cx=602.53, cy=357.19 vs centre 640,360 -- ~37px horizontal offset a fovy-only
camera ignores). MuJoCo principalpixel is +X-LEFT, +Y-UP:
principalpixel = [-(cx - W/2), -(cy - H/2)] = [37.47, 2.81]
(verified by rendering: on-axis point lands at (602,357)). pos/quat unchanged so
_WRIST_CAM_OFFSET / the mesh offset are unaffected. -->


<camera name="camera" pos="0 0.0255 0" quat="0.5 0.5 -0.5 -0.5" fovy="52"/>
<camera name="camera" pos="0 0.0255 0" quat="0.5 0.5 -0.5 -0.5"
resolution="1280 720" sensorsize="0.005325 0.0029953"
focalpixel="740.23132324 740.23132324" principalpixel="37.4699097 2.8115540"/>
</body>
</worldbody>

Expand Down
5 changes: 3 additions & 2 deletions assets/objects/droid_wrist_mount/droid_wrist_mount.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,10 @@

<worldbody>
<body name="base" childclass="wrist_camera_setup">
<geom mesh="wrist_camera_setup" quat="1 0 0 0" material="black" class="visual"/>
<geom mesh="wrist_camera_setup" quat="1 0 0 0" material="black" class="visual" mass="0"/>
<!-- <geom mesh="wrist_camera_setup" quat="1 0 0 0" material="black" class="collision"/> -->
<geom mesh="wrist_camera_setup_collision" quat="1 0 0 0" material="black" class="collision"/>
<!-- Real measured mass 47.7 g; COM/inertia derived from the mesh at this mass. -->
<geom mesh="wrist_camera_setup_collision" quat="1 0 0 0" material="black" class="collision" mass="0.0477"/>
</body>
</worldbody>

Expand Down
16 changes: 8 additions & 8 deletions assets/robots/fr3/fr3.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@

<default>
<default class="fr3">
<!-- Overwritten in joints 5, 6, and 7. -->
<!-- Default; every arm joint below overrides these with sysid-calibrated values. -->
<joint damping="0.21" armature="0.195" frictionloss="1.137"/>
<position inheritrange="1"/>
<default class="visual">
Expand Down Expand Up @@ -80,34 +80,34 @@
<body name="fr3_link1" pos="0 0 0.333">
<inertial pos="4.128e-07 -0.0181251 -0.0386036" quat="0.998098 -0.0605364 0.00380499 0.0110109" mass="2.92747"
diaginertia="0.0239286 0.0227246 0.00610634"/>
<joint name="fr3_joint1" axis="0 0 1" range="-2.7437 2.7437" actuatorfrcrange="-87 87"/>
<joint name="fr3_joint1" axis="0 0 1" range="-2.7437 2.7437" actuatorfrcrange="-87 87" frictionloss="1.819" damping="0.910" armature="0.546"/>
<geom name="fr3_link1_collision" class="collision" mesh="link1_coll"/>
<geom material="white" mesh="link1" class="visual"/>
<body name="fr3_link2" quat="1 -1 0 0">
<inertial pos="0.00318289 -0.0743222 0.00881461" quat="0.502599 0.584437 -0.465998 0.434366" mass="2.93554"
diaginertia="0.0629567 0.0411924 0.0246371"/>
<joint name="fr3_joint2" axis="0 0 1" range="-1.7837 1.7837" actuatorfrcrange="-87 87"/>
<joint name="fr3_joint2" axis="0 0 1" range="-1.7837 1.7837" actuatorfrcrange="-87 87" frictionloss="1.478" damping="0.910" armature="0.546"/>
<geom material="white" mesh="link2" class="visual"/>
<geom name="fr3_link2_collision" class="collision" mesh="link2_coll"/>
<body name="fr3_link3" pos="0 -0.316 0" quat="1 1 0 0">
<inertial pos="0.0407016 -0.00482006 -0.0289731" quat="0.921025 -0.244161 0.155272 0.260745" mass="2.2449"
diaginertia="0.0267409 0.0189869 0.0171587"/>
<joint name="fr3_joint3" axis="0 0 1" range="-2.9007 2.9007" actuatorfrcrange="-87 87"/>
<joint name="fr3_joint3" axis="0 0 1" range="-2.9007 2.9007" actuatorfrcrange="-87 87" frictionloss="1.819" damping="0.810" armature="0.546"/>
<geom mesh="link3_0" material="white" class="visual"/>
<geom mesh="link3_1" material="black" class="visual"/>
<geom name="fr3_link3_collision" class="collision" mesh="link3_coll"/>
<body name="fr3_link4" pos="0.0825 0 0" quat="1 1 0 0">
<inertial pos="-0.0459101 0.0630493 -0.00851879" quat="0.438018 0.803311 0.00937812 0.403414"
mass="2.6156" diaginertia="0.05139 0.0372717 0.0160047"/>
<joint name="fr3_joint4" axis="0 0 1" range="-3.0421 -0.1518" actuatorfrcrange="-87 87"/>
<joint name="fr3_joint4" axis="0 0 1" range="-3.0421 -0.1518" actuatorfrcrange="-87 87" frictionloss="1.819" damping="0.910" armature="0.546"/>
<geom mesh="link4_0" material="white" class="visual"/>
<geom mesh="link4_1" material="black" class="visual"/>
<geom name="fr3_link4_collision" class="collision" mesh="link4_coll"/>
<body name="fr3_link5" pos="-0.0825 0.384 0" quat="1 -1 0 0">
<inertial pos="-0.00160396 0.0292536 -0.0972966" quat="0.919031 0.125604 0.0751531 -0.366003"
mass="2.32712" diaginertia="0.0579335 0.0449144 0.0130634"/>
<joint name="fr3_joint5" axis="0 0 1" range="-2.8065 2.8065" actuatorfrcrange="-12 12"
armature="0.074" frictionloss="0.763"/>
frictionloss="1.679" damping="0.910" armature="0.254"/>
<geom mesh="link5_0" material="white" class="visual"/>
<geom mesh="link5_1" material="white" class="visual"/>
<geom mesh="link5_2" material="black" class="visual"/>
Expand All @@ -116,7 +116,7 @@
<inertial pos="0.0597131 -0.0410295 -0.0101693" quat="0.621301 0.552665 0.510011 0.220081"
mass="1.81704" diaginertia="0.0175039 0.0161123 0.00193529"/>
<joint name="fr3_joint6" axis="0 0 1" range="0.5445 4.5169" actuatorfrcrange="-12 12"
armature="0.074" frictionloss="0.44"/>
frictionloss="1.840" damping="0.910" armature="0.254"/>
<geom mesh="link6_0" material="button_green" class="visual"/>
<geom mesh="link6_1" material="white" class="visual"/>
<geom mesh="link6_2" material="white" class="visual"/>
Expand All @@ -130,7 +130,7 @@
<inertial pos="0.00452258 0.00862619 -0.0161633" quat="0.727579 0.0978688 -0.24906 0.63168"
mass="0.627143" diaginertia="0.000223836 0.000223642 5.64132e-07"/>
<joint name="fr3_joint7" axis="0 0 1" range="-3.0159 3.0159" actuatorfrcrange="-12 12"
armature="0.074" frictionloss="0.248"/>
frictionloss="1.448" damping="0.910" armature="0.254"/>
<geom mesh="link7_0" material="black" class="visual"/>
<geom mesh="link7_1" material="white" class="visual"/>
<geom mesh="link7_2" material="white" class="visual"/>
Expand Down
1 change: 1 addition & 0 deletions python/rcs/envs/configs.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"],
Expand Down
9 changes: 9 additions & 0 deletions python/rcs/envs/scenes.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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:
Expand Down
11 changes: 9 additions & 2 deletions python/rcs/sim/composer.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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
Loading