def update_sim(self):
for handle, velocity in zip(self.joint_handles, self.curr_action):
catch_errors(
vrep.simxSetJointTargetVelocity(self.cid, int(handle),
velocity,
vrep.simx_opmode_oneshot))
vrep.simxSynchronousTrigger(self.cid)
vrep.simxGetPingTime(self.cid)
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.subject_handle = catch_errors(vrep.simxGetObjectHandle(self.cid, "Subject",
vrep.simx_opmode_blocking))
self.target_handle = catch_errors(vrep.simxGetObjectHandle(self.cid, "Target",
vrep.simx_opmode_blocking))
self.subject_pos = [0.]*3
self.target_pos = [0.]*3
def setup_vision(self):
self.vis_handle = catch_errors(
vrep.simxGetObjectHandle(self.cid, "Vision_sensor",
vrep.simx_opmode_blocking))
self.res = self.call_lua_function('get_resolution')[0]
self.subject_handle = catch_errors(
vrep.simxGetObjectHandle(self.cid, "Plate",
vrep.simx_opmode_blocking))
self.cloth_handle = catch_errors(
vrep.simxGetObjectHandle(self.cid, "Cloth",
vrep.simx_opmode_blocking))
self.stand_h = catch_errors(
vrep.simxGetObjectHandle(self.cid, "Stand",
vrep.simx_opmode_blocking))
self.init_cam_pos = catch_errors(
vrep.simxGetObjectPosition(self.cid, self.vis_handle, -1,
vrep.simx_opmode_blocking))
self.init_cam_rot = catch_errors(
vrep.simxGetObjectOrientation(self.cid, self.vis_handle, -1,
vrep.simx_opmode_blocking))
self.init_stand_pos = catch_errors(
vrep.simxGetObjectPosition(self.cid, self.stand_h, -1,
vrep.simx_opmode_blocking))
def init_color(handle, scale):
return [(handle, self.call_lua_function('get_color',
ints=[handle])[1], scale)]
self.init_colors += init_color(self.subject_handle, 0.025)
self.init_colors += init_color(self.rack_handle, 0.05)
self.init_colors += init_color(self.cloth_handle, 0.05)
self.light_handles = [
catch_errors(
vrep.simxGetObjectHandle(self.cid, f'LocalLight{c}',
vrep.simx_opmode_blocking))
for c in ['A', 'B', 'C', 'D']
]
self.light_poss = [
catch_errors(
vrep.simxGetObjectPosition(self.cid, handle, -1,
vrep.simx_opmode_blocking))
for handle in self.light_handles
]
self.light_rots = [
catch_errors(
vrep.simxGetObjectOrientation(self.cid, handle, -1,
vrep.simx_opmode_blocking))
for handle in self.light_handles
]
def reset(self):
super(DishRackEnv, self).reset()
self.rack_pos[0] = self.np_random.uniform(rack_lower[0], rack_upper[0])
self.rack_pos[1] = self.np_random.uniform(rack_lower[1], rack_upper[1])
self.rack_rot[0] = self.np_random.uniform(rack_lower[2], rack_upper[2])
vrep.simxSetObjectPosition(self.cid, self.rack_handle, -1,
self.rack_pos, vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.cid, self.rack_handle,
self.rack_rot_ref, self.rack_rot,
vrep.simx_opmode_blocking)
trg_rot = catch_errors(
vrep.simxGetObjectOrientation(self.cid, self.target_handle, -1,
vrep.simx_opmode_blocking))
vrep.simxSetObjectOrientation(self.cid, self.mv_trg_handle, -1,
trg_rot, vrep.simx_opmode_blocking)
if self.vis_mode:
# OTHER POSES
stand_height_diff = np.append([0, 0],
self.np_random.uniform(
-self.max_height_displacement,
self.max_height_displacement, 1))
vrep.simxSetObjectPosition(self.cid, self.stand_h, -1,
self.init_stand_pos + stand_height_diff,
vrep.simx_opmode_blocking)
self.randomise_domain()
return self._get_obs()
def __init__(self, *args):
super().__init__(*args, random_joints=False)
self.ep_len = 64
self.mv_trg_handle = catch_errors(
vrep.simxGetObjectHandle(self.cid, "MvTarget",
vrep.simx_opmode_blocking))
self.anchor_handle = catch_errors(
vrep.simxGetObjectHandle(self.cid, "Anchor",
vrep.simx_opmode_blocking))
self.start_rot = catch_errors(
vrep.simxGetObjectOrientation(self.cid, self.mv_trg_handle, -1,
vrep.simx_opmode_blocking))
self.target_pos = self.get_position(self.target_handle)
self.target_pos[0] = trg_pos[0]
self.target_pos[1] = trg_pos[1]
vrep.simxSetObjectPosition(self.cid, self.target_handle, -1,
self.target_pos, vrep.simx_opmode_blocking)
def __init__(self, *args, random_joints=True):
super().__init__(*args)
self.random_joints = random_joints
self.np_random = np.random.RandomState()
# Get the initial configuration of the robot (needed to later reset the robot's pose)
self.init_config_tree, _, _, _ = self.call_lua_function(
'get_configuration_tree')
_, self.init_joint_angles, _, _ = self.call_lua_function(
'get_joint_angles')
self.joint_handles = np.array([None] * self.num_joints)
for i in range(self.num_joints):
handle = catch_errors(
vrep.simxGetObjectHandle(self.cid, 'Sawyer_joint' + str(i + 1),
vrep.simx_opmode_blocking))
self.joint_handles[i] = handle
# Start the simulation (the "Play" button in V-Rep should now be in a "Pressed" state)
catch_errors(
vrep.simxStartSimulation(self.cid, vrep.simx_opmode_blocking))
def __init__(self, *args):
super().__init__(*args)
self.ep_len = 100
return_code, self.sphere_handle = vrep.simxGetObjectHandle(self.cid,
"Sphere", vrep.simx_opmode_blocking)
check_for_errors(return_code)
return_code, self.wall_handle = vrep.simxGetObjectHandle(self.cid,
"Wall", vrep.simx_opmode_blocking)
check_for_errors(return_code)
self.wall_pos = vrep.simxGetObjectPosition(self.cid, self.wall_handle,
-1, vrep.simx_opmode_blocking)[1]
self.init_wall_rot = vrep.simxGetObjectOrientation(self.cid,
self.wall_handle, -1, vrep.simx_opmode_blocking)[1]
self.wall_orientation = self.init_wall_rot
self.mv_trg_handle = catch_errors(vrep.simxGetObjectHandle(self.cid, "MvTarget",
vrep.simx_opmode_blocking))
def __init__(self, *args):
super().__init__(*args)
self.ep_len = 64
self.rack_handle = catch_errors(
vrep.simxGetObjectHandle(self.cid, "DishRack",
vrep.simx_opmode_blocking))
self.rack_pos = catch_errors(
vrep.simxGetObjectPosition(self.cid, self.rack_handle, -1,
vrep.simx_opmode_blocking))
self.rack_rot_ref = catch_errors(
vrep.simxGetObjectHandle(self.cid, "DefaultOrientation",
vrep.simx_opmode_blocking))
self.rack_rot = catch_errors(
vrep.simxGetObjectOrientation(self.cid, self.rack_handle,
self.rack_rot_ref,
vrep.simx_opmode_blocking))
self.target_handle = catch_errors(
vrep.simxGetObjectHandle(self.cid, "Target",
vrep.simx_opmode_blocking))
self.joint_targets = None
self.mv_trg_handle = catch_errors(
vrep.simxGetObjectHandle(self.cid, "MvTarget",
vrep.simx_opmode_blocking))
def get_mug_orientation(self):
orientation = catch_errors(
vrep.simxGetObjectOrientation(self.cid, self.subject_handle,
self.target_handle,
vrep.simx_opmode_blocking))
return np.array(orientation[:-1])