def _write_frame(self, frames_grp: h5py.Group, resp: List[bytes], frame_num: int) -> \
Tuple[h5py.Group, h5py.Group, dict, bool]:
frame, objs, tr, done = super()._write_frame(frames_grp=frames_grp, resp=resp, frame_num=frame_num)
num_objects = len(self.object_ids)
# Physics data.
velocities = np.empty(dtype=np.float32, shape=(num_objects, 3))
angular_velocities = np.empty(dtype=np.float32, shape=(num_objects, 3))
# Collision data.
collision_ids = np.empty(dtype=np.int32, shape=(0, 2))
collision_relative_velocities = np.empty(dtype=np.float32, shape=(0, 3))
collision_contacts = np.empty(dtype=np.float32, shape=(0, 2, 3))
# Environment Collision data.
env_collision_ids = np.empty(dtype=np.int32, shape=(0, 1))
env_collision_contacts = np.empty(dtype=np.float32, shape=(0, 2, 3))
sleeping = True
for r in resp[:-1]:
r_id = OutputData.get_data_type_id(r)
if r_id == "rigi":
ri = Rigidbodies(r)
ri_dict = dict()
for i in range(ri.get_num()):
ri_dict.update({ri.get_id(i): {"vel": ri.get_velocity(i),
"ang": ri.get_angular_velocity(i)}})
# Check if any objects are sleeping that aren't in the abyss.
if not ri.get_sleeping(i) and tr[ri.get_id(i)]["pos"][1] >= -1:
sleeping = False
# Add the Rigibodies data.
for o_id, i in zip(self.object_ids, range(num_objects)):
velocities[i] = ri_dict[o_id]["vel"]
angular_velocities[i] = ri_dict[o_id]["ang"]
elif r_id == "coll":
co = Collision(r)
collision_ids = np.append(collision_ids, [co.get_collider_id(), co.get_collidee_id()])
collision_relative_velocities = np.append(collision_relative_velocities, co.get_relative_velocity())
for i in range(co.get_num_contacts()):
collision_contacts = np.append(collision_contacts, (co.get_contact_normal(i),
co.get_contact_point(i)))
elif r_id == "enco":
en = EnvironmentCollision(r)
env_collision_ids = np.append(env_collision_ids, en.get_object_id())
for i in range(en.get_num_contacts()):
env_collision_contacts = np.append(env_collision_contacts, (en.get_contact_normal(i),
en.get_contact_point(i)))
objs.create_dataset("velocities", data=velocities.reshape(num_objects, 3), compression="gzip")
objs.create_dataset("angular_velocities", data=angular_velocities.reshape(num_objects, 3), compression="gzip")
collisions = frame.create_group("collisions")
collisions.create_dataset("object_ids", data=collision_ids.reshape((-1, 2)), compression="gzip")
collisions.create_dataset("relative_velocities", data=collision_relative_velocities.reshape((-1, 3)),
compression="gzip")
collisions.create_dataset("contacts", data=collision_contacts.reshape((-1, 2, 3)), compression="gzip")
env_collisions = frame.create_group("env_collisions")
env_collisions.create_dataset("object_ids", data=env_collision_ids, compression="gzip")
env_collisions.create_dataset("contacts", data=env_collision_contacts.reshape((-1, 2, 3)),
compression="gzip")
return frame, objs, tr, sleeping
def get_object_target_collision(self, obj_id: int, target_id: int, resp: List[bytes]):
contact_points = []
contact_normals = []
for r in resp[:-1]:
r_id = OutputData.get_data_type_id(r)
if r_id == "coll":
co = Collision(r)
coll_ids = [co.get_collider_id(), co.get_collidee_id()]
if [obj_id, target_id] == coll_ids or [target_id, obj_id] == coll_ids:
contact_points = [co.get_contact_point(i) for i in range(co.get_num_contacts())]
contact_normals = [co.get_contact_normal(i) for i in range(co.get_num_contacts())]
return (contact_points, contact_normals)
def trial(self, d_f, s_f, b_f, d_c, s_c, b_c, collision_types):
"""
Collide a chair with a fridge.
:param d_f: The dynamic friction of the fridge.
:param s_f: The static friction of the fridge.
:param b_f: The bounciness of the fridge.
:param d_c: The dynamic friction of the chair.
:param s_c: The static friction of the chair.
:param b_c: The bounciness of the chair.
:param collision_types: The types of collisions to listen for.
"""
print("###\n\nNew Trial\n\n###\n")
fridge_id = 0
chair_id = 1
# Destroy all objects currently in the scene.
init_commands = [{"$type": "destroy_all_objects"}]
# Create the avatar.
init_commands.extend(
TDWUtils.create_avatar(position={
"x": 1,
"y": 2.5,
"z": 5
},
look_at=TDWUtils.VECTOR3_ZERO))
# Add the objects.
# Set the masses and physic materials.
# Apply a force to the chair.
# Receive collision data (note that by setting "stay" to True, you will receive a LOT of data;
# see "Performance Optimizations" documentation.)
init_commands.extend([
self.get_add_object("fridge_large", object_id=fridge_id),
self.get_add_object("chair_billiani_doll",
object_id=chair_id,
position={
"x": 4,
"y": 0,
"z": 0
}), {
"$type": "set_mass",
"id": fridge_id,
"mass": 40
}, {
"$type": "set_mass",
"id": chair_id,
"mass": 20
}, {
"$type": "set_physic_material",
"id": fridge_id,
"dynamic_friction": d_f,
"static_friction": s_f,
"bounciness": b_f
}, {
"$type": "set_physic_material",
"id": chair_id,
"dynamic_friction": d_c,
"static_friction": s_c,
"bounciness": b_c
}, {
"$type": "apply_force_to_object",
"force": {
"x": -200,
"y": 0,
"z": 0
},
"id": chair_id
}, {
"$type": "send_collisions",
"collision_types": collision_types,
"enter": True,
"exit": True,
"stay": True
}
])
self.communicate(init_commands)
# Iterate through 500 frames.
# Every frame, listen for collisions, and parse the output data.
for i in range(500):
resp = self.communicate([])
if len(resp) > 1:
for r in resp[:-1]:
r_id = OutputData.get_data_type_id(r)
# There was a collision between two objects.
if r_id == "coll":
collision = Collision(r)
print("Collision between two objects:")
print("\tEvent: " + collision.get_state())
print("\tCollider: " +
str(collision.get_collider_id()))
print("\tCollidee: " +
str(collision.get_collidee_id()))
print("\tRelative velocity: " +
str(collision.get_relative_velocity()))
print("\tContacts:")
for j in range(collision.get_num_contacts()):
print(
str(collision.get_contact_normal(j)) + "\t" +
str(collision.get_contact_point(j)))
# There was a collision between an object and the environment.
elif r_id == "enco":
collision = EnvironmentCollision(r)
print(
"Collision between an object and the environment:")
print("\tEvent: " + collision.get_state())
print("\tCollider: " + str(collision.get_object_id()))
print("\tContacts:")
for j in range(collision.get_num_contacts()):
print(
str(collision.get_contact_normal(j)) + "\t" +
str(collision.get_contact_point(j)))
else:
raise Exception(r_id)