def configure_location(self):
"""Configures correct locations for this arena"""
# Run superclass first
super().configure_location()
# Determine peg friction
friction = max(0.001, np.random.normal(self.table_friction[0], self.table_friction_std))
# Grab reference to the table body in the xml
table_subtree = self.worldbody.find(".//body[@name='{}']".format("table"))
# Define start position for drawing the line
pos = self.sample_start_pos()
# Define dirt material for markers
tex_attrib = {
"type": "cube",
}
mat_attrib = {
"texrepeat": "1 1",
"specular": "0.0",
"shininess": "0.0",
}
dirt = CustomMaterial(
texture="Dirt",
tex_name="dirt",
mat_name="dirt_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
# Define line(s) drawn on table
for i in range(self.num_sensors):
# If we're using two clusters, we resample the starting position and direction at the halfway point
if self.two_clusters and i == int(np.floor(self.num_sensors / 2)):
pos = self.sample_start_pos()
square_name2 = 'contact_'+str(i)
square2 = CylinderObject(
name=square_name2,
size=[self.line_width / 2, 0.001],
rgba=[1, 1, 1, 1],
density=1,
material=dirt,
friction=friction,
)
self.merge_asset(square2)
visual_c = square2.get_visual(site=True)
visual_c.set("pos", array_to_string([pos[0], pos[1], self.table_half_size[2]]))
visual_c.find("site").set("pos", [0, 0, 0.005])
visual_c.find("site").set("rgba", array_to_string([0, 0, 0, 0]))
table_subtree.append(visual_c)
sensor_name = square_name2 + "_sensor"
sensor_site_name = square_name2
self.sensor_names += [sensor_name]
self.sensor_site_names[sensor_name] = sensor_site_name
# Add to the current dirt path
pos = self.sample_path_pos(pos)
def configure_location(self):
"""Configures correct locations for this arena"""
# Run superclass first
super().configure_location()
# Define start position for drawing the line
pos = self.sample_start_pos()
# Define dirt material for markers
tex_attrib = {
"type": "cube",
}
mat_attrib = {
"texrepeat": "1 1",
"specular": "0.0",
"shininess": "0.0",
}
dirt = CustomMaterial(
texture="Dirt",
tex_name="dirt",
mat_name="dirt_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
shared=True,
)
# Define line(s) drawn on table
for i in range(self.num_markers):
# If we're using two clusters, we resample the starting position and direction at the halfway point
if self.two_clusters and i == int(np.floor(self.num_markers / 2)):
pos = self.sample_start_pos()
marker_name = f'contact{i}'
marker = CylinderObject(
name=marker_name,
size=[self.line_width / 2, 0.001],
rgba=[1, 1, 1, 1],
material=dirt,
obj_type="visual",
joints=None,
)
# Manually add this object to the arena xml
self.merge_assets(marker)
table = find_elements(root=self.worldbody, tags="body", attribs={"name": "table"}, return_first=True)
table.append(marker.get_obj())
# Add this marker to our saved list of all markers
self.markers.append(marker)
# Add to the current dirt path
pos = self.sample_path_pos(pos)
def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
super()._load_model()
self.mujoco_robot.set_base_xpos([0, 0, 0])
# load model for table top workspace
self.mujoco_arena = TableArena(table_full_size=self.table_full_size,
table_friction=self.table_friction)
if self.use_indicator_object:
self.mujoco_arena.add_pos_indicator()
# The panda robot has a pedestal, we want to align it with the table
self.mujoco_arena.set_origin(
[0.16 + self.table_full_size[0] / 2, 0, 0])
# initialize objects of interest
# in original robosuite, a simple domain randomization is included in BoxObject implementation, and called here. We choose to discard that implementation.
cube = FullyFrictionalBoxObject(
size=self.boxobject_size,
friction=self.boxobject_friction,
density=self.boxobject_density,
rgba=[1, 0, 0, 1],
)
self.mujoco_cube = cube
goal = CylinderObject(
size=[0.03, 0.001],
rgba=[0, 1, 0, 1],
)
self.mujoco_goal = goal
self.mujoco_objects = OrderedDict([("cube", cube), ("goal", goal)])
# task includes arena, robot, and objects of interest
self.model = TableTopTask(
self.mujoco_arena,
self.mujoco_robot,
self.mujoco_objects,
initializer=self.placement_initializer,
visual_objects=['goal'],
)
self.model.place_objects()
def __init__(
self,
cylinder_radius=(0.015, 0.03),
cylinder_length=0.13,
use_object_obs=True,
reward_shaping=True,
**kwargs
):
"""
Args:
cylinder_radius (2-tuple): low and high limits of the (uniformly sampled)
radius of the cylinder
cylinder_length (float): length of the cylinder
use_object_obs (bool): if True, include object information in the observation.
reward_shaping (bool): if True, use dense rewards
Inherits the Baxter environment; refer to other parameters described there.
"""
# initialize objects of interest
self.hole = PlateWithHoleObject()
cylinder_radius = np.random.uniform(0.015, 0.03)
self.cylinder = CylinderObject(
size_min=(cylinder_radius, cylinder_length),
size_max=(cylinder_radius, cylinder_length),
)
self.mujoco_objects = OrderedDict()
# whether to use ground-truth object states
self.use_object_obs = use_object_obs
# reward configuration
self.reward_shaping = reward_shaping
super().__init__(gripper_left=None, gripper_right=None, **kwargs)
def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
super()._load_model()
# Adjust base pose(s) accordingly
if self.env_configuration == "bimanual":
xpos = self.robots[0].robot_model.base_xpos_offset["empty"]
self.robots[0].robot_model.set_base_xpos(xpos)
else:
if self.env_configuration == "single-arm-opposed":
# Set up robots facing towards each other by rotating them from their default position
for robot, rotation in zip(self.robots,
(np.pi / 2, -np.pi / 2)):
xpos = robot.robot_model.base_xpos_offset["empty"]
rot = np.array((0, 0, rotation))
xpos = T.euler2mat(rot) @ np.array(xpos)
robot.robot_model.set_base_xpos(xpos)
robot.robot_model.set_base_ori(rot)
else: # "single-arm-parallel" configuration setting
# Set up robots parallel to each other but offset from the center
for robot, offset in zip(self.robots, (-0.25, 0.25)):
xpos = robot.robot_model.base_xpos_offset["empty"]
xpos = np.array(xpos) + np.array((0, offset, 0))
robot.robot_model.set_base_xpos(xpos)
# Add arena and robot
self.model = MujocoWorldBase()
self.mujoco_arena = EmptyArena()
if self.use_indicator_object:
self.mujoco_arena.add_pos_indicator()
self.model.merge(self.mujoco_arena)
for robot in self.robots:
self.model.merge(robot.robot_model)
# initialize objects of interest
self.hole = PlateWithHoleObject(name="hole", )
tex_attrib = {
"type": "cube",
}
mat_attrib = {
"texrepeat": "1 1",
"specular": "0.4",
"shininess": "0.1",
}
greenwood = CustomMaterial(
texture="WoodGreen",
tex_name="greenwood",
mat_name="greenwood_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
self.peg = CylinderObject(
name="peg",
size_min=(self.peg_radius[0], self.peg_length),
size_max=(self.peg_radius[1], self.peg_length),
material=greenwood,
rgba=[0, 1, 0, 1],
)
# Load hole object
self.hole_obj = self.hole.get_collision(site=True)
self.hole_obj.set("quat", "0 0 0.707 0.707")
self.hole_obj.set("pos", "0.11 0 0.17")
self.model.merge_asset(self.hole)
# Load peg object
self.peg_obj = self.peg.get_collision(site=True)
self.peg_obj.set("pos", array_to_string((0, 0, self.peg_length)))
self.model.merge_asset(self.peg)
# Depending on env configuration, append appropriate objects to arms
if self.env_configuration == "bimanual":
self.model.worldbody.find(".//body[@name='{}']".format(
self.robots[0].robot_model.eef_name["left"])).append(
self.hole_obj)
self.model.worldbody.find(".//body[@name='{}']".format(
self.robots[0].robot_model.eef_name["right"])).append(
self.peg_obj)
else:
self.model.worldbody.find(".//body[@name='{}']".format(
self.robots[1].robot_model.eef_name)).append(self.hole_obj)
self.model.worldbody.find(".//body[@name='{}']".format(
self.robots[0].robot_model.eef_name)).append(self.peg_obj)
def __init__(
self,
name,
box1_size=(0.025, 0.025, 0.025),
box2_size=(0.025, 0.025, 0.0125),
use_texture=True,
):
# Set box sizes
self.box1_size = np.array(box1_size)
self.box2_size = np.array(box2_size)
# Set texture attributes
self.use_texture = use_texture
self.box1_material = None
self.box2_material = None
self.box1_rgba = RED
self.box2_rgba = BLUE
# Define materials we want to use for this object
if self.use_texture:
# Remove RGBAs
self.box1_rgba = None
self.box2_rgba = None
# Set materials for each box
tex_attrib = {
"type": "cube",
}
mat_attrib = {
"texrepeat": "3 3",
"specular": "0.4",
"shininess": "0.1",
}
self.box1_material = CustomMaterial(
texture="WoodRed",
tex_name="box1_tex",
mat_name="box1_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
self.box2_material = CustomMaterial(
texture="WoodBlue",
tex_name="box2_tex",
mat_name="box2_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
# Create objects
objects = []
for i, (size, mat, rgba) in enumerate(
zip(
(self.box1_size, self.box2_size),
(self.box1_material, self.box2_material),
(self.box1_rgba, self.box2_rgba),
)):
objects.append(
BoxObject(
name=f"box{i + 1}",
size=size,
rgba=rgba,
material=mat,
))
# Also add hinge for visualization
objects.append(
CylinderObject(
name="hinge",
size=np.array([
min(self.box1_size[2], self.box2_size[2]) / 5.0,
min(self.box1_size[0], self.box2_size[0])
]),
rgba=[0.5, 0.5, 0, 1],
obj_type="visual",
))
# Define hinge joint
rel_hinge_pos = [self.box2_size[0], 0, -self.box2_size[2]
] # want offset in all except y-axis
hinge_joint = {
"name": "box_hinge",
"type": "hinge",
"axis": "0 1 0", # y-axis hinge
"pos": array_to_string(rel_hinge_pos),
"stiffness": "0.0001",
"limited": "true",
"range": "0 1.57",
}
# Define positions -- second box should lie on top of first box with edge aligned at hinge joint
# Hinge visualizer should be aligned at hinge joint location
positions = [
np.zeros(3), # First box is centered at top-level body anyways
np.array([
-(self.box2_size[0] - self.box1_size[0]), 0,
self.box1_size[2] + self.box2_size[2]
]),
np.array(rel_hinge_pos),
]
quats = [
None, # Default quaternion for box 1
None, # Default quaternion for box 2
[0.707, 0.707, 0, 0], # Rotated 90 deg about x-axis
]
# Define parents -- which body each is aligned to
parents = [
None, # box 1 attached to top-level body
objects[0].root_body, # box 2 attached to box 1
objects[1].root_body, # hinge attached to box 2
]
# Run super init
super().__init__(
name=name,
objects=objects,
object_locations=positions,
object_quats=quats,
object_parents=parents,
body_joints={objects[1].root_body: [hinge_joint]},
)
def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
super()._load_model()
# Adjust base pose(s) accordingly
if self.env_configuration == "bimanual":
xpos = self.robots[0].robot_model.base_xpos_offset["empty"]
self.robots[0].robot_model.set_base_xpos(xpos)
else:
if self.env_configuration == "single-arm-opposed":
# Set up robots facing towards each other by rotating them from their default position
for robot, rotation in zip(self.robots,
(np.pi / 2, -np.pi / 2)):
xpos = robot.robot_model.base_xpos_offset["empty"]
rot = np.array((0, 0, rotation))
xpos = T.euler2mat(rot) @ np.array(xpos)
robot.robot_model.set_base_xpos(xpos)
robot.robot_model.set_base_ori(rot)
else: # "single-arm-parallel" configuration setting
# Set up robots parallel to each other but offset from the center
for robot, offset in zip(self.robots, (-0.25, 0.25)):
xpos = robot.robot_model.base_xpos_offset["empty"]
xpos = np.array(xpos) + np.array((0, offset, 0))
robot.robot_model.set_base_xpos(xpos)
# Add arena and robot
mujoco_arena = EmptyArena()
# Arena always gets set to zero origin
mujoco_arena.set_origin([0, 0, 0])
# Modify default agentview camera
mujoco_arena.set_camera(camera_name="agentview",
pos=[
1.0666432116509934, 1.4903257668114777e-08,
2.0563394967349096
],
quat=[
0.6530979871749878, 0.27104058861732483,
0.27104055881500244, 0.6530978679656982
])
# initialize objects of interest
self.hole = PlateWithHoleObject(name="hole")
tex_attrib = {
"type": "cube",
}
mat_attrib = {
"texrepeat": "1 1",
"specular": "0.4",
"shininess": "0.1",
}
greenwood = CustomMaterial(
texture="WoodGreen",
tex_name="greenwood",
mat_name="greenwood_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
self.peg = CylinderObject(
name="peg",
size_min=(self.peg_radius[0], self.peg_length),
size_max=(self.peg_radius[1], self.peg_length),
material=greenwood,
rgba=[0, 1, 0, 1],
joints=None,
)
# Load hole object
hole_obj = self.hole.get_obj()
hole_obj.set("quat", "0 0 0.707 0.707")
hole_obj.set("pos", "0.11 0 0.17")
# Load peg object
peg_obj = self.peg.get_obj()
peg_obj.set("pos", array_to_string((0, 0, self.peg_length)))
# Append appropriate objects to arms
if self.env_configuration == "bimanual":
r_eef, l_eef = [
self.robots[0].robot_model.eef_name[arm]
for arm in self.robots[0].arms
]
r_model, l_model = [
self.robots[0].robot_model, self.robots[0].robot_model
]
else:
r_eef, l_eef = [
robot.robot_model.eef_name for robot in self.robots
]
r_model, l_model = [
self.robots[0].robot_model, self.robots[1].robot_model
]
r_body = find_elements(root=r_model.worldbody,
tags="body",
attribs={"name": r_eef},
return_first=True)
l_body = find_elements(root=l_model.worldbody,
tags="body",
attribs={"name": l_eef},
return_first=True)
r_body.append(peg_obj)
l_body.append(hole_obj)
# task includes arena, robot, and objects of interest
# We don't add peg and hole directly since they were already appended to the robots
self.model = ManipulationTask(
mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
)
# Make sure to add relevant assets from peg and hole objects
self.model.merge_assets(self.hole)
self.model.merge_assets(self.peg)
