allenai / allenai/molmospaces

RBY1 holonomic base under-rotates after theta target mapping change

Abierto
#175 1 comentario 0 reacciones 0 asignados Ver en GitHub
Lenguaje dominante
Python
Estrellas
470
Forks
72
Merge medio
5 d 14 h
PR fusionados (30 d)
5

Descripción

### Description

The RBY1 holonomic base no longer tracks repeated yaw position targets with the same behavior as the earlier implementation. This causes continuous rotations, such as a 360-degree observation scan, to under-rotate even though every control command is accepted and applied.

### Reproduction

Using RBY1 with `holo_joint_planar_position` control and repeatedly commanding an angular velocity of `1.25 rad/s` with a `0.2 s` policy step:

- 40 commands are dispatched and acknowledged.
- No control steps are missed.
- The accumulated odometry yaw is only about `1.14 rad`.
- A 360-degree scan therefore cannot reach `2π`.

The earlier RBY1 implementation completes the same rotation in 27 acknowledged steps, accumulating about `6.50 rad`.

### Expected behavior

Repeated yaw targets should produce continuous rotation. With the configuration above, the base should accumulate approximately `0.25 rad` per policy step and complete a full rotation in roughly 26–27 steps.

### Suspected cause

`HoloJointsRobotBaseGroup.ctrl` previously normalized the theta target to `[-π, π]` and moved the theta joint qpos to the equivalent angular branch when the target crossed the `±π` boundary. The newer implementation wraps the target relative to the current joint yaw but does not perform the old qpos branch synchronization.

This leaves the MuJoCo hinge state and actuator target on incompatible angular branches during continuous rotation. The actuator commands are accepted, but the physical yaw advances much less than requested.

### Verified workaround

Restoring the previous theta mapping behavior fixes the regression:

```python
ctrl[2] = normalize_ang_error(ctrl[2])
theta_qpos_idx = self.mj_model.jnt_qposadr[self._joint_ids[2]]
current_theta = self.mj_data.qpos[theta_qpos_idx]
if abs(current_theta - ctrl[2]) > np.pi:
self.mj_data.qpos[theta_qpos_idx] = ctrl[2]
```

After restoring this behavior, the same test accumulates `6.45 rad` in 27 acknowledged steps and the 360-degree scan completes normally.

Guía de contribución

No hay ninguna guía de contribución indexada para este repositorio

Evaluación

Este issue todavía no se ha evaluado.

Recibe los nuevos issues en tu correo

Un resumen breve de issues de GitHub para principiantes.