Skip to content

Commit ff45db2

Browse files
duburcqaduburcqa
authored
[BUG FIX] Prevent unrealistic angular velocity of Drone entities causing numerical instability. (Genesis-Embodied-AI#1405)
* Add drone morph option to add tiny angular damping on floating base by default. * Remove redundant 'root_COM' link state.
1 parent b4e945a commit ff45db2

File tree

6 files changed

+23
-30
lines changed

6 files changed

+23
-30
lines changed

‎genesis/engine/entities/rigid_entity/rigid_entity.py‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -407,6 +407,9 @@ def _load_scene(self, morph, surface):
407407
j_info["dofs_stiffness"] = np.zeros(6)
408408
j_info["dofs_invweight"] = np.zeros(6)
409409
j_info["dofs_damping"] = np.zeros(6)
410+
if isinstance(morph, gs.morphs.Drone):
411+
mass_tot = sum(l_info["inertial_mass"] for l_info in l_infos)
412+
j_info["dofs_damping"][3:] = mass_tot * morph.default_base_ang_damping_scale
410413
j_info["dofs_armature"] = np.zeros(6)
411414
j_info["dofs_kp"] = np.zeros((6,), dtype=gs.np_float)
412415
j_info["dofs_kv"] = np.zeros((6,), dtype=gs.np_float)
@@ -1113,13 +1116,13 @@ def inverse_kinematics_multilink(
11131116
gs.raise_exception("Target link not provided.")
11141117

11151118
if len(poss) == 0:
1116-
poss = [None] * n_links
1119+
poss = [None for _ in range(n_links)]
11171120
pos_mask = [False, False, False]
11181121
elif len(poss) != n_links:
11191122
gs.raise_exception("Accepting only `poss` with length equal to `links` or empty list.")
11201123

11211124
if len(quats) == 0:
1122-
quats = [None] * n_links
1125+
quats = [None for _ in range(n_links)]
11231126
rot_mask = [False, False, False]
11241127
elif len(quats) != n_links:
11251128
gs.raise_exception("Accepting only `quats` with length equal to `links` or empty list.")

‎genesis/engine/entities/rigid_entity/rigid_joint.py‎

Lines changed: 0 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -67,13 +67,6 @@ def __init__(
6767
self._dofs_kv = dofs_kv
6868
self._dofs_force_range = dofs_force_range
6969

70-
# NOTE: temp hack to use 0 damping/armature for drone
71-
if isinstance(self._entity, gs.engine.entities.DroneEntity) and self._type == gs.JOINT_TYPE.FREE:
72-
import numpy as np
73-
74-
self._dofs_damping = np.zeros_like(self._dofs_damping)
75-
self._dofs_armature = np.zeros_like(self._dofs_armature)
76-
7770
# ------------------------------------------------------------------------------------
7871
# -------------------------------- real-time state -----------------------------------
7972
# ------------------------------------------------------------------------------------

‎genesis/engine/solvers/rigid/constraint_solver_decomp.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -170,7 +170,7 @@ def add_collision_constraints(self):
170170
cdot_vel = self._solver.dofs_state[i_d, i_b].cdof_vel
171171

172172
t_quat = gu.ti_identity_quat()
173-
t_pos = contact_data.pos - self._solver.links_state[link, i_b].root_COM
173+
t_pos = contact_data.pos - self._solver.links_state[link, i_b].COM
174174
_, vel = gu.ti_transform_motion_by_trans_quat(cdof_ang, cdot_vel, t_pos, t_quat)
175175

176176
diff = sign * vel
@@ -258,7 +258,7 @@ def _func_equality_connect(self, i_b, i_e):
258258
cdot_vel = self._solver.dofs_state[i_d, i_b].cdof_vel
259259

260260
t_quat = gu.ti_identity_quat()
261-
t_pos = pos - self._solver.links_state[link, i_b].root_COM
261+
t_pos = pos - self._solver.links_state[link, i_b].COM
262262
ang, vel = gu.ti_transform_motion_by_trans_quat(cdof_ang, cdot_vel, t_pos, t_quat)
263263

264264
diff = sign * vel
@@ -444,7 +444,7 @@ def _func_equality_weld(self, i_b, i_e):
444444
cdot_vel = self._solver.dofs_state[i_d, i_b].cdof_vel
445445

446446
t_quat = gu.ti_identity_quat()
447-
t_pos = pos_anchor - self._solver.links_state[link, i_b].root_COM
447+
t_pos = pos_anchor - self._solver.links_state[link, i_b].COM
448448
ang, vel = gu.ti_transform_motion_by_trans_quat(cdof_ang, cdot_vel, t_pos, t_quat)
449449
diff = sign * vel
450450
jac = diff[i]

‎genesis/engine/solvers/rigid/constraint_solver_decomp_island.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -178,7 +178,7 @@ def add_collision_constraints(self, island, i_b):
178178
cdot_vel = self._solver.dofs_state[i_d, i_b].cdof_vel
179179

180180
t_quat = gu.ti_identity_quat()
181-
t_pos = contact_data.pos - self._solver.links_state[link, i_b].root_COM
181+
t_pos = contact_data.pos - self._solver.links_state[link, i_b].COM
182182
_, vel = gu.ti_transform_motion_by_trans_quat(cdof_ang, cdot_vel, t_pos, t_quat)
183183

184184
diff = sign * vel

‎genesis/engine/solvers/rigid/rigid_solver_decomp.py‎

Lines changed: 10 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -676,7 +676,6 @@ def _init_link_fields(self):
676676
# cd
677677
cd_ang=gs.ti_vec3,
678678
cd_vel=gs.ti_vec3,
679-
root_COM=gs.ti_vec3,
680679
mass_sum=gs.ti_float,
681680
COM=gs.ti_vec3,
682681
mass_shift=gs.ti_float,
@@ -2164,7 +2163,7 @@ def _func_COM_links(
21642163
for i_l_ in range(rgi.n_awake_links[i_b]):
21652164
i_l = rgi.awake_links[i_l_, i_b]
21662165

2167-
links_state[i_l, i_b].root_COM = ti.Vector.zero(gs.ti_float, 3)
2166+
links_state[i_l, i_b].COM.fill(0.0)
21682167
links_state[i_l, i_b].mass_sum = 0.0
21692168

21702169
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
@@ -2183,10 +2182,8 @@ def _func_COM_links(
21832182
)
21842183

21852184
i_r = links_info[I_l].root_idx
2186-
ti.atomic_add(links_state[i_r, i_b].mass_sum, mass)
2187-
2188-
COM = mass * links_state[i_l, i_b].i_pos
2189-
ti.atomic_add(links_state[i_r, i_b].root_COM, COM)
2185+
links_state[i_r, i_b].mass_sum += mass
2186+
links_state[i_r, i_b].COM += mass * links_state[i_l, i_b].i_pos
21902187

21912188
ti.loop_config(serialize=sstatic_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
21922189
for i_l_ in range(rgi.n_awake_links[i_b]):
@@ -2195,15 +2192,15 @@ def _func_COM_links(
21952192

21962193
i_r = links_info[I_l].root_idx
21972194
if i_l == i_r:
2198-
links_state[i_l, i_b].root_COM = links_state[i_l, i_b].root_COM / links_state[i_l, i_b].mass_sum
2195+
links_state[i_l, i_b].COM = links_state[i_l, i_b].COM / links_state[i_l, i_b].mass_sum
21992196

22002197
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
22012198
for i_l_ in range(rgi.n_awake_links[i_b]):
22022199
i_l = rgi.awake_links[i_l_, i_b]
22032200
I_l = [i_l, i_b] if ti.static(static_rigid_sim_config.batch_links_info) else i_l
22042201

22052202
i_r = links_info[I_l].root_idx
2206-
links_state[i_l, i_b].root_COM = links_state[i_r, i_b].root_COM
2203+
links_state[i_l, i_b].COM = links_state[i_r, i_b].COM
22072204

22082205
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
22092206
for i_l_ in range(rgi.n_awake_links[i_b]):
@@ -2214,7 +2211,6 @@ def _func_COM_links(
22142211
l_info = links_info[I_l]
22152212

22162213
i_r = links_info[I_l].root_idx
2217-
links_state[i_l, i_b].COM = links_state[i_r, i_b].root_COM
22182214
links_state[i_l, i_b].i_pos = links_state[i_l, i_b].i_pos - links_state[i_l, i_b].COM
22192215

22202216
i_inertial = l_info.inertial_i
@@ -2337,7 +2333,7 @@ def _func_COM_links(
23372333
else:
23382334
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
23392335
for i_l in range(n_links):
2340-
links_state[i_l, i_b].root_COM = ti.Vector.zero(gs.ti_float, 3)
2336+
links_state[i_l, i_b].COM.fill(0.0)
23412337
links_state[i_l, i_b].mass_sum = 0.0
23422338

23432339
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
@@ -2355,10 +2351,8 @@ def _func_COM_links(
23552351
)
23562352

23572353
i_r = links_info[I_l].root_idx
2358-
ti.atomic_add(links_state[i_r, i_b].mass_sum, mass)
2359-
2360-
COM = mass * links_state[i_l, i_b].i_pos
2361-
ti.atomic_add(links_state[i_r, i_b].root_COM, COM)
2354+
links_state[i_r, i_b].mass_sum += mass
2355+
links_state[i_r, i_b].COM += mass * links_state[i_l, i_b].i_pos
23622356

23632357
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
23642358
for i_l in range(n_links):
@@ -2367,14 +2361,14 @@ def _func_COM_links(
23672361
i_r = links_info[I_l].root_idx
23682362
if i_l == i_r:
23692363
if links_state[i_l, i_b].mass_sum > 0.0:
2370-
links_state[i_l, i_b].root_COM = links_state[i_l, i_b].root_COM / links_state[i_l, i_b].mass_sum
2364+
links_state[i_l, i_b].COM = links_state[i_l, i_b].COM / links_state[i_l, i_b].mass_sum
23712365

23722366
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
23732367
for i_l in range(n_links):
23742368
I_l = [i_l, i_b] if ti.static(static_rigid_sim_config.batch_links_info) else i_l
23752369

23762370
i_r = links_info[I_l].root_idx
2377-
links_state[i_l, i_b].root_COM = links_state[i_r, i_b].root_COM
2371+
links_state[i_l, i_b].COM = links_state[i_r, i_b].COM
23782372

23792373
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
23802374
for i_l in range(n_links):
@@ -2384,7 +2378,6 @@ def _func_COM_links(
23842378
l_info = links_info[I_l]
23852379

23862380
i_r = links_info[I_l].root_idx
2387-
links_state[i_l, i_b].COM = links_state[i_r, i_b].root_COM
23882381
links_state[i_l, i_b].i_pos = links_state[i_l, i_b].i_pos - links_state[i_l, i_b].COM
23892382

23902383
i_inertial = l_info.inertial_i

‎genesis/options/morphs.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -814,6 +814,9 @@ class Drone(FileMorph):
814814
default_armature : float, optional
815815
Default rotor inertia of the actuators. In practice it is applied to all joints regardless of whether they are
816816
actuated. None to disable. Default to 0.1.
817+
default_base_ang_damping_scale : float, optional
818+
Default angular damping applied on the floating base that will be rescaled by the total mass.
819+
None to disable. Default to 1e-5.
817820
"""
818821

819822
model: str = "CF2X"
@@ -825,6 +828,7 @@ class Drone(FileMorph):
825828
merge_fixed_links: bool = True
826829
links_to_keep: Sequence[str] = ()
827830
default_armature: Optional[float] = 0.1
831+
default_base_ang_damping_scale: Optional[float] = 1e-5
828832

829833
def __init__(self, **data):
830834
super().__init__(**data)

0 commit comments

Comments
 (0)