def reset(self):
sim_ret, self.UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking)
time.sleep(1)
sim_ret, self.RG2_tip_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking)
sim_ret, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1,
(-0.47, 0, 0.254),
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle,
-1, (np.pi / 2, 0, np.pi / 2),
vrep.simx_opmode_blocking)
while gripper_position[
2] > 0.4: # V-REP bug requiring multiple starts and stops to restart
self.open_griper()
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(self.sim_client,
vrep.simx_opmode_blocking)
time.sleep(1)
sim_ret, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle,
-1, (-0.47, 0, 0.254),
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.sim_client,
self.UR5_target_handle, -1,
(np.pi / 2, 0, np.pi / 2),
vrep.simx_opmode_blocking)
# print('started!')
target_x = random.random() * (self.object_work_space[0][1] - self.object_work_space[0][0]) + \
self.object_work_space[0][0]
target_y = random.random() * (self.object_work_space[1][1] - self.object_work_space[1][0]) + \
self.object_work_space[1][0]
# target_z = random.random() * (self.work_space[2][1] - self.work_space[2][0]) + self.work_space[2][0]
# target_x = -0.5283
# target_y = 0.1640
target_z = 0.025
self.open_griper()
self.move_position([target_x + 0.47, target_y, 0.03 - 0.254])
self.target = [target_x, target_y, target_z]
orientation_y = -random.random() * np.pi + np.pi / 2
# print()
# vrep.simxPauseCommunication(self.sim_client, False)
object_orientation = [-np.pi / 2, orientation_y, -np.pi / 2]
curr_mesh_file = '/home/chen/stl-model/surface_car.obj'
curr_shape_name = 'shape001'
object_color = [0.4, 0.7, 1]
ret_resp, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction(
self.sim_client, 'remoteApiCommandServer',
vrep.sim_scripttype_childscript, 'importShape', [0, 0, 255, 0],
self.target + object_orientation + object_color,
[curr_mesh_file, curr_shape_name], bytearray(),
vrep.simx_opmode_blocking)
self.target = np.asarray(self.target)
self.open_griper()
sim_ret, self.current_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
sim_ret, self.current_position = vrep.simxGetObjectPosition(
self.sim_client, self.current_handle, -1,
vrep.simx_opmode_blocking)
if orientation_y < 0:
ratio = -1
else:
ratio = 1
# target_state = np.append(self.target[0:2], ratio * (np.pi/2 - abs(orientation_y))/(np.pi))
# current_state = np.append(self.current_position[0:2], 0)
self.pre_distance = np.linalg.norm(self.target[0:2] -
self.current_position[0:2])
self.pre_orientation_distance = np.linalg.norm(
ratio * (np.pi / 2 - abs(orientation_y)) / (np.pi))
self.current_state, frame, pos = self.get_state()
# test_pos = np.append([0, 0, 0], orientation_y - np.pi/2)
# self.move_to(test_pos)
cs = self.current_state[0:2]
cs = np.append(cs, self.current_state[-1])
return cs, frame, pos
def reset(self):
sim_ret, self.UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking)
time.sleep(1)
sim_ret, self.RG2_tip_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking)
sim_ret, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1,
(-0.47, 0, 0.3), vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle,
-1, (np.pi / 2, 0, np.pi / 2),
vrep.simx_opmode_blocking)
while gripper_position[
2] > 0.4: # V-REP bug requiring multiple starts and stops to restart
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(self.sim_client,
vrep.simx_opmode_blocking)
time.sleep(1)
sim_ret, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle,
-1, (-0.47, 0, 0.3),
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.sim_client,
self.UR5_target_handle, -1,
(np.pi / 2, 0, np.pi / 2),
vrep.simx_opmode_blocking)
# print('started!')
target_x = random.random() * (
self.object_work_space[0][1] -
self.object_work_space[0][0]) + self.object_work_space[0][0]
target_y = random.random() * (
self.object_work_space[1][1] -
self.object_work_space[1][0]) + self.object_work_space[1][0]
# target_z = random.random() * (self.work_space[2][1] - self.work_space[2][0]) + self.work_space[2][0]
target_z = 0.075
self.target = [target_x, target_y, target_z]
# print()
# vrep.simxPauseCommunication(self.sim_client, False)
# orientation_y = -random.random() * np.pi + np.pi / 2
# print()
# vrep.simxPauseCommunication(self.sim_client, False)
# object_orientation = [-np.pi / 2, orientation_y, -np.pi / 2]
object_orientation = [0, 0, 0]
curr_mesh_file = '/home/chen/stl-model/cup.obj'
curr_shape_name = 'shape001'
object_color = [0, 0.6, 1]
ret_resp, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction(
self.sim_client, 'remoteApiCommandServer',
vrep.sim_scripttype_childscript, 'importShape', [0, 0, 255, 0],
self.target + object_orientation + object_color,
[curr_mesh_file, curr_shape_name], bytearray(),
vrep.simx_opmode_blocking)
self.target = np.asarray(self.target)
sim_ret, self.current_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
sim_ret, self.current_position = vrep.simxGetObjectPosition(
self.sim_client, self.current_handle, -1,
vrep.simx_opmode_blocking)
self.pre_distance = np.linalg.norm(self.target[0:2] -
self.current_position[0:2])
self.current_state, frame, pos = self.get_state()
return self.current_state, frame, pos