def drop_cube_on_body_demo(): world = MujocoWorldBase() arena = EmptyArena() arena.set_origin([0, 0, 0]) world.merge(arena) soft_torso = SoftTorsoObject() obj = soft_torso.get_collision() box = BoxObject() box_obj = box.get_collision() obj.append(new_joint(name='soft_torso_free_joint', type='free')) box_obj.append(new_joint(name='box_free_joint', type='free')) world.merge_asset(soft_torso) world.worldbody.append(obj) world.worldbody.append(box_obj) # Place torso on ground collision_soft_torso = world.worldbody.find("./body") collision_soft_torso.set("pos", array_to_string(np.array([-0.1, 0, 0.1]))) model = world.get_model(mode="mujoco_py") sim = MjSim(model) viewer = MjViewer(sim) for _ in range(10000): sim.step() viewer.render()
def _append_joints(self, root, body_name=None, joint_specs="default"): """ Appends all joints as specified by @joint_specs to @body. Args: root (ET.Element): Top-level element to iteratively search through for @body_name body_name (None or str): Name of the body to append the joints to. None defaults to "root" (top-level body) joint_specs (str or list): List of joint specifications to add to the specified body, or "default", which results in a single free joint """ # Standardize inputs if body_name is None: body_name = "root" if joint_specs == "default": joint_specs = [self.get_joint_attrib_template()] for i, joint_spec in enumerate(joint_specs): if "name" not in joint_spec: joint_spec["name"] = f"{body_name}_joint{i}" # Search for body and make sure it exists body = find_elements(root=root, tags="body", attribs={"name": body_name}, return_first=True) assert body is not None, "Could not find body with name: {}".format( body_name) # Add joint(s) to this body for joint_spec in joint_specs: body.append(new_joint(**joint_spec))
def merge_objects(self, mujoco_objects, is_visual=False): """ Adds object models to the MJCF model. Args: mujoco_objects (OrderedDict or MujocoObject): objects to merge into this MJCF model is_visual (bool): Whether the object is a visual object or not """ if not is_visual: self.max_horizontal_radius = 0 for obj_name, obj_mjcf in mujoco_objects.items(): assert (isinstance(obj_mjcf, MujocoGeneratedObject) or isinstance(obj_mjcf, MujocoXMLObject)) self.merge_asset(obj_mjcf) # Load object if is_visual: obj = obj_mjcf.get_visual(site=False) else: obj = obj_mjcf.get_collision(site=True) for i, joint in enumerate(obj_mjcf.joints): obj.append( new_joint(name="{}_jnt{}".format(obj_name, i), **joint)) self.objects.append(obj) self.worldbody.append(obj) if not is_visual: self.max_horizontal_radius = max( self.max_horizontal_radius, obj_mjcf.get_horizontal_radius())
def _get_object_subtree(self): # Initialize top-level body obj = new_body(name="root") # Give main body a small mass in order to have a free joint (only needed for mujoco 1.5) obj.append( new_inertial(pos=(0, 0, 0), mass=0.0001, diaginertia=(0.0001, 0.0001, 0.0001))) # 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 objects and associated args and append them appropriately for o, o_parent, o_pos, o_quat in zip(self.objects, self.object_parents, self.object_locations, self.object_quats): self._append_object(root=obj, obj=o, parent_name=o_parent, pos=o_pos, quat=o_quat) # Loop through all joints and append them appropriately for body_name, joint_specs in self.body_joint_specs.items(): self._append_joints(root=obj, body_name=body_name, joint_specs=joint_specs) # Return final object 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 merge_objects(self, slide_object): """Adds a physical object to the MJCF model.""" self.push_object_idx = 0 self.max_horizontal_radius = 0 self.slide_object = slide_object self.merge_asset(self.slide_object) # Add object to xml obj = self.slide_object.get_collision(name="slide_object", site=True) obj.append(new_joint(name="slide_object", type="free")) self.xml_object = obj self.worldbody.append(obj) self.max_horizontal_radius = self.slide_object.get_horizontal_radius()
def merge_objects(self, mujoco_objects): """Adds physical objects to the MJCF model.""" self.n_objects = len(mujoco_objects) 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.append(obj) self.worldbody.append(obj) self.max_horizontal_radius = max(self.max_horizontal_radius, obj_mjcf.get_horizontal_radius())
def _load_objects_into_model(self, mujoco_objects, object_container, is_visual): for obj_name, obj_mjcf in mujoco_objects.items(): assert (isinstance(obj_mjcf, MujocoGeneratedObject) or isinstance(obj_mjcf, MujocoXMLObject)) self.merge_asset(obj_mjcf) # Load object if is_visual: obj = obj_mjcf.get_visual(site=False) else: obj = obj_mjcf.get_collision(site=True) for i, joint in enumerate(obj_mjcf.joints): obj.append( new_joint(name="{}_jnt{}".format(obj_name, i), **joint)) object_container.append(obj) self.worldbody.append(obj)
def _get_object_subtree(self): # Parse object obj = copy.deepcopy(self.worldbody.find("./body/body[@name='object']")) # Rename this top level object body (will have self.naming_prefix added later) obj.attrib["name"] = "main" # Get all geom_pairs in this tree geom_pairs = self._get_geoms(obj) # Define a temp function so we don't duplicate so much code obj_type = self.obj_type def _should_keep(el): return int(el.get("group")) in GEOMTYPE2GROUP[obj_type] # Loop through each of these pairs and modify them according to @elements arg for i, (parent, element) in enumerate(geom_pairs): # Delete non-relevant geoms and rename remaining ones if not _should_keep(element): parent.remove(element) else: g_name = element.get("name") g_name = g_name if g_name is not None else f"g{i}" element.set("name", g_name) # Also optionally duplicate collision geoms if requested (and this is a collision geom) if self.duplicate_collision_geoms and element.get("group") in { None, "0" }: parent.append( self._duplicate_visual_from_collision(element)) # Also manually set the visual appearances to the original collision model element.set("rgba", array_to_string(OBJECT_COLLISION_COLOR)) if element.get("material") is not None: del element.attrib["material"] # add joint(s) for joint_spec in self.joint_specs: obj.append(new_joint(**joint_spec)) # Lastly, add a site for this object template = self.get_site_attrib_template() template["rgba"] = "1 0 0 0" template["name"] = "default_site" obj.append(ET.Element("site", attrib=template)) return obj
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 merge_objects(self, mujoco_objects, visual_objects=[]): """Adds physical objects to the MJCF model.""" self.mujoco_objects = mujoco_objects self.objects = [] # xml manifestation self.targets = [] # xml manifestation self.max_horizontal_radius = 0 for obj_name, obj_mjcf in mujoco_objects.items(): self.merge_asset(obj_mjcf) # Load object if obj_name in visual_objects: obj = obj_mjcf.get_visual(name=obj_name, site=True) else: obj = obj_mjcf.get_collision(name=obj_name, site=True) obj.append(new_joint(name=obj_name, type="free")) self.objects.append(obj) self.worldbody.append(obj) self.max_horizontal_radius = max(self.max_horizontal_radius, obj_mjcf.get_horizontal_radius())
def merge_objects(self, mujoco_objects): """Adds a physical object to the MJCF model.""" self.xml_objects = [] self.mujoco_objects = [] self.object_names = [] self.push_object_idx = 0 self.max_horizontal_radius = 0 for obj_name, obj_mjcf in mujoco_objects.items(): self.mujoco_objects.append(obj_mjcf) self.object_names.append(obj_name) 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")) self.xml_objects.append(obj) self.worldbody.append(obj) self.max_horizontal_radius = max(self.max_horizontal_radius, obj_mjcf.get_horizontal_radius())
def _load_model(self): # Load the desired controller's default config as a dict controller_config = load_controller_config( default_controller="JOINT_VELOCITY") controller_config["output_max"] = 1.0 controller_config["output_min"] = -1.0 robot_noise = {"magnitude": [0.05] * 7, "type": "gaussian"} self.robot = SingleArm( robot_type="IIWA", idn=0, controller_config=controller_config, initial_qpos=[0.0, 0.7, 0.0, -1.4, 0.0, -0.56, 0.0], initialization_noise=robot_noise, gripper_type="PaddleGripper", gripper_visualization=True, control_freq=self.control_freq) self.robot.load_model() self.robot.robot_model.set_base_xpos([0, 0, 0]) self.arena = EmptyArena() self.arena.set_origin([0.8, 0, 0]) self.pingpong = BallObject(name="pingpong", size=[0.02], rgba=[0.8, 0.8, 0, 1], solref=[0.1, 0.03], solimp=[0, 0, 1], density=100) pingpong_model = self.pingpong.get_collision() pingpong_model.append( new_joint(name="pingpong_free_joint", type="free")) pingpong_model.set("pos", "0.8 0 2.0") # merge into one self.model = MujocoWorldBase() self.model.merge(self.robot.robot_model) self.model.merge(self.arena) self.model.worldbody.append(pingpong_model)
def __init__( self, gripper, pos, quat, gripper_low_pos, gripper_high_pos, box_size=None, box_density=10000, step_time=400, render=True, ): # define viewer self.viewer = None world = MujocoWorldBase() # Add a table arena = TableArena(table_full_size=(0.4, 0.4, 0.1), table_offset=(0, 0, 0.1), has_legs=False) world.merge(arena) # Add a gripper self.gripper = gripper # Create another body with a slider joint to which we'll add this gripper gripper_body = ET.Element("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")) # Add all gripper bodies to this higher level body for body in gripper.worldbody: gripper_body.append(body) # Merge the all of the gripper tags except its bodies world.merge(gripper, merge_body=None) # Manually add the higher level body we created world.worldbody.append(gripper_body) # Create a new actuator to control our slider joint 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) self.cube = BoxObject( name="object", size=box_size, rgba=[1, 0, 0, 1], friction=[1, 0.005, 0.0001], density=box_density ) object_pos = np.array(TABLE_TOP + box_size * [0, 0, 1]) mujoco_object = self.cube.get_obj() # Set the position of this object mujoco_object.set("pos", array_to_string(object_pos)) # Add our object to the world body world.worldbody.append(mujoco_object) # add reference objects for x and y axes x_ref = BoxObject( name="x_ref", size=[0.01, 0.01, 0.01], rgba=[0, 1, 0, 1], obj_type="visual", joints=None ).get_obj() x_ref.set("pos", "0.2 0 0.105") world.worldbody.append(x_ref) y_ref = BoxObject( name="y_ref", size=[0.01, 0.01, 0.01], rgba=[0, 0, 1, 1], obj_type="visual", joints=None ).get_obj() 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.step_time = step_time 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
world = MujocoWorldBase() mujoco_robot = Panda() gripper = gripper_factory('PandaGripper') gripper.hide_visualization() mujoco_robot.add_gripper(gripper) mujoco_robot.set_base_xpos([0, 0, 0]) world.merge(mujoco_robot) sphere = BallObject( name="sphere", size=[0.04], rgba=[0, 0.5, 0.5, 1]).get_collision() sphere.append(new_joint(name='sphere_free_joint', type='free')) sphere.set('pos', '1.0 0 1.0') world.worldbody.append(sphere) model = world.get_model(mode="mujoco_py") sim = MjSim(model) viewer = MjViewer(sim) viewer.vopt.geomgroup[0] = 0 # disable visualization of collision mesh for i in range(10000): sim.data.ctrl[:] = 0 sim.step() viewer.render()
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
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, step_time=400, render=True): world = MujocoWorldBase() # Add a table arena = TableArena(table_full_size=(0.4, 0.4, 0.1), table_offset=(0, 0, 0.1), has_legs=False) world.merge(arena) # Add a gripper 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(name="box", size=box_size, rgba=[1, 0, 0, 1], friction=[1, 0.005, 0.0001], 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(name="x_ref", 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(name="y_ref", 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.step_time = step_time 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
# add a table arena = TableArena(table_full_size=(0.4, 0.4, 0.05), table_offset=(0, 0, 0.1), has_legs=False) world.merge(arena) # add a gripper gripper = RethinkGripper() # Create another body with a slider joint to which we'll add this gripper gripper_body = ET.Element("body", name="gripper_base") 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")) # Add the dummy body with the joint to the global worldbody world.worldbody.append(gripper_body) # Merge the actual gripper as a child of the dummy body world.merge(gripper, merge_body="gripper_base") # Create a new actuator to control our slider joint 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(name="box", size=[0.02, 0.02, 0.02],
obj_xml_path = "objects/rect_box.xml" elif 'square_box' in obj.name: obj_xml_path = "objects/square_box.xml" elif 'half_cylinder_box' in obj.name: obj_xml_path = "objects/half_cylinder_box.xml" elif 'triangle_box' in obj.name: obj_xml_path = "objects/triangle_box.xml" mj_obj_model = MujocoXMLObject( xml_path_completion(obj_xml_path), name=obj.name) arena_model.merge_asset(mj_obj_model) mj_obj = mj_obj_model.get_collision(site=True) joint = mj_obj_model.joints[0] mj_obj.append(new_joint(name=obj.name, **joint)) arena_model.worldbody.append(mj_obj) if goal_obj is not None: mj_goal_obj_model = MujocoXML( xml_path_completion("objects/" + goal_obj.name + ".xml")) goal_body = mj_goal_obj_model.worldbody.find( "./body[@name='" + goal_obj.name + "']") goal_pos_arr, goal_quat_arr = T.mat2pose(goal_obj.pose) goal_body.set("pos", array_to_string(goal_pos_arr)) goal_body.set("quat", array_to_string(goal_quat_arr[[3, 0, 1, 2]])) arena_model.merge(mj_goal_obj_model)