Feat/sim backend kinematics api

src/unilab/base/backend/base.py

@@ -6,6 +6,24 @@
 class SimBackend(abc.ABC):
     """仿真后端统一接口"""
 
+    # ------------------------------------------------------------------ #
+    # Properties                                                           #
+    # ------------------------------------------------------------------ #
+
+    @property
+    @abc.abstractmethod
+    def num_envs(self) -> int:
+        """环境数量"""
+
+    @property
+    @abc.abstractmethod
+    def model(self):
+        """底层物理模型"""
+
+    # ------------------------------------------------------------------ #
+    # Simulation control                                                   #
+    # ------------------------------------------------------------------ #
+
     @abc.abstractmethod
     def step(self, ctrl: np.ndarray, nsteps: int = 1) -> None:
         """执行物理步进
@@ -15,6 +33,63 @@ def step(self, ctrl: np.ndarray, nsteps: int = 1) -> None:
             nsteps: 步进次数
         """
 
+    @abc.abstractmethod
+    def set_state(self, env_indices: np.ndarray, qpos: np.ndarray, qvel: np.ndarray) -> None:
+        """设置指定环境的物理状态
+
+        Args:
+            env_indices: 环境索引
+            qpos: 位置状态
+            qvel: 速度状态
+        """
+
+    # ------------------------------------------------------------------ #
+    # Base kinematics                                                      #
+    # ------------------------------------------------------------------ #
+
+    @abc.abstractmethod
+    def get_base_pos(self) -> np.ndarray:
+        """获取 base 在世界系下的位置
+
+        Returns:
+            (num_envs, 3)
+        """
+
+    @abc.abstractmethod
+    def get_base_quat(self) -> np.ndarray:
+        """获取 base 在世界系下的四元数（wxyz）
+
+        Returns:
+            (num_envs, 4)
+        """
+
+    @abc.abstractmethod
+    def get_base_lin_vel(self) -> np.ndarray:
+        """获取 base 在世界系下的线速度
+
+        即广义速度 qvel 的前 3 维，表达在世界坐标系中。
+
+        Returns:
+            (num_envs, 3)
+        """
+
+    @abc.abstractmethod
+    def get_base_ang_vel(self) -> np.ndarray:
+        """获取 base 在世界系下的角速度
+
+        即广义速度 qvel 的第 3-5 维，表达在世界坐标系中。
+        注意与陀螺仪（gyro）读数的区别：陀螺仪返回的是角速度在 body/sensor
+        局部坐标系下的分量（即 body frame 表达），而本接口返回的是世界系表达。
+        若需要 body frame 下的角速度，请使用对应的传感器接口gyro。
+
+        Returns:
+            (num_envs, 3)
+        """
+
+    # ------------------------------------------------------------------ #
+    # DOF state                                                            #
+    # ------------------------------------------------------------------ #
+
     @abc.abstractmethod
     def get_dof_pos(self) -> np.ndarray:
         """获取关节位置（不含 base）
@@ -31,41 +106,113 @@ def get_dof_vel(self) -> np.ndarray:
             (num_envs, num_dof)
         """
 
+    # ------------------------------------------------------------------ #
+    # Body kinematics — world frame                                        #
+    # ------------------------------------------------------------------ #
+
     @abc.abstractmethod
-    def get_qpos(self) -> np.ndarray:
-        """获取完整位置状态（含 base）
+    def get_body_pos_w(self, body_ids: np.ndarray) -> np.ndarray:
+        """获取指定 body 在世界系下的位置
+
+        Args:
+            body_ids: body 索引数组
 
         Returns:
-            (num_envs, nq)
+            (num_envs, len(body_ids), 3)
         """
 
     @abc.abstractmethod
-    def get_sensor_data(self, name: str) -> np.ndarray:
-        """获取传感器数据
+    def get_body_quat_w(self, body_ids: np.ndarray) -> np.ndarray:
+        """获取指定 body 在世界系下的四元数（wxyz）
 
         Args:
-            name: 传感器名称
+            body_ids: body 索引数组
 
         Returns:
-            传感器数据数组
+            (num_envs, len(body_ids), 4)
         """
 
     @abc.abstractmethod
-    def set_state(self, env_indices: np.ndarray, qpos: np.ndarray, qvel: np.ndarray) -> None:
-        """设置指定环境的物理状态
+    def get_body_lin_vel_w(self, body_ids: np.ndarray) -> np.ndarray:
+        """获取指定 body 在世界系下的线速度
 
         Args:
-            env_indices: 环境索引
-            qpos: 位置状态
-            qvel: 速度状态
+            body_ids: body 索引数组
+
+        Returns:
+            (num_envs, len(body_ids), 3)
         """
 
-    @property
     @abc.abstractmethod
-    def num_envs(self) -> int:
-        """环境数量"""
+    def get_body_ang_vel_w(self, body_ids: np.ndarray) -> np.ndarray:
+        """获取指定 body 在世界系下的角速度
+
+        Args:
+            body_ids: body 索引数组
+
+        Returns:
+            (num_envs, len(body_ids), 3)
+        """
+
+    # ------------------------------------------------------------------ #
+    # Body kinematics — baselink frame                                     #
+    # ------------------------------------------------------------------ #
 
-    @property
     @abc.abstractmethod
-    def model(self):
-        """底层物理模型"""
+    def get_body_pos_b(self, body_ids: np.ndarray) -> np.ndarray:
+        """获取指定 body 在 baselink 系下的位置
+
+        Args:
+            body_ids: body 索引数组
+
+        Returns:
+            (num_envs, len(body_ids), 3)
+        """
+
+    @abc.abstractmethod
+    def get_body_quat_b(self, body_ids: np.ndarray) -> np.ndarray:
+        """获取指定 body 在 baselink 系下的四元数（wxyz）
+
+        Args:
+            body_ids: body 索引数组
+
+        Returns:
+            (num_envs, len(body_ids), 4)
+        """
+
+    @abc.abstractmethod
+    def get_body_lin_vel_b(self, body_ids: np.ndarray) -> np.ndarray:
+        """获取指定 body 在 baselink 系下的线速度
+
+        Args:
+            body_ids: body 索引数组
+
+        Returns:
+            (num_envs, len(body_ids), 3)
+        """
+
+    @abc.abstractmethod
+    def get_body_ang_vel_b(self, body_ids: np.ndarray) -> np.ndarray:
+        """获取指定 body 在 baselink 系下的角速度
+
+        Args:
+            body_ids: body 索引数组
+
+        Returns:
+            (num_envs, len(body_ids), 3)
+        """
+
+    # ------------------------------------------------------------------ #
+    # Sensors                                                              #
+    # ------------------------------------------------------------------ #
+
+    @abc.abstractmethod
+    def get_sensor_data(self, name: str) -> np.ndarray:
+        """获取传感器数据
+
+        Args:
+            name: 传感器名称
+
+        Returns:
+            传感器数据数组
+        """