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