def _get_object_subtree(self):
# Initialize top-level body
obj = new_body(name="root")
# Give main body a small mass in order to have a free joint (only needed for mujoco 1.5)
obj.append(
new_inertial(pos=(0, 0, 0),
mass=0.0001,
diaginertia=(0.0001, 0.0001, 0.0001)))
# Add all joints and sites
for joint_spec in self.joint_specs:
obj.append(new_joint(**joint_spec))
for site_spec in self.site_specs:
obj.append(new_site(**site_spec))
# Loop through all objects and associated args and append them appropriately
for o, o_parent, o_pos, o_quat in zip(self.objects,
self.object_parents,
self.object_locations,
self.object_quats):
self._append_object(root=obj,
obj=o,
parent_name=o_parent,
pos=o_pos,
quat=o_quat)
# Loop through all joints and append them appropriately
for body_name, joint_specs in self.body_joint_specs.items():
self._append_joints(root=obj,
body_name=body_name,
joint_specs=joint_specs)
# Return final object
return obj
def _add_indicators_to_model(self, model):
"""
Adds indicators to the mujoco simulation model
Args:
model (Task): Task instance including all mujoco models for the current simulation to be loaded
"""
if self.indicator_configs is not None:
for indicator_config in self.indicator_configs:
config = deepcopy(indicator_config)
indicator_body = new_body(name=config["name"] + "_body", pos=config.pop("pos", (0, 0, 0)))
indicator_body.append(new_site(**config))
model.worldbody.append(indicator_body)
def _get_object_subtree_(self, ob_type="box"):
# Create element tree
obj = new_body(name="main")
# Get base element attributes
element_attr = {
"name": "g0",
"type": ob_type,
"size": array_to_string(self.size)
}
# Add collision geom if necessary
if self.obj_type in {"collision", "all"}:
col_element_attr = deepcopy(element_attr)
col_element_attr.update(self.get_collision_attrib_template())
col_element_attr["density"] = str(self.density)
col_element_attr["friction"] = array_to_string(self.friction)
col_element_attr["solref"] = array_to_string(self.solref)
col_element_attr["solimp"] = array_to_string(self.solimp)
obj.append(new_geom(**col_element_attr))
# Add visual geom if necessary
if self.obj_type in {"visual", "all"}:
vis_element_attr = deepcopy(element_attr)
vis_element_attr.update(self.get_visual_attrib_template())
vis_element_attr["name"] += "_vis"
if self.material == "default":
vis_element_attr["rgba"] = "0.5 0.5 0.5 1" # mujoco default
vis_element_attr["material"] = "mat"
elif self.material is not None:
vis_element_attr["material"] = self.material.mat_attrib["name"]
else:
vis_element_attr["rgba"] = array_to_string(self.rgba)
obj.append(new_geom(**vis_element_attr))
# add joint(s)
for joint_spec in self.joint_specs:
obj.append(new_joint(**joint_spec))
# add a site as well
site_element_attr = self.get_site_attrib_template()
site_element_attr["name"] = "default_site"
obj.append(new_site(**site_element_attr))
return obj
def get_collision(self, site=None):
main_body = new_body()
main_body.set("name", self.name)
for geom in five_sided_box(
size=self.body_half_size,
rgba=None if self.use_texture else self.rgba_body,
group=1,
thickness=self.thickness,
material="pot_mat" if self.use_texture else None,
):
main_body.append(geom)
handle_z = self.body_half_size[2] - self.handle_radius
handle_1_center = [0, self.body_half_size[1] + self.handle_length, handle_z]
handle_2_center = [
0,
-1 * (self.body_half_size[1] + self.handle_length),
handle_z,
]
# the bar on handle horizontal to body
main_bar_size = [
self.handle_width / 2 + self.handle_radius,
self.handle_radius,
self.handle_radius,
]
side_bar_size = [self.handle_radius, self.handle_length / 2, self.handle_radius]
handle_1 = new_body(name="handle_1")
if self.solid_handle:
handle_1.append(
new_geom(
geom_type="box",
name="handle_1",
pos=[0, self.body_half_size[1] + self.handle_length / 2, handle_z],
size=[
self.handle_width / 2,
self.handle_length / 2,
self.handle_radius,
],
rgba=None if self.use_texture else self.rgba_handle_1,
group=1,
material="handle1_mat" if self.use_texture else None,
)
)
else:
handle_1.append(
new_geom(
geom_type="box",
name="handle_1_c",
pos=handle_1_center,
size=main_bar_size,
rgba=None if self.use_texture else self.rgba_handle_1,
group=1,
material="handle1_mat" if self.use_texture else None,
)
)
handle_1.append(
new_geom(
geom_type="box",
name="handle_1_+", # + for positive x
pos=[
self.handle_width / 2,
self.body_half_size[1] + self.handle_length / 2,
handle_z,
],
size=side_bar_size,
rgba=None if self.use_texture else self.rgba_handle_1,
group=1,
material="handle1_mat" if self.use_texture else None,
)
)
handle_1.append(
new_geom(
geom_type="box",
name="handle_1_-",
pos=[
-self.handle_width / 2,
self.body_half_size[1] + self.handle_length / 2,
handle_z,
],
size=side_bar_size,
rgba=None if self.use_texture else self.rgba_handle_1,
group=1,
material="handle1_mat" if self.use_texture else None,
)
)
handle_2 = new_body(name="handle_2")
if self.solid_handle:
handle_2.append(
new_geom(
geom_type="box",
name="handle_2",
pos=[0, -self.body_half_size[1] - self.handle_length / 2, handle_z],
size=[
self.handle_width / 2,
self.handle_length / 2,
self.handle_radius,
],
rgba=None if self.use_texture else self.rgba_handle_2,
group=1,
material="handle2_mat" if self.use_texture else None,
)
)
else:
handle_2.append(
new_geom(
geom_type="box",
name="handle_2_c",
pos=handle_2_center,
size=main_bar_size,
rgba=None if self.use_texture else self.rgba_handle_2,
group=1,
material="handle2_mat" if self.use_texture else None,
)
)
handle_2.append(
new_geom(
geom_type="box",
name="handle_2_+", # + for positive x
pos=[
self.handle_width / 2,
-self.body_half_size[1] - self.handle_length / 2,
handle_z,
],
size=side_bar_size,
rgba=None if self.use_texture else self.rgba_handle_2,
group=1,
material="handle2_mat" if self.use_texture else None,
)
)
handle_2.append(
new_geom(
geom_type="box",
name="handle_2_-",
pos=[
-self.handle_width / 2,
-self.body_half_size[1] - self.handle_length / 2,
handle_z,
],
size=side_bar_size,
rgba=None if self.use_texture else self.rgba_handle_2,
group=1,
material="handle2_mat" if self.use_texture else None,
)
)
main_body.append(handle_1)
main_body.append(handle_2)
main_body.append(
new_site(
name="pot_handle_1",
rgba=self.rgba_handle_1,
pos=handle_1_center - np.array([0, 0.005, 0]),
size=[0.005],
)
)
main_body.append(
new_site(
name="pot_handle_2",
rgba=self.rgba_handle_2,
pos=handle_2_center + np.array([0, 0.005, 0]),
size=[0.005],
)
)
main_body.append(
new_site(
name="pot_center",
pos=[0, 0, 0],
rgba=[1, 0, 0, 0],
)
)
return main_body
def _get_object_subtree(self):
# Initialize top-level body
obj = new_body(name="root")
# Add all joints and sites
for joint_spec in self.joint_specs:
obj.append(new_joint(**joint_spec))
for site_spec in self.site_specs:
obj.append(new_site(**site_spec))
# Loop through all geoms and generate the composite object
for i, (
obj_type,
g_type,
g_size,
g_loc,
g_name,
g_rgba,
g_friction,
g_quat,
g_material,
g_density,
g_solref,
g_solimp,
) in enumerate(
zip(
self.obj_types,
self.geom_types,
self.geom_sizes,
self.geom_locations,
self.geom_names,
self.geom_rgbas,
self.geom_frictions,
self.geom_quats,
self.geom_materials,
self.density,
self.solref,
self.solimp,
)):
# geom type
geom_type = g_type
# get cartesian size from size spec
size = g_size
cartesian_size = self._size_to_cartesian_half_lengths(
geom_type, size)
if self.locations_relative_to_center:
# no need to convert
pos = g_loc
else:
# use geom location to convert to position coordinate (the origin is the
# center of the composite object)
pos = [
(-self.total_size[0] + cartesian_size[0]) + g_loc[0],
(-self.total_size[1] + cartesian_size[1]) + g_loc[1],
(-self.total_size[2] + cartesian_size[2]) + g_loc[2],
]
# geom name
geom_name = g_name if g_name is not None else f"g{i}"
# geom rgba
geom_rgba = g_rgba if g_rgba is not None else self.rgba
# geom friction
geom_friction = (
array_to_string(g_friction) if g_friction is not None else
array_to_string(np.array([1.0, 0.005, 0.0001]))
) # mujoco default
# Define base geom attributes
geom_attr = {
"size": size,
"pos": pos,
"name": geom_name,
"type": geom_type,
}
# Optionally define quat if specified
if g_quat is not None:
geom_attr["quat"] = array_to_string(g_quat)
# Add collision geom if necessary
if obj_type in {"collision", "all"}:
col_geom_attr = deepcopy(geom_attr)
col_geom_attr.update(self.get_collision_attrib_template())
if g_density is not None:
col_geom_attr["density"] = str(g_density)
col_geom_attr["friction"] = geom_friction
col_geom_attr["solref"] = array_to_string(g_solref)
col_geom_attr["solimp"] = array_to_string(g_solimp)
col_geom_attr["rgba"] = OBJECT_COLLISION_COLOR
obj.append(new_geom(**col_geom_attr))
# Add visual geom if necessary
if obj_type in {"visual", "all"}:
vis_geom_attr = deepcopy(geom_attr)
vis_geom_attr.update(self.get_visual_attrib_template())
vis_geom_attr["name"] += "_vis"
if g_material is not None:
vis_geom_attr["material"] = g_material
vis_geom_attr["rgba"] = geom_rgba
obj.append(new_geom(**vis_geom_attr))
return obj