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
})))