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)
