def five_sided_box(size, rgba, group, thickness):
"""
Args:
size ([float,flat,float]):
rgba ([float,float,float,float]): color
group (int): Mujoco group
thickness (float): wall thickness
Returns:
[]: array of geoms corresponding to the
5 sides of the pot used in BaxterLift
"""
geoms = []
x, y, z = size
r = thickness / 2
geoms.append(
new_geom(geom_type="box",
size=[x, y, r],
pos=[0, 0, -z + r],
rgba=rgba,
group=group))
geoms.append(
new_geom(geom_type="box",
size=[x, r, z],
pos=[0, -y + r, 0],
rgba=rgba,
group=group))
geoms.append(
new_geom(geom_type="box",
size=[x, r, z],
pos=[0, y - r, 0],
rgba=rgba,
group=group))
geoms.append(
new_geom(geom_type="box",
size=[r, y, z],
pos=[x - r, 0, 0],
rgba=rgba,
group=group))
geoms.append(
new_geom(geom_type="box",
size=[r, y, z],
pos=[-x + r, 0, 0],
rgba=rgba,
group=group))
return geoms
def five_sided_box(size, rgba, group, thickness, material):
"""
Procedurally generates a five-sided (open) box
Args:
size (3-array of float): the (x,y,z) half-size desired for the box
rgba (4-array of float): rgba color for this box
group (int): Mujoco group to assign these geoms
thickness (float): wall thickness
material (str): material for this box
Returns:
list: array of geoms corresponding to the 5 sides of the generated box
"""
geoms = []
x, y, z = size
r = thickness / 2
geoms.append(
new_geom(
geom_type="box", size=[x, y, r], pos=[0, 0, -z + r], rgba=rgba, group=group, material=material
)
)
geoms.append(
new_geom(
geom_type="box", size=[x, r, z], pos=[0, -y + r, 0], rgba=rgba, group=group, material=material
)
)
geoms.append(
new_geom(
geom_type="box", size=[x, r, z], pos=[0, y - r, 0], rgba=rgba, group=group, material=material
)
)
geoms.append(
new_geom(
geom_type="box", size=[r, y, z], pos=[x - r, 0, 0], rgba=rgba, group=group, material=material
)
)
geoms.append(
new_geom(
geom_type="box", size=[r, y, z], pos=[-x + r, 0, 0], rgba=rgba, group=group, material=material
)
)
return geoms
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 add_pos_indicator(self):
"""Adds a new position indicator."""
body = new_body(name="pos_indicator")
body.append(
new_geom(
"sphere",
[0.03],
rgba=[1, 0, 0, 0.5],
group=1,
contype="0",
conaffinity="0",
))
body.append(new_joint(type="free", name="pos_indicator"))
self.worldbody.append(body)
def __index__(self, pos=None, size=None, rgba=RED, group=1):
super().__init__()
if size is None:
size = [0.02, 0.02, 0.2]
if pos is None:
pos = [0., 0., 0.]
self.arrow = new_body()
self.arrow.append(
new_geom(geom_type="box",
size=size,
pos=pos,
rgba=rgba,
group=group))
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_collision(self, site=None):
# Create new body
main_body = new_body()
main_body.set("name", self.name)
# Define handle and append to the main body
if self.handle_shape == "cylinder":
main_body.append(
new_geom(
geom_type="cylinder",
name="hammer_handle",
size=[self.handle_radius, self.handle_length / 2.0],
pos=(0, 0, 0),
rgba=None if self.use_texture else self.rgba_handle,
group=1,
density=str(self.handle_density),
friction=array_to_string((self.handle_friction, 0.005, 0.0001)),
material="wood_mat" if self.use_texture else None,
)
)
elif self.handle_shape == "box":
main_body.append(
new_geom(
geom_type="box",
name="hammer_handle",
size=[self.handle_radius, self.handle_radius, self.handle_length / 2.0],
pos=(0, 0, 0),
rgba=None if self.use_texture else self.rgba_handle,
group=1,
density=str(self.handle_density),
friction=array_to_string((self.handle_friction, 0.005, 0.0001)),
material="wood_mat" if self.use_texture else None,
)
)
else:
# Raise error
raise ValueError("Error loading hammer: Handle type must either be 'box' or 'cylinder', got {}.".format(
self.handle_shape
))
# Define head and append to the main body
main_body.append(
new_geom(
geom_type="box",
name="hammer_head",
size=[self.head_halfsize * 2, self.head_halfsize, self.head_halfsize],
pos=(0, 0, self.handle_length / 2.0 + self.head_halfsize),
rgba=None if self.use_texture else self.rgba_head,
group=1,
density=str(self.handle_density * self.head_density_ratio),
material="metal_mat" if self.use_texture else None,
)
)
# Define face (and neck) and append to the main body
main_body.append(
new_geom(
geom_type="cylinder",
name="hammer_neck",
size=[self.head_halfsize * 0.8, self.head_halfsize * 0.2],
pos=(self.head_halfsize * 2.2, 0, self.handle_length / 2.0 + self.head_halfsize),
quat=array_to_string([0.707106, 0, 0.707106, 0]),
rgba=None if self.use_texture else self.rgba_face,
group=1,
density=str(self.handle_density * self.head_density_ratio),
material="metal_mat" if self.use_texture else None,
)
)
main_body.append(
new_geom(
geom_type="cylinder",
name="hammer_face",
size=[self.head_halfsize, self.head_halfsize * 0.4],
pos=(self.head_halfsize * 2.8, 0, self.handle_length / 2.0 + self.head_halfsize),
quat=array_to_string([0.707106, 0, 0.707106, 0]),
rgba=None if self.use_texture else self.rgba_face,
group=1,
density=str(self.handle_density * self.head_density_ratio),
material="metal_mat" if self.use_texture else None,
)
)
# Define claw and append to the main body
main_body.append(
new_geom(
geom_type="box",
name="hammer_claw",
size=[self.head_halfsize * 0.7072, self.head_halfsize * 0.95, self.head_halfsize * 0.7072],
pos=(-self.head_halfsize * 2, 0, self.handle_length / 2.0 + self.head_halfsize),
quat=array_to_string([0.9238795, 0, 0.3826834, 0]),
rgba=None if self.use_texture else self.rgba_claw,
group=1,
density=str(self.handle_density * self.head_density_ratio),
material="metal_mat" if self.use_texture else None,
)
)
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