def _rand_walls(self):
wall_bids = {name: self.model.body_name2id(name) for name in ['wall_'+dir for dir in 'nesw']}
window_gid = self.model.geom_name2id('west_window')
#floor_gid = self.model.geom_name2id('floor')
WA_X = Range(-0.2, 0.2)
WA_Y = Range(-0.2, 0.2)
WA_Z = Range(-0.1, 0.1)
WA_R3D = Range3D(WA_X, WA_Y, WA_Z)
WI_X = Range(-0.1, 0.1)
WI_Y = Range(0, 0)
WI_Z = Range(-0.5, 0.5)
WI_R3D = Range3D(WI_X, WI_Y, WI_Z)
R = Range(0,0)
P = Range(-10,10)
Y = Range(0,0)
RPY_R = Range3D(R,P,Y)
#self.model.geom_quat[floor_gid] = jitter_quat(self.START_GEOM_QUAT[floor_gid], 0.01)
#self.model.geom_pos[floor_gid] = self.START_GEOM_POS[floor_gid] + [0,0,sample(-0.1,0.1)
self.model.geom_quat[window_gid] = sample_quat(RPY_R)
#self.model.geom_quat[window_gid] = jitter_quat(self.START_GEOM_QUAT[window_gid], 0.01)
self.model.geom_pos[window_gid] = self.START_GEOM_POS[window_gid] + sample_xyz(WI_R3D)
for name in wall_bids:
gid = wall_bids[name]
self.model.body_quat[gid] = jitter_quat(self.START_BODY_QUAT[gid], 0.01)
self.model.body_pos[gid] = self.START_BODY_POS[gid] + sample_xyz(WA_R3D)
def _rand_robot(self):
"""Randomize joint angles and jitter orientation"""
jnt_shape = self.sim.data.qpos.shape
self.sim.data.qpos[:] = sample_joints(self.model.jnt_range, jnt_shape)
robot_gid = self.model.geom_name2id('robot_table_link')
self.model.geom_quat[robot_gid] = jitter_quat(self.START_GEOM_QUAT[robot_gid], 0.01)
def _rand_object(self):
obj_gid = self.sim.model.geom_name2id('object')
obj_bid = self.sim.model.geom_name2id('object')
table_gid = self.model.geom_name2id('object_table')
table_bid = self.model.body_name2id('object_table')
obj_pose = self.start_obj_pose.copy()
xval = self.model.geom_size[table_gid][
0] #- self.model.geom_size[obj_gid][0]
yval = self.model.geom_size[table_gid][
1] #- self.model.geom_size[obj_gid][1]
O_X = Range(-xval, xval)
O_Y = Range(-yval, yval)
O_Z = Range(0, 0)
O_R3D = Range3D(O_X, O_Y, O_Z)
newpos = obj_pose[:3] + sample_xyz(O_R3D)
newquat = jitter_quat(obj_pose[3:], 0.1)
obj_pose[:3] = newpos
obj_pose[3:] = newquat
self.sim.data.set_joint_qpos('object:joint', obj_pose)