def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
super()._load_model()
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["table"](
self.table_full_size[0])
self.robots[0].robot_model.set_base_xpos(xpos)
# load model for table top workspace
mujoco_arena = TableArena(
table_full_size=self.table_full_size,
table_friction=self.table_friction,
table_offset=self.table_offset,
)
# Arena always gets set to zero origin
mujoco_arena.set_origin([0, 0, 0])
# initialize objects of interest
self.cube = BoxObject(
name="cube",
size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015],
size_max=[0.022, 0.022, 0.022], # [0.018, 0.018, 0.018])
rgba=[1, 0, 0, 1],
)
self.goal = BoxObject(
name="goal",
size_min=[0.005, 0.005, 0.005],
size_max=[0.005, 0.005, 0.005],
rgba=[0, 0, 1, 1],
density=11342, # Density of lead such that the goal is hard to move
)
# Create placement initializer
if self.placement_initializer is not None:
self.placement_initializer.reset()
self.placement_initializer.add_objects([self.cube, self.goal])
else:
self.placement_initializer = UniformRandomSampler(
name="ObjectSampler",
mujoco_objects=[self.cube, self.goal],
x_range=[-0.20, 0.20],
y_range=[-0.20, 0.20],
rotation=None,
ensure_object_boundary_in_range=False,
ensure_valid_placement=True,
reference_pos=self.table_offset,
z_offset=0.01,
)
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
mujoco_objects=[self.cube, self.goal],
)
def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
super()._load_model()
self.mujoco_robot.set_base_xpos([0, 0, 0])
# load model for table top workspace
self.mujoco_arena = TableArena(table_full_size=self.table_full_size,
table_friction=self.table_friction)
if self.use_indicator_object:
self.mujoco_arena.add_pos_indicator()
# The sawyer robot has a pedestal, we want to align it with the table
self.mujoco_arena.set_origin(
[0.16 + self.table_full_size[0] / 2, 0, 0])
# initialize objects of interest
cube = BoxObject(
size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015],
size_max=[0.020, 0.020, 0.020], # [0.018, 0.018, 0.018])
rgba=[1, 0, 0, 1],
)
cube_ = BoxObject(
size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015],
size_max=[0.020, 0.020, 0.020], # [0.018, 0.018, 0.018])
rgba=[1, 0, 0, 1],
)
cube1 = BoxObject(
size_min=[0.120, 0.010, 0.080], # [0.015, 0.015, 0.015],
size_max=[0.120, 0.010, 0.080], # [0.018, 0.018, 0.018])
rgba=[0, 1, 0, 1],
)
cube2 = BoxObject(
size_min=[0.120, 0.010, 0.080], # [0.015, 0.015, 0.015],
size_max=[0.120, 0.010, 0.080], # [0.018, 0.018, 0.018])
rgba=[0, 1, 0, 1],
)
self.mujoco_objects = OrderedDict([("cube", cube), ("cube_", cube_),
("cube1", cube1), ("cube2", cube2)])
self.n_objects = len(self.mujoco_objects)
# task includes arena, robot, and objects of interest
self.model = TableTopTask(
self.mujoco_arena,
self.mujoco_robot,
self.mujoco_objects,
initializer=self.placement_initializer,
)
self.model.place_objects()
def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
super()._load_model()
self.mujoco_robot.set_base_xpos([0, 0, 0])
# load model for table top workspace
self.mujoco_arena = TableArena(table_full_size=self.table_full_size,
table_friction=self.table_friction)
if self.use_indicator_object:
self.mujoco_arena.add_pos_indicator()
# The sawyer robot has a pedestal, we want to align it with the table
self.mujoco_arena.set_origin(
[0.16 + self.table_full_size[0] / 2, 0, 0])
# initialize objects of interest
cube = BoxObject(
size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015],
size_max=[0.022, 0.022, 0.022], # [0.018, 0.018, 0.018])
rgba=[1, 0, 0, 1],
)
if self.target_object == 'can':
cube = CanObject()
elif self.target_object == 'bottle':
cube = BottleObject()
elif self.target_object == 'milk':
cube = MilkObject()
elif self.target_object == 'cereal':
cube = CerealObject()
elif self.target_object == 'lemon':
cube = LemonObject()
self.mujoco_objects = OrderedDict([("cube", cube)])
# task includes arena, robot, and objects of interest
self.model = TableTopTask(
self.mujoco_arena,
self.mujoco_robot,
self.mujoco_objects,
initializer=self.placement_initializer,
)
self.model.place_objects()
def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
SingleArmEnv._load_model(self)
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["table"](
self.table_full_size[0])
self.robots[0].robot_model.set_base_xpos(xpos)
# load model for table top workspace
mujoco_arena = TableArena(
table_full_size=self.table_full_size,
table_friction=self.table_friction,
table_offset=self.table_offset,
)
# Arena always gets set to zero origin
mujoco_arena.set_origin([0, 0, 0])
cube_material = self._get_cube_material()
self.cube = BoxObject(
name="cube",
size_min=(0.025, 0.025, 0.025),
size_max=(0.025, 0.025, 0.025),
rgba=[1, 0, 0, 1],
material=cube_material,
)
self.placement_initializer.reset()
self.placement_initializer.add_objects(self.cube)
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
mujoco_objects=self.cube,
)
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
def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
super()._load_model()
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["table"](
self.table_full_size[0])
self.robots[0].robot_model.set_base_xpos(xpos)
# load model for table top workspace
mujoco_arena = TableArena(
table_full_size=self.table_full_size,
table_friction=self.table_friction,
table_offset=self.table_offset,
)
# Arena always gets set to zero origin
mujoco_arena.set_origin([0, 0, 0])
# initialize objects of interest
tex_attrib = {
"type": "cube",
}
mat_attrib = {
"texrepeat": "1 1",
"specular": "0.4",
"shininess": "0.1",
}
redwood = CustomMaterial(
texture="WoodRed",
tex_name="redwood",
mat_name="redwood_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
greenwood = CustomMaterial(
texture="WoodGreen",
tex_name="greenwood",
mat_name="greenwood_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
self.cubeA = BoxObject(
name="cubeA",
size_min=[0.02, 0.02, 0.02],
size_max=[0.02, 0.02, 0.02],
rgba=[1, 0, 0, 1],
material=redwood,
)
self.cubeB = BoxObject(
name="cubeB",
size_min=[0.025, 0.025, 0.025],
size_max=[0.025, 0.025, 0.025],
rgba=[0, 1, 0, 1],
material=greenwood,
)
cubes = [self.cubeA, self.cubeB]
# Create placement initializer
if self.placement_initializer is not None:
self.placement_initializer.reset()
self.placement_initializer.add_objects(cubes)
else:
self.placement_initializer = UniformRandomSampler(
name="ObjectSampler",
mujoco_objects=cubes,
x_range=[-0.08, 0.08],
y_range=[-0.08, 0.08],
rotation=None,
ensure_object_boundary_in_range=False,
ensure_valid_placement=True,
reference_pos=self.table_offset,
z_offset=0.01,
)
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
mujoco_objects=cubes,
)
def __init__(
self,
name,
box1_size=(0.025, 0.025, 0.025),
box2_size=(0.025, 0.025, 0.0125),
use_texture=True,
):
# Set box sizes
self.box1_size = np.array(box1_size)
self.box2_size = np.array(box2_size)
# Set texture attributes
self.use_texture = use_texture
self.box1_material = None
self.box2_material = None
self.box1_rgba = RED
self.box2_rgba = BLUE
# Define materials we want to use for this object
if self.use_texture:
# Remove RGBAs
self.box1_rgba = None
self.box2_rgba = None
# Set materials for each box
tex_attrib = {
"type": "cube",
}
mat_attrib = {
"texrepeat": "3 3",
"specular": "0.4",
"shininess": "0.1",
}
self.box1_material = CustomMaterial(
texture="WoodRed",
tex_name="box1_tex",
mat_name="box1_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
self.box2_material = CustomMaterial(
texture="WoodBlue",
tex_name="box2_tex",
mat_name="box2_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
# Create objects
objects = []
for i, (size, mat, rgba) in enumerate(
zip(
(self.box1_size, self.box2_size),
(self.box1_material, self.box2_material),
(self.box1_rgba, self.box2_rgba),
)):
objects.append(
BoxObject(
name=f"box{i + 1}",
size=size,
rgba=rgba,
material=mat,
))
# Also add hinge for visualization
objects.append(
CylinderObject(
name="hinge",
size=np.array([
min(self.box1_size[2], self.box2_size[2]) / 5.0,
min(self.box1_size[0], self.box2_size[0])
]),
rgba=[0.5, 0.5, 0, 1],
obj_type="visual",
))
# Define hinge joint
rel_hinge_pos = [self.box2_size[0], 0, -self.box2_size[2]
] # want offset in all except y-axis
hinge_joint = {
"name": "box_hinge",
"type": "hinge",
"axis": "0 1 0", # y-axis hinge
"pos": array_to_string(rel_hinge_pos),
"stiffness": "0.0001",
"limited": "true",
"range": "0 1.57",
}
# Define positions -- second box should lie on top of first box with edge aligned at hinge joint
# Hinge visualizer should be aligned at hinge joint location
positions = [
np.zeros(3), # First box is centered at top-level body anyways
np.array([
-(self.box2_size[0] - self.box1_size[0]), 0,
self.box1_size[2] + self.box2_size[2]
]),
np.array(rel_hinge_pos),
]
quats = [
None, # Default quaternion for box 1
None, # Default quaternion for box 2
[0.707, 0.707, 0, 0], # Rotated 90 deg about x-axis
]
# Define parents -- which body each is aligned to
parents = [
None, # box 1 attached to top-level body
objects[0].root_body, # box 2 attached to box 1
objects[1].root_body, # hinge attached to box 2
]
# Run super init
super().__init__(
name=name,
objects=objects,
object_locations=positions,
object_quats=quats,
object_parents=parents,
body_joints={objects[1].root_body: [hinge_joint]},
)
def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
super()._load_model()
# Verify the correct robot has been loaded
assert isinstance(self.robots[0], SingleArm), \
"Error: Expected one single-armed robot! Got {} type instead.".format(type(self.robots[0]))
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["table"](
self.table_full_size[0])
self.robots[0].robot_model.set_base_xpos(xpos)
# load model for table top workspace
self.mujoco_arena = TableArena(
table_full_size=self.table_full_size,
table_friction=self.table_friction,
table_offset=(0, 0, 0.8),
)
if self.use_indicator_object:
self.mujoco_arena.add_pos_indicator()
# Arena always gets set to zero origin
self.mujoco_arena.set_origin([0, 0, 0])
# initialize objects of interest
tex_attrib = {
"type": "cube",
}
mat_attrib = {
"texrepeat": "1 1",
"specular": "0.4",
"shininess": "0.1",
}
redwood = CustomMaterial(
texture="WoodRed",
tex_name="redwood",
mat_name="redwood_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
greenwood = CustomMaterial(
texture="WoodGreen",
tex_name="greenwood",
mat_name="greenwood_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
cubeA = BoxObject(
name="cubeA",
size_min=[0.02, 0.02, 0.02],
size_max=[0.02, 0.02, 0.02],
rgba=[1, 0, 0, 1],
material=redwood,
)
cubeB = BoxObject(
name="cubeB",
size_min=[0.025, 0.025, 0.025],
size_max=[0.025, 0.025, 0.025],
rgba=[0, 1, 0, 1],
material=greenwood,
)
self.mujoco_objects = OrderedDict([("cubeA", cubeA), ("cubeB", cubeB)])
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
self.mujoco_arena,
[robot.robot_model for robot in self.robots],
self.mujoco_objects,
initializer=self.placement_initializer,
)
self.model.place_objects()
def _load_model(self):
"""
Loads an xml model, puts it in self.model. This sets up the mujoco xml for the scene.
"""
super()._load_model()
self.mujoco_robot.set_base_xpos([0, 0, 0])
### Domain Randomisation ###
if (self.initialised):
for key, val in self._parameter_for_randomisation_generator(parameters=self.parameters_to_randomise):
self.dynamics_parameters[key] = val
## Queues for adding time delays
self.eef_pos_queue = deque(maxlen=int(self.dynamics_parameters['eef_timedelay'] + 1))
self.eef_vel_queue = deque(maxlen=int(self.dynamics_parameters['eef_timedelay'] + 1))
if (self.pid is not None):
self.pid.sample_time = self.dynamics_parameters['pid_iteration_time']
### Create the Task ###
## Load the Arena ##
self.mujoco_arena = TableArena(
table_full_size=self.table_full_size, table_friction=(0.1, 0.001, 0.001)
)
if self.use_indicator_object:
self.mujoco_arena.add_pos_indicator()
# The sawyer robot has a pedestal, we want to align it with the table
self.mujoco_arena.set_origin([0.16 + self.table_full_size[0] / 2, 0, 0])
goal = BoxObject(size=[0.023 / 2, 0.023 / 2, 0.001 / 2],
rgba=[0, 1, 0, 1], )
## Put everything together into the task ##
self.model = ReachTask(self.mujoco_arena, self.mujoco_robot, goal)
### Set the goal position ###
# Gripper position at neutral
gripper_pos_neutral = [0.44969246, 0.16029991, 1.00875409]
if(self.specific_goal_position is not None):
init_pos = np.array([gripper_pos_neutral[0] +self.specific_goal_position[0],
gripper_pos_neutral[1] + self.specific_goal_position[1],
self.model.table_top_offset[2]])
init_pos = array_to_string(init_pos)
elif (self.randomise_initial_conditions):
# Place goal in a 20x20cm square directly below the eef neutral position.
noise = self.np_random.uniform(-1, 1, 3) * np.array([0.20, 0.20, 0.0])
offset = np.array([gripper_pos_neutral[0], gripper_pos_neutral[1], self.model.table_top_offset[2]])
init_pos = array_to_string(noise + offset)
else:
init_pos = np.concatenate([gripper_pos_neutral[:2], [self.model.table_top_offset[2]]])
init_pos = array_to_string(init_pos)
self.model.xml_goal.set("pos", init_pos)
### Set the xml parameters to the values given by the dynamics_parameters attribute ###
if (self.initialised):
self._apply_xml_dynamics_parameters()
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],
rgba=[1, 0, 0, 1],
friction=[1, 0.005, 0.0001]).get_obj()
# Set the position of this object
mujoco_object.set("pos", "0 0 0.11")
# 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",
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")
geoms = mujoco_object.findall("./geom")
for geom in geoms:
if geom.get("contype"):
pass
geom.set("name", "object")
geom.set("density", "10000") # 1000 for water
world.worldbody.append(mujoco_object)
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()
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(
name="box",
size=[0.02, 0.02, 0.02],
rgba=[1, 0, 0, 1],
friction=[1, 0.005, 0.0001]
).get_collision()
mujoco_object.append(new_joint(name="object_free_joint", type="free"))
mujoco_object.set("pos", "0 0 0.11")
geoms = mujoco_object.findall("./geom")
for geom in geoms:
if geom.get("contype"):
pass
geom.set("name", "object")
geom.set("density", "10000") # 1000 for water
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]).get_visual()
x_ref.set("pos", "0.2 0 0.105")
