Skip to content

Commit c737208

Browse files
YilingQiaoYilingQiao
committed
Merge branch 'main' into yiling/250713_kernel_step_2
2 parents 33ad9a0 + efab8d1 commit c737208

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
@@ -695,7 +695,6 @@ def _init_link_fields(self):
695695
# cd
696696
cd_ang=gs.ti_vec3,
697697
cd_vel=gs.ti_vec3,
698-
root_COM=gs.ti_vec3,
699698
mass_sum=gs.ti_float,
700699
COM=gs.ti_vec3,
701700
mass_shift=gs.ti_float,
@@ -2441,7 +2440,7 @@ def _func_COM_links(
24412440
for i_l_ in range(rgi.n_awake_links[i_b]):
24422441
i_l = rgi.awake_links[i_l_, i_b]
24432442

2444-
links_state[i_l, i_b].root_COM = ti.Vector.zero(gs.ti_float, 3)
2443+
links_state[i_l, i_b].COM.fill(0.0)
24452444
links_state[i_l, i_b].mass_sum = 0.0
24462445

24472446
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
@@ -2460,10 +2459,8 @@ def _func_COM_links(
24602459
)
24612460

24622461
i_r = links_info[I_l].root_idx
2463-
ti.atomic_add(links_state[i_r, i_b].mass_sum, mass)
2464-
2465-
COM = mass * links_state[i_l, i_b].i_pos
2466-
ti.atomic_add(links_state[i_r, i_b].root_COM, COM)
2462+
links_state[i_r, i_b].mass_sum += mass
2463+
links_state[i_r, i_b].COM += mass * links_state[i_l, i_b].i_pos
24672464

24682465
ti.loop_config(serialize=sstatic_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
24692466
for i_l_ in range(rgi.n_awake_links[i_b]):
@@ -2472,15 +2469,15 @@ def _func_COM_links(
24722469

24732470
i_r = links_info[I_l].root_idx
24742471
if i_l == i_r:
2475-
links_state[i_l, i_b].root_COM = links_state[i_l, i_b].root_COM / links_state[i_l, i_b].mass_sum
2472+
links_state[i_l, i_b].COM = links_state[i_l, i_b].COM / links_state[i_l, i_b].mass_sum
24762473

24772474
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
24782475
for i_l_ in range(rgi.n_awake_links[i_b]):
24792476
i_l = rgi.awake_links[i_l_, i_b]
24802477
I_l = [i_l, i_b] if ti.static(static_rigid_sim_config.batch_links_info) else i_l
24812478

24822479
i_r = links_info[I_l].root_idx
2483-
links_state[i_l, i_b].root_COM = links_state[i_r, i_b].root_COM
2480+
links_state[i_l, i_b].COM = links_state[i_r, i_b].COM
24842481

24852482
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
24862483
for i_l_ in range(rgi.n_awake_links[i_b]):
@@ -2491,7 +2488,6 @@ def _func_COM_links(
24912488
l_info = links_info[I_l]
24922489

24932490
i_r = links_info[I_l].root_idx
2494-
links_state[i_l, i_b].COM = links_state[i_r, i_b].root_COM
24952491
links_state[i_l, i_b].i_pos = links_state[i_l, i_b].i_pos - links_state[i_l, i_b].COM
24962492

24972493
i_inertial = l_info.inertial_i
@@ -2614,7 +2610,7 @@ def _func_COM_links(
26142610
else:
26152611
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
26162612
for i_l in range(n_links):
2617-
links_state[i_l, i_b].root_COM = ti.Vector.zero(gs.ti_float, 3)
2613+
links_state[i_l, i_b].COM.fill(0.0)
26182614
links_state[i_l, i_b].mass_sum = 0.0
26192615

26202616
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
@@ -2632,10 +2628,8 @@ def _func_COM_links(
26322628
)
26332629

26342630
i_r = links_info[I_l].root_idx
2635-
ti.atomic_add(links_state[i_r, i_b].mass_sum, mass)
2636-
2637-
COM = mass * links_state[i_l, i_b].i_pos
2638-
ti.atomic_add(links_state[i_r, i_b].root_COM, COM)
2631+
links_state[i_r, i_b].mass_sum += mass
2632+
links_state[i_r, i_b].COM += mass * links_state[i_l, i_b].i_pos
26392633

26402634
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
26412635
for i_l in range(n_links):
@@ -2644,14 +2638,14 @@ def _func_COM_links(
26442638
i_r = links_info[I_l].root_idx
26452639
if i_l == i_r:
26462640
if links_state[i_l, i_b].mass_sum > 0.0:
2647-
links_state[i_l, i_b].root_COM = links_state[i_l, i_b].root_COM / links_state[i_l, i_b].mass_sum
2641+
links_state[i_l, i_b].COM = links_state[i_l, i_b].COM / links_state[i_l, i_b].mass_sum
26482642

26492643
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
26502644
for i_l in range(n_links):
26512645
I_l = [i_l, i_b] if ti.static(static_rigid_sim_config.batch_links_info) else i_l
26522646

26532647
i_r = links_info[I_l].root_idx
2654-
links_state[i_l, i_b].root_COM = links_state[i_r, i_b].root_COM
2648+
links_state[i_l, i_b].COM = links_state[i_r, i_b].COM
26552649

26562650
ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
26572651
for i_l in range(n_links):
@@ -2661,7 +2655,6 @@ def _func_COM_links(
26612655
l_info = links_info[I_l]
26622656

26632657
i_r = links_info[I_l].root_idx
2664-
links_state[i_l, i_b].COM = links_state[i_r, i_b].root_COM
26652658
links_state[i_l, i_b].i_pos = links_state[i_l, i_b].i_pos - links_state[i_l, i_b].COM
26662659

26672660
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)