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
class GripperTester:
"""
A class that is used to test gripper
Args:
gripper (GripperModel): A gripper instance to be tested
pos (str): (x y z) position to place the gripper in string form, e.g. '0 0 0.3'
quat (str): rotation to apply to gripper in string form, 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 (None or 3-tuple of int): the size of the box to grasp, None defaults to [0.02, 0.02, 0.02]
box_density (int): the density of the box to grasp
step_time (int): the interval between two gripper actions
render (bool): if True, show rendering
"""
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 start_simulation(self):
"""
Starts simulation of the test world
"""
model = self.world.get_model(mode="mujoco_py")
self.sim = MjSim(model)
if self.render:
self.viewer = MjViewer(self.sim)
self.sim_state = self.sim.get_state()
# For gravity correction
gravity_corrected = ["gripper_z_joint"]
self._gravity_corrected_qvels = [self.sim.model.get_joint_qvel_addr(x) for x in gravity_corrected]
self.gripper_z_id = self.sim.model.actuator_name2id("gripper_z")
self.gripper_z_is_low = False
self.gripper_actuator_ids = [self.sim.model.actuator_name2id(x) for x in self.gripper.actuators]
self.gripper_is_closed = True
self.object_id = self.sim.model.body_name2id(self.cube.root_body)
object_default_pos = self.sim.data.body_xpos[self.object_id]
self.object_default_pos = np.array(object_default_pos, copy=True)
self.reset()
self.simulation_ready = True
def reset(self):
"""
Resets the simulation to the initial state
"""
self.sim.set_state(self.sim_state)
self.cur_step = 0
def close(self):
"""
Close the viewer if it exists
"""
if self.viewer is not None:
self.viewer.close()
def step(self):
"""
Forward the simulation by one timestep
Raises:
RuntimeError: if start_simulation is not yet called.
"""
if not self.simulation_ready:
raise RuntimeError("Call start_simulation before calling step")
if self.gripper_z_is_low:
self.sim.data.ctrl[self.gripper_z_id] = self.gripper_low_pos
else:
self.sim.data.ctrl[self.gripper_z_id] = self.gripper_high_pos
if self.gripper_is_closed:
self._apply_gripper_action(1)
else:
self._apply_gripper_action(-1)
self._apply_gravity_compensation()
self.sim.step()
if self.render:
self.viewer.render()
self.cur_step += 1
def _apply_gripper_action(self, action):
"""
Applies binary gripper action
Args:
action (int): Action to apply. Should be -1 (open) or 1 (closed)
"""
gripper_action_actual = self.gripper.format_action(np.array([action]))
# rescale normalized gripper action to control ranges
ctrl_range = self.sim.model.actuator_ctrlrange[self.gripper_actuator_ids]
bias = 0.5 * (ctrl_range[:, 1] + ctrl_range[:, 0])
weight = 0.5 * (ctrl_range[:, 1] - ctrl_range[:, 0])
applied_gripper_action = bias + weight * gripper_action_actual
self.sim.data.ctrl[self.gripper_actuator_ids] = applied_gripper_action
def _apply_gravity_compensation(self):
"""
Applies gravity compensation to the simulation
"""
self.sim.data.qfrc_applied[self._gravity_corrected_qvels] = self.sim.data.qfrc_bias[
self._gravity_corrected_qvels
]
def loop(self, total_iters=1, test_y=False, y_baseline=0.01):
"""
Performs lower, grip, raise and release actions of a gripper,
each separated with T timesteps
Args:
total_iters (int): Iterations to perform before exiting
test_y (bool): test if object is lifted
y_baseline (float): threshold for determining that object is lifted
"""
seq = [(False, False), (True, False), (True, True), (False, True)]
for cur_iter in range(total_iters):
for cur_plan in seq:
self.gripper_z_is_low, self.gripper_is_closed = cur_plan
for step in range(self.step_time):
self.step()
if test_y:
if not self.object_height > y_baseline:
raise ValueError(
"object is lifed by {}, ".format(self.object_height)
+ "not reaching the requirement {}".format(y_baseline)
)
@property
def object_height(self):
"""
Queries the height (z) of the object compared to on the ground
Returns:
float: Object height relative to default (ground) object position
"""
return self.sim.data.body_xpos[self.object_id][2] - self.object_default_pos[2]
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()
