Skip to content

Commit cbf15df

Browse files
ooctipusooctipus
committed
update bin packing
1 parent d01d71f commit cbf15df

File tree

1 file changed

+51
-37
lines changed

1 file changed

+51
-37
lines changed

‎scripts/demos/bin_packing.py‎

Lines changed: 51 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -61,26 +61,29 @@
6161
assets_cfg=[
6262
sim_utils.UsdFileCfg(
6363
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/004_sugar_box.usd",
64+
rigid_props=sim_utils.RigidBodyPropertiesCfg(solver_position_iteration_count=4),
6465
),
6566
sim_utils.UsdFileCfg(
6667
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/003_cracker_box.usd",
68+
rigid_props=sim_utils.RigidBodyPropertiesCfg(solver_position_iteration_count=4),
6769
),
6870
sim_utils.UsdFileCfg(
6971
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/005_tomato_soup_can.usd",
72+
rigid_props=sim_utils.RigidBodyPropertiesCfg(solver_position_iteration_count=4),
7073
),
7174
sim_utils.UsdFileCfg(
7275
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/YCB/Axis_Aligned_Physics/006_mustard_bottle.usd",
76+
rigid_props=sim_utils.RigidBodyPropertiesCfg(solver_position_iteration_count=4),
7377
),
7478
# note: the placeholder, this allows the effect of having less objects in some env ids
7579
sim_utils.SphereCfg(
76-
radius=0.1, collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=False), visible=False
80+
radius=0.1,
81+
rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True),
82+
collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=False),
83+
visible=False
7784
),
7885
],
7986
random_choice=True,
80-
rigid_props=sim_utils.RigidBodyPropertiesCfg(
81-
solver_position_iteration_count=4, solver_velocity_iteration_count=0
82-
),
83-
mass_props=sim_utils.MassPropertiesCfg(mass=1.0),
8487
),
8588
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.0)),
8689
)
@@ -117,40 +120,68 @@ class MultiObjectSceneCfg(InteractiveSceneCfg):
117120
rigid_objects={
118121
"Object_A_Layer1": RANDOM_YCB_RIGID_OBJECT_CFG.replace(
119122
prim_path="/World/envs/env_.*/Object_A_Layer1",
120-
init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.085, -0.12, 0.2)),
123+
init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.035, -0.06, 0.2)),
121124
),
122125
"Object_B_Layer1": RANDOM_YCB_RIGID_OBJECT_CFG.replace(
123126
prim_path="/World/envs/env_.*/Object_B_Layer1",
124-
init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.085, 0.12, 0.2)),
127+
init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.035, 0.06, 0.2)),
125128
),
126129
"Object_C_Layer1": RANDOM_YCB_RIGID_OBJECT_CFG.replace(
127130
prim_path="/World/envs/env_.*/Object_C_Layer1",
128-
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.085, 0.12, 0.2)),
131+
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.035, 0.06, 0.2)),
129132
),
130133
"Object_D_Layer1": RANDOM_YCB_RIGID_OBJECT_CFG.replace(
131134
prim_path="/World/envs/env_.*/Object_D_Layer1",
132-
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.085, -0.12, 0.2)),
135+
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.035, -0.06, 0.2)),
133136
),
134137
"Object_A_Layer2": RANDOM_YCB_RIGID_OBJECT_CFG.replace(
135138
prim_path="/World/envs/env_.*/Object_A_Layer2",
136-
init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.085, -0.12, 0.4)),
139+
init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.035, -0.06, 0.4)),
137140
),
138141
"Object_B_Layer2": RANDOM_YCB_RIGID_OBJECT_CFG.replace(
139142
prim_path="/World/envs/env_.*/Object_B_Layer2",
140-
init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.085, 0.12, 0.4)),
143+
init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.035, 0.06, 0.4)),
141144
),
142145
"Object_C_Layer2": RANDOM_YCB_RIGID_OBJECT_CFG.replace(
143146
prim_path="/World/envs/env_.*/Object_C_Layer2",
144-
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.085, 0.12, 0.4)),
147+
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.035, 0.06, 0.4)),
145148
),
146149
"Object_D_Layer2": RANDOM_YCB_RIGID_OBJECT_CFG.replace(
147150
prim_path="/World/envs/env_.*/Object_D_Layer2",
148-
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.085, -0.12, 0.4)),
151+
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.035, -0.06, 0.4)),
149152
),
150153
}
151154
)
152155

153156

