fix: restore rotation_q_wxyz helper usage

Author: juelg

Makefile

@@ -33,7 +33,7 @@ stubgen:
 	find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
 	find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
 	sed -i 's/    q_home: numpy\.ndarray\[tuple\[M\], numpy\.dtype\[numpy\.float64\]\] | None/    q_home: numpy.ndarray | None/' python/rcs/_core/common.pyi
-	python -c "from pathlib import Path; p=Path('python/rcs/_core/common.pyi'); lines=p.read_text().splitlines(); lines=[line for line in lines if 'def rotation_q_wxyz(' not in line]; t='\n'.join(lines)+'\n'; t=t.replace('numpy.ndarray[tuple[typing.Literal[2], N], numpy.dtype[numpy.float64]]', 'numpy.ndarray[tuple[typing.Literal[2], typing.Any], numpy.dtype[numpy.float64]]'); p.write_text(t)"
+	python -c "from pathlib import Path; p=Path('python/rcs/_core/common.pyi'); t=p.read_text(); t=t.replace('numpy.ndarray[tuple[typing.Literal[2], N], numpy.dtype[numpy.float64]]', 'numpy.ndarray[tuple[typing.Literal[2], typing.Any], numpy.dtype[numpy.float64]]'); p.write_text(t)"
 	python -c "from pathlib import Path; p=Path('python/rcs/_core/sim.pyi'); t=p.read_text(); t=t.replace('numpy.ndarray[tuple[typing.Literal[2], N], numpy.dtype[numpy.float64]]', 'numpy.ndarray[tuple[typing.Literal[2], typing.Any], numpy.dtype[numpy.float64]]'); t=t.replace(', max_buffer_frames: int = 100', ''); p.write_text(t)"
 	python scripts/generate_common_typing.py
 	ruff check --fix python/rcs/_core python/rcs/common_typing.py

python/rcs/_core/common.pyi

@@ -197,6 +197,7 @@ class Pose:
     def pose_matrix(self) -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
     def rotation_m(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
     def rotation_q(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
+    def rotation_q_wxyz(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
     def rotation_rpy(self) -> RPY: ...
     def rotvec(self) -> numpy.ndarray[tuple[typing.Literal[6]], numpy.dtype[numpy.float64]]: ...
     def total_angle(self) -> float: ...

python/rcs/envs/tasks.py

@@ -132,10 +132,8 @@ def reset(
         pose_in_world_frame = self.center2world * pose_in_center_frame
 
         # qpos array format for a free joint: [x, y, z, qw, qx, qy, qz]
-        quat_xyzw = pose_in_world_frame.rotation_q()
-        quat_wxyz = np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]])
         self.get_wrapper_attr("sim").data.joint(self.obj_joint_name).qpos = np.append(
-            pose_in_world_frame.translation(), quat_wxyz
+            pose_in_world_frame.translation(), pose_in_world_frame.rotation_q_wxyz()
         )
 
         # reset of remaining stack, must happen after our reset!

python/rcs/sim/composer.py

@@ -65,8 +65,7 @@ def _find_camera(self, name: str) -> Optional[mujoco._specs.MjsCamera]:
 
     def _apply_pose(self, body: mujoco._specs.MjsBody, pose: Pose):
         body.pos = list(pose.translation())
-        quat_xyzw = pose.rotation_q()
-        body.quat = [quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]
+        body.quat = list(pose.rotation_q_wxyz())
 
     def add_camera(
         self,