def get_state(self):
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)
sim_ret, self.current_orientation = vrep.simxGetObjectOrientation(self.sim_client, self.current_handle, -1, vrep.simx_opmode_blocking)
sim_ret, self.target_handle = vrep.simxGetObjectHandle(self.sim_client, 'shape001', vrep.simx_opmode_blocking)
sim_ret, self.target = vrep.simxGetObjectPosition(self.sim_client, self.target_handle, -1, vrep.simx_opmode_blocking)
sim_ret, self.target_orientation = vrep.simxGetObjectOrientation(self.sim_client, self.target_handle, -1, vrep.simx_opmode_blocking)
self.target = np.asarray(self.target)
self.current_position = np.asarray(self.current_position)
if self.target_orientation[1] < 0:
ratio = -1
else:
ratio = 1
if self.object_choose_int == 1:
orientation_dis = 0.
else:
orientation_dis = (self.current_orientation[1] - ratio * (np.pi/2 - abs(self.target_orientation[1]))) / (np.pi)
self.current_state = self.current_position - self.target
self.current_state = np.append(self.current_state, orientation_dis)
# print(orientation_dis)
# print('current position : ', self.current_position, 'target position : ', self.target, 'target orientation : ', self.target_orientation, 'current orientation : ', self.current_orientation)
# print('current state : ', self.current_state)
frame = self.get_sensor_data()
# pos = self.current_position
pos = np.append(self.current_position, self.current_orientation[1])
return self.current_state, frame, pos
def move_down(self, direction=1):
sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
# UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking)
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, UR5_target_handle, -1, vrep.simx_opmode_blocking)
# print(UR5_target_position)
# time.sleep(200)
if direction == 1:
move_direction = np.asarray([0, 0, 0.01 - UR5_target_position[2]])
move_magnitude = UR5_target_position[2] - 0.01
else:
move_direction = np.asarray([0, 0, 0.301 - UR5_target_position[2]])
move_magnitude = 0.301 - UR5_target_position[2]
if move_magnitude == 0 or not move_magnitude == move_magnitude:
move_step = [0, 0, 0]
num_move_steps = 0
print('magnitude error~!', move_magnitude)
else:
move_step = 0.005 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.005))
# print('move_direction : ', move_direction, 'move_magnitude : ', move_magnitude)
# print('move step : ', move_step, 'num : ', num_move_steps)
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(
self.sim_client, UR5_target_handle, -1,
(UR5_target_position[0], UR5_target_position[1],
UR5_target_position[2] + move_step[2] * (step_iter + 1)),
vrep.simx_opmode_blocking)
def move_position(self, tool_position):
sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
# UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking)
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, UR5_target_handle, -1, vrep.simx_opmode_blocking)
if UR5_target_position[2] <= 0.031:
print(UR5_target_position[2])
tool_position[2] = 0
move_direction = np.asarray(tool_position)
move_magnitude = np.linalg.norm(move_direction)
if move_magnitude == 0 or not move_magnitude == move_magnitude:
move_step = [0, 0, 0]
num_move_steps = 0
# print('magnitude error~!', move_magnitude, tool_position)
else:
move_step = 0.005 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.005))
# print(move_step)
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(
self.sim_client, UR5_target_handle, -1,
(UR5_target_position[0] +
move_step[0] * min(step_iter, num_move_steps),
UR5_target_position[1] +
move_step[1] * min(step_iter, num_move_steps),
UR5_target_position[2] +
move_step[2] * min(step_iter, num_move_steps)),
vrep.simx_opmode_blocking)
def get_state(self):
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.current_position = np.asarray(self.current_position)
# print('current position : ', self.current_position, 'target position : ', self.target)
self.current_state = self.current_position - self.target
frame = self.get_sensor_data()
return self.current_state[0:2], frame, self.current_position
def move_to(self, tool_position):
sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
# UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking)
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, UR5_target_handle, -1, vrep.simx_opmode_blocking)
sim_ret, UR5_target_orientation = vrep.simxGetObjectOrientation(
self.sim_client, UR5_target_handle, -1, vrep.simx_opmode_blocking)
# print(UR5_target_position)
tool_orientation = tool_position[-1]
tool_position = tool_position[0:3]
if UR5_target_position[2] <= 0.031:
tool_position[2] = 0
move_direction = np.asarray(tool_position)
move_magnitude = np.linalg.norm(move_direction)
if move_magnitude == 0 or not move_magnitude == move_magnitude:
move_step = [0, 0, 0]
num_move_steps = 0
# print('magnitude error~!', move_magnitude, tool_position)
else:
move_step = 0.005 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.005))
rotation_step = 0.05 if (tool_orientation > 0) else -0.05
num_rotation_steps = int(np.floor(tool_orientation / rotation_step))
# print('move direction : ', move_direction, 'move magnitude : ', move_magnitude, 'move step : ', move_step, 'num : ', num_move_steps)
for step_iter in range(max(num_move_steps, num_rotation_steps)):
vrep.simxSetObjectPosition(
self.sim_client, UR5_target_handle, -1,
(UR5_target_position[0] +
move_step[0] * min(step_iter, num_move_steps),
UR5_target_position[1] +
move_step[1] * min(step_iter, num_move_steps),
UR5_target_position[2] +
move_step[2] * min(step_iter, num_move_steps)),
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(
self.sim_client, UR5_target_handle, -1,
(np.pi / 2, UR5_target_orientation[1] +
rotation_step * min(step_iter, num_rotation_steps),
np.pi / 2), vrep.simx_opmode_blocking)
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