157+
def reset_object_collections(scene: InteractiveScene, view_ids: torch.Tensor):
158+
if len(view_ids) == 0:
159+
return
160+
rigid_object_collection: RigidObjectCollection = scene["object_collection"]
161+
default_state_w = rigid_object_collection.data.default_object_state.clone()
162+
default_state_w[..., :3] = default_state_w[..., :3] + scene.env_origins.unsqueeze(1)
163+
default_state_w_view = rigid_object_collection.reshape_data_to_view(default_state_w)[view_ids]
164+
range_list = [POSE_RANGE.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]]
165+
ranges = torch.tensor(range_list, device=scene.device)
166+
samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(view_ids), 6), device=scene.device)
167+
168+
positions = default_state_w_view[:, :3] + samples[..., 0:3]
169+
orientations_delta = math_utils.quat_from_euler_xyz(samples[..., 3], samples[..., 4], samples[..., 5])
170+
orientations = math_utils.quat_mul(default_state_w_view[:, 3:7], orientations_delta)
171+
# velocities
172+
range_list = [VELOCITY_RANGE.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]]
173+
ranges = torch.tensor(range_list, device=scene.device)
174+
samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(view_ids), 6), device=scene.device)
175+
176+
velocities = default_state_w_view[:, 7:13] + samples
177+
new_poses = torch.concat((positions, orientations), dim=-1)
178+
179+
new_poses[..., 3:] = math_utils.convert_quat(new_poses[..., 3:], to="xyzw")
180+
rigid_object_collection.root_physx_view.set_transforms(new_poses, indices=view_ids.view(-1, 1))
181+
rigid_object_collection.root_physx_view.set_velocities(velocities, indices=view_ids.view(-1, 1))
182+
183+
184+
154185
##
155186
# Simulation Loop
156187
##
@@ -162,7 +193,7 @@ def run_simulator(sim: SimulationContext, scene: InteractiveScene):
162193
# note: we only do this here for readability.
163194
rigid_object: RigidObject = scene["object"]
164195
rigid_object_collection: RigidObjectCollection = scene["object_collection"]
165-
# robot: Articulation = scene["robot"]
196+
view_indices = torch.arange(scene.num_envs * rigid_object_collection.num_objects, device=scene.device)
166197
# Define simulation stepping
167198
sim_dt = sim.get_physics_dt()
168199
count = 0
@@ -179,36 +210,19 @@ def run_simulator(sim: SimulationContext, scene: InteractiveScene):
179210
rigid_object.write_root_pose_to_sim(root_state[:, :7])
180211
rigid_object.write_root_velocity_to_sim(root_state[:, 7:])
181212
# object collection
182-
object_state = rigid_object_collection.data.default_object_state.clone()
183-
range_list = [POSE_RANGE.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]]
184-
ranges = torch.tensor(range_list, device=sim.device)
185-
rand_samples = math_utils.sample_uniform(
186-
ranges[:, 0], ranges[:, 1], (scene.num_envs, rigid_object_collection.num_objects, 6), device=sim.device
187-
)
188-
189-
positions = object_state[..., :3] + scene.env_origins.unsqueeze(1) + rand_samples[..., 0:3]
190-
orientations_delta = math_utils.quat_from_euler_xyz(
191-
rand_samples[..., 3], rand_samples[..., 4], rand_samples[..., 5]
192-
)
193-
orientations = math_utils.quat_mul(object_state[..., 3:7], orientations_delta)
194-
# velocities
195-
range_list = [VELOCITY_RANGE.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]]
196-
ranges = torch.tensor(range_list, device=sim.device)
197-
rand_samples = math_utils.sample_uniform(
198-
ranges[:, 0], ranges[:, 1], (scene.num_envs, rigid_object_collection.num_objects, 6), device=sim.device
199-
)
200-
201-
velocities = object_state[..., 7:13] + rand_samples
202-
new_state = torch.concat((positions, orientations, velocities), dim=-1)
203-
rigid_object_collection.write_object_state_to_sim(new_state)
204-
213+
reset_object_collections(scene, view_indices)
205214
scene.reset()
206215
print("[INFO]: Resetting scene state...")
207216

208217
# Write data to sim
209218
scene.write_data_to_sim()
210219
# Perform step
211220
sim.step()
221+
object_pos_b = rigid_object_collection.data.object_pos_w - scene.env_origins.unsqueeze(1)
222+
object_pos_b_view = rigid_object_collection.reshape_data_to_view(object_pos_b)
223+
inbound_mask = (-1.0 < object_pos_b_view[:, 0]) & (object_pos_b_view[:, 0] < 1.0)
224+
inbound_mask &= (-1.0 < object_pos_b_view[:, 1]) & (object_pos_b_view[:, 1] < 1.0)
225+
reset_object_collections(scene, view_indices[~inbound_mask])
212226
# Increment counter
213227
count += 1
214228
# Update buffers

0 commit comments

Comments
 (0)