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 merge_objects(self, mujoco_objects): """Adds physical objects to the MJCF model.""" self.mujoco_objects = mujoco_objects self.objects = {} # xml manifestation self.max_horizontal_radius = 0 for obj_name, obj_mjcf in mujoco_objects.items(): self.merge_asset(obj_mjcf) # Load object obj = obj_mjcf.get_collision(name=obj_name, site=True) obj.append(new_joint(name=obj_name, type="free", damping="0.0005")) self.objects[obj_name] = obj self.worldbody.append(obj) self.max_horizontal_radius = max(self.max_horizontal_radius, obj_mjcf.get_horizontal_radius())
world = MujocoWorldBase() # add a table arena = TableArena(table_full_size=(0.4, 0.4, 0.1)) world.merge(arena) # add a gripper gripper = TwoFingerGripper() gripper_body = ET.Element("body") for body in gripper.worldbody: gripper_body.append(body) gripper_body.set("pos", "0 0 0.3") gripper_body.set("quat", "0 0 1 0") # 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 mujoco_object = BoxObject(size=[0.02, 0.02, 0.02], rgba=[1, 0, 0, 1], friction=1).get_collision() mujoco_object.append(new_joint(name="object_free_joint", type="free")) mujoco_object.set("pos", "0 0 0.11")
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
def _load_model(self): """ Loads an xml model and puts it in self.model. """ super()._load_model() # creating the model self.model = MujocoWorldBase() # adding robot self.mujoco_robot = GripperOnlyRobot() self.mujoco_robot.set_base_xpos(self.robot_pos) # add free-form gripper self.gripper = WideRangeGripper() self.gripper.hide_visualization() self.mujoco_robot.add_gripper("hand", self.gripper) self.model.merge(self.mujoco_robot) # adding table self.mujoco_arena = TableArena(table_full_size=self.table_full_size) self.mujoco_arena.set_origin(self.arena_pos) self.model.merge(self.mujoco_arena) # add invisible boundary walls if self._hp.add_boundary_walls: TOP_OFFSET = 0.2 # not sure why this is necessary, but robot seems to have invisible top-part? for name, size, pos in zip(['wall_left', 'wall_right', 'wall_top'], [[self.table_full_size[0]/2, 0.01, self.max_height/2 + TOP_OFFSET/2], [self.table_full_size[0]/2, 0.01, self.max_height/2 + TOP_OFFSET/2], [self.table_full_size[0]/2, self.table_full_size[1]/2, 0.01]], [[0, -self.table_full_size[1] / 2, self.max_height / 2 + self._hp.table_size[2] + TOP_OFFSET/2], [0, self.table_full_size[1] / 2, self.max_height / 2 + self._hp.table_size[2] + TOP_OFFSET/2], [0, 0, self.max_height + self._hp.table_size[2] + TOP_OFFSET]]): wall_body = new_body(name, pos=pos) geom_obj = new_geom("box", size=size, rgba=(0,0,0,0), group=1, contype="2", conaffinity="2",) wall_body.append(geom_obj) self.model.worldbody.append(wall_body) # adding objects self._blocks = [] for ix in range(self._hp.n_blocks): item_color = color_dict item_name = 'obj{}'.format(ix) block_color = np.array(color_dict[self._hp.block_color]) block_size = [self._hp.block_size] * 3 mujoco_obj = NumberedBoxObject(size=block_size, rgba=block_color.tolist(), friction=self._hp.friction, number=ix if self._hp.number_blocks else None) self.model.merge_asset(mujoco_obj) mjcf_obj = mujoco_obj.get_collision(name=item_name, site=True) mjcf_obj.append(new_joint(name=item_name, type="free")) self.model.worldbody.append(mjcf_obj) self._blocks.append(Block(AttrDict({ "env": self, "name": item_name, "size": block_size, "mujoco_obj": mujoco_obj, "mjcf_obj": mjcf_obj })))