Skip to content

Commit a2bc231

Browse files
TATP-233TATP-233
authored
Merge pull request #42 from TATP-233/feat/sim-backend-kinematics-api
Feat/sim backend kinematics api
2 parents 67c62f0 + 88cb5aa commit a2bc231

18 files changed

Lines changed: 1440 additions & 402 deletions

File tree

‎src/unilab/base/backend/base.py‎

Lines changed: 165 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,24 @@
66
class SimBackend(abc.ABC):
77
"""仿真后端统一接口"""
88

9+
# ------------------------------------------------------------------ #
10+
# Properties #
11+
# ------------------------------------------------------------------ #
12+
13+
@property
14+
@abc.abstractmethod
15+
def num_envs(self) -> int:
16+
"""环境数量"""
17+
18+
@property
19+
@abc.abstractmethod
20+
def model(self):
21+
"""底层物理模型"""
22+
23+
# ------------------------------------------------------------------ #
24+
# Simulation control #
25+
# ------------------------------------------------------------------ #
26+
927
@abc.abstractmethod
1028
def step(self, ctrl: np.ndarray, nsteps: int = 1) -> None:
1129
"""执行物理步进
@@ -15,6 +33,63 @@ def step(self, ctrl: np.ndarray, nsteps: int = 1) -> None:
1533
nsteps: 步进次数
1634
"""
1735

36+
@abc.abstractmethod
37+
def set_state(self, env_indices: np.ndarray, qpos: np.ndarray, qvel: np.ndarray) -> None:
38+
"""设置指定环境的物理状态
39+
40+
Args:
41+
env_indices: 环境索引
42+
qpos: 位置状态
43+
qvel: 速度状态
44+
"""
45+
46+
# ------------------------------------------------------------------ #
47+
# Base kinematics #
48+
# ------------------------------------------------------------------ #
49+
50+
@abc.abstractmethod
51+
def get_base_pos(self) -> np.ndarray:
52+
"""获取 base 在世界系下的位置
53+
54+
Returns:
55+
(num_envs, 3)
56+
"""
57+
58+
@abc.abstractmethod
59+
def get_base_quat(self) -> np.ndarray:
60+
"""获取 base 在世界系下的四元数（wxyz）
61+
62+
Returns:
63+
(num_envs, 4)
64+
"""
65+
66+
@abc.abstractmethod
67+
def get_base_lin_vel(self) -> np.ndarray:
68+
"""获取 base 在世界系下的线速度
69+
70+
即广义速度 qvel 的前 3 维，表达在世界坐标系中。
71+
72+
Returns:
73+
(num_envs, 3)
74+
"""
75+
76+
@abc.abstractmethod
77+
def get_base_ang_vel(self) -> np.ndarray:
78+
"""获取 base 在世界系下的角速度
79+
80+
即广义速度 qvel 的第 3-5 维，表达在世界坐标系中。
81+
注意与陀螺仪（gyro）读数的区别：陀螺仪返回的是角速度在 body/sensor
82+
局部坐标系下的分量（即 body frame 表达），而本接口返回的是世界系表达。
83+
若需要 body frame 下的角速度，请使用对应的传感器接口gyro。
84+
85+
Returns:
86+
(num_envs, 3)
87+
"""
88+
89+
# ------------------------------------------------------------------ #
90+
# DOF state #
91+
# ------------------------------------------------------------------ #
92+
1893
@abc.abstractmethod
1994
def get_dof_pos(self) -> np.ndarray:
2095
"""获取关节位置（不含 base）
@@ -31,41 +106,113 @@ def get_dof_vel(self) -> np.ndarray:
31106
(num_envs, num_dof)
32107
"""
33108

109+
# ------------------------------------------------------------------ #
110+
# Body kinematics — world frame #
111+
# ------------------------------------------------------------------ #
112+
34113
@abc.abstractmethod
35-
def get_qpos(self) -> np.ndarray:
36-
"""获取完整位置状态（含 base）
114+
def get_body_pos_w(self, body_ids: np.ndarray) -> np.ndarray:
115+
"""获取指定 body 在世界系下的位置
116+
117+
Args:
118+
body_ids: body 索引数组
37119
38120
Returns:
39-
(num_envs, nq)
121+
(num_envs, len(body_ids), 3)
40122
"""
41123

42124
@abc.abstractmethod
43-
def get_sensor_data(self, name: str) -> np.ndarray:
44-
"""获取传感器数据
125+
def get_body_quat_w(self, body_ids: np.ndarray) -> np.ndarray:
126+
"""获取指定 body 在世界系下的四元数（wxyz）
45127
46128
Args:
47-
name: 传感器名称
129+
body_ids: body 索引数组
48130
49131
Returns:
50-
传感器数据数组
132+
(num_envs, len(body_ids), 4)
51133
"""
52134

53135
@abc.abstractmethod
54-
def set_state(self, env_indices: np.ndarray, qpos: np.ndarray, qvel: np.ndarray) -> None:
55-
"""设置指定环境的物理状态
136+
def get_body_lin_vel_w(self, body_ids: np.ndarray) -> np.ndarray:
137+
"""获取指定 body 在世界系下的线速度
56138
57139
Args:
58-
env_indices: 环境索引
59-
qpos: 位置状态
60-
qvel: 速度状态
140+
body_ids: body 索引数组
141+
142+
Returns:
143+
(num_envs, len(body_ids), 3)
61144
"""
62145

63-
@property
64146
@abc.abstractmethod
65-
def num_envs(self) -> int:
66-
"""环境数量"""
147+
def get_body_ang_vel_w(self, body_ids: np.ndarray) -> np.ndarray:
148+
"""获取指定 body 在世界系下的角速度
149+
150+
Args:
151+
body_ids: body 索引数组
152+
153+
Returns:
154+
(num_envs, len(body_ids), 3)
155+
"""
156+
157+
# ------------------------------------------------------------------ #
158+
# Body kinematics — baselink frame #
159+
# ------------------------------------------------------------------ #
67160

68-
@property
69161
@abc.abstractmethod
70-
def model(self):
71-
"""底层物理模型"""
162+
def get_body_pos_b(self, body_ids: np.ndarray) -> np.ndarray:
163+
"""获取指定 body 在 baselink 系下的位置
164+
165+
Args:
166+
body_ids: body 索引数组
167+
168+
Returns:
169+
(num_envs, len(body_ids), 3)
170+
"""
171+
172+
@abc.abstractmethod
173+
def get_body_quat_b(self, body_ids: np.ndarray) -> np.ndarray:
174+
"""获取指定 body 在 baselink 系下的四元数（wxyz）
175+
176+
Args:
177+
body_ids: body 索引数组
178+
179+
Returns:
180+
(num_envs, len(body_ids), 4)
181+
"""
182+
183+
@abc.abstractmethod
184+
def get_body_lin_vel_b(self, body_ids: np.ndarray) -> np.ndarray:
185+
"""获取指定 body 在 baselink 系下的线速度
186+
187+
Args:
188+
body_ids: body 索引数组
189+
190+
Returns:
191+
(num_envs, len(body_ids), 3)
192+
"""
193+
194+
@abc.abstractmethod
195+
def get_body_ang_vel_b(self, body_ids: np.ndarray) -> np.ndarray:
196+
"""获取指定 body 在 baselink 系下的角速度
197+
198+
Args:
199+
body_ids: body 索引数组
200+
201+
Returns:
202+
(num_envs, len(body_ids), 3)
203+
"""
204+
205+
# ------------------------------------------------------------------ #
206+
# Sensors #
207+
# ------------------------------------------------------------------ #
208+
209+
@abc.abstractmethod
210+
def get_sensor_data(self, name: str) -> np.ndarray:
211+
"""获取传感器数据
212+
213+
Args:
214+
name: 传感器名称
215+
216+
Returns:
217+
传感器数据数组
218+
"""

0 commit comments

Comments
 (0)