def set_xpos(self, pos):
"""Places the cursor on position @pos."""
node = self.worldbody.find("./body[@name='cursor0']")
node.set("pos", array_to_string(pos))
node = self.worldbody.find("./body[@name='cursor1']")
node.set("pos", array_to_string(pos))
def set_size(self, size):
"""Change the size of cursors."""
node = self.worldbody.find("./body/geom[@name='cursor0']")
node.set("size", array_to_string([size] * 3))
node.set("margin", array_to_string([size]))
node = self.worldbody.find("./body/geom[@name='cursor1']")
node.set("size", array_to_string([size] * 3))
node.set("margin", array_to_string([size]))
def set_origin(self, offset):
"""Applies a constant offset to all objects."""
offset = np.array(offset)
for node in self.worldbody.findall("./*[@pos]"):
cur_pos = string_to_array(node.get("pos"))
new_pos = cur_pos + offset
node.set("pos", array_to_string(new_pos))
def __init__(self,
floor_full_size=(1., 1.),
floor_friction=(1, 0.005, 0.0001)):
"""
Args:
floor_full_size: full dimensions of the floor
friction: friction parameters of the floor
"""
super().__init__(xml_path_completion("arenas/floor_arena.xml"))
self.floor_full_size = np.array(
[floor_full_size[0], floor_full_size[1], 0.125])
self.floor_half_size = self.floor_full_size / 2
self.floor_friction = floor_friction
self.floor = self.worldbody.find("./geom[@name='floor']")
self.floor.set("size", array_to_string(self.floor_half_size))
self.floor.set("friction", array_to_string(self.floor_friction))
def configure_location(self):
self.bottom_pos = np.array([0, 0, 0])
self.floor.set("pos", array_to_string(self.bottom_pos))
self.center_pos = self.bottom_pos + np.array(
[0, 0, self.table_half_size[2]])
self.table_body.set("pos", array_to_string(self.center_pos))
self.table_collision.set("size", array_to_string(self.table_half_size))
self.table_collision.set("friction",
array_to_string(self.table_friction))
self.table_visual.set("size", array_to_string(self.table_half_size))
self.table_top.set(
"pos", array_to_string(np.array([0, 0, self.table_half_size[2]])))
def get_collision(self, name=None, site=False, friction=(1, .5, .5)):
#collision = copy.deepcopy(self.worldbody.find("./body/body[@name='collision']"))
self.name = name
collision = copy.deepcopy(
self.worldbody.find("./body[@name='%s']" % name))
collision.attrib.pop("name")
if name is not None:
collision.attrib["name"] = name
geoms = collision.findall("geom")
for i in range(len(geoms)):
if not geoms[i].get("name").startswith('noviz'):
geoms[i].set("name", "{}-{}".format(name, i))
geoms[i].set("friction", array_to_string(friction))
if site:
# add a site as well
template = self.get_site_attrib_template()
template["rgba"] = "1 0 0 0"
if name is not None:
template["name"] = name
collision.append(ET.Element("site", attrib=template))
return collision
def place_objects(self):
"""Places objects randomly until no collisions or max iterations hit."""
pos_arr, quat_arr = self.initializer.sample()
for i in range(len(self.objects)):
self.objects[i].set("pos", array_to_string(pos_arr[i]))
self.objects[i].set("quat", array_to_string(quat_arr[i]))
def set_base_xquat(self, quat):
"""Places the robot on position @quat."""
node = self.worldbody.find("./body[@name='base']")
node.set("quat", array_to_string(quat))
def set_base_xpos(self, pos):
"""Places the robot on position @pos."""
node = self.worldbody.find("./body[@name='base']")
node.set("pos", array_to_string(pos - self.bottom_offset))
def __init__(
self,
gripper,
pos,
quat,
gripper_low_pos,
gripper_high_pos,
box_size=None,
box_density=10000,
render=True
):
"""
Initializes world and gripper positioning
Args:
gripper: A Gripper instance
pos: position to place the gripper
e.g. '0 0 0.3'
quat: rotation to apply to gripper
e.g. '0 0 1 0' to flip z axis
gripper_low_pos (float): controls the gipper y position,
larger -> higher
gripper_high_pos (float): controls the gipper y high position
larger -> higher, must be larger
than gripper_low_pos
box_size list(int * 3): the size of the box to grasp,
default [0.02, 0.02, 0.02]
box_density (int): the density of the box to grasp,
render: show rendering
"""
world = MujocoWorldBase()
# Add a table
arena = TableArena(table_full_size=(0.4, 0.4, 0.1))
world.merge(arena)
# Add a gripper
# gripper = TwoFingerGripper()
self.gripper = gripper
gripper_body = ET.Element("body")
for body in gripper.worldbody:
gripper_body.append(body)
gripper_body.set("pos", pos)
gripper_body.set("quat", quat) # flip z
gripper_body.append(
new_joint(name="gripper_z_joint",
type="slide",
axis="0 0 -1",
damping="50")
)
world.merge(gripper, merge_body=False)
world.worldbody.append(gripper_body)
world.actuator.append(
new_actuator(
joint="gripper_z_joint",
act_type="position",
name="gripper_z",
kp="500"
)
)
# Add an object for grasping
# density is in units kg / m3
TABLE_TOP = [0, 0, 0.09]
if box_size is None:
box_size = [0.02, 0.02, 0.02]
box_size = np.array(box_size)
mujoco_object = BoxObject(size=box_size,
rgba=[1, 0, 0, 1],
friction=10,
density=box_density).get_collision()
mujoco_object.append(new_joint(name='object_free_joint', type='free'))
mujoco_object.set('name', "object")
object_pos = np.array(TABLE_TOP + box_size * [0, 0, 1])
mujoco_object.set('pos', array_to_string(object_pos))
geoms = mujoco_object.findall('./geom')
for geom in geoms:
if geom.get('contype'):
pass
world.worldbody.append(mujoco_object)
# Adding reference object for x and y axis
x_ref = BoxObject(size=[0.01, 0.01, 0.01],
rgba=[0, 1, 0, 1]).get_visual()
x_ref.set('pos', '0.2 0 0.105')
world.worldbody.append(x_ref)
y_ref = BoxObject(size=[0.01, 0.01, 0.01],
rgba=[0, 0, 1, 1]).get_visual()
y_ref.set('pos', '0 0.2 0.105')
world.worldbody.append(y_ref)
self.world = world
self.render = render
self.simulation_ready = False
self.cur_step = 0
if gripper_low_pos > gripper_high_pos:
raise ValueError(
"gripper_low_pos {} is larger "
"than gripper_high_pos {}"
.format(gripper_low_pos, gripper_high_pos)
)
self.gripper_low_pos = gripper_low_pos
self.gripper_high_pos = gripper_high_pos
