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 grasp(self):
self.total_try += 1
self.before_grasp()
self.move_down()
# time.sleep(0.5)
self.close_gripper()
self.move_down(-1) # move up
time.sleep(1)
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(
self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
if 0.055 >= gripper_joint_position >= -0.04:
success = True
else:
success = False
if success:
self.total_success += 1
accuracy = float(self.total_success) / self.total_try
info = {'grasp success rate': accuracy}
for tag, value in info.items():
self.logger.scalar_summary(tag, value, step=self.total_try)
return success
def get_sensor_data(self, is_save=False):
sim_ret, cam_handle = vrep.simxGetObjectHandle(
self.sim_client, 'Vision_sensor_hand', vrep.simx_opmode_blocking)
# Get color image from simulation
sim_ret, resolution, raw_image = vrep.simxGetVisionSensorImage(
self.sim_client, cam_handle, 0, vrep.simx_opmode_blocking)
color_img = np.asarray(raw_image)
# print(color_img.shape)
color_img.shape = (resolution[1], resolution[0], 3)
# print(color_img)
color_img = color_img.astype(np.float) / 255
color_img[color_img < 0] += 1
# color_img *= 255
color_img = np.fliplr(color_img)
test_show_image = color_img * 255
test_show_image = test_show_image.astype(np.uint8)
test_show_image = cv.cvtColor(test_show_image, cv.COLOR_BGR2GRAY)
resize_color = cv.resize(test_show_image, (image_pix, image_pix))
# print(resize_color.shape)
# cropped_color = resize_color[84:132, 78:126]
cropped_color = resize_color
if is_save:
# print(test_show_image.shape)
# test_show_image = Image.fromarray(test_show_image)
# test_show_image.show()
cv.imwrite(
'/home/chen/PycharmProjects/Reinforcement/VisualGrasp/image/aob'
+ str(self.target_orientation[0]) + '_' +
str(self.target_orientation[1]) + '_' +
str(self.target_orientation[2]) + '_' + '.png', cropped_color)
cv.imwrite(
'/home/chen/PycharmProjects/Reinforcement/VisualGrasp/image/aob'
+ str(self.target_orientation[0]) + '_' +
str(self.target_orientation[1]) + '_' +
str(self.target_orientation[2]) + '_big' + '.png',
resize_color)
# print(cropped_color.shape)
cropped_color = np.asarray(cropped_color)
# print(cropped_color.shape)
# cv.imwrite('t.png', resize_color)
cropped_color = cropped_color.astype(np.float) / 255.
# print(cropped_color.shape)
# np.set_printoptions(threshold=np.inf, suppress=True)
# f = open('test.txt', 'w')
# print(resize_color, file=f)
# f.close()
# print('ok')
# time.sleep(5)
# print(cropped_color.shape)
return cropped_color
def get_target_orientation(self):
sim_ret, self.target_handle = vrep.simxGetObjectHandle(self.sim_client, 'shape001', vrep.simx_opmode_blocking)
sim_ret, self.target_orientation = vrep.simxGetObjectOrientation(self.sim_client, self.target_handle, -1,
vrep.simx_opmode_blocking)
# print('target orientation : ', self.target_orientation)
if self.object_choose_int == 1:
return np.pi / 2
else:
return self.target_orientation[1]
def move_orientation(self, tool_orientation):
sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
sim_ret, UR5_target_orientation = vrep.simxGetObjectOrientation(self.sim_client, UR5_target_handle, -1,
vrep.simx_opmode_blocking)
rotation_step = 0.05 if (tool_orientation > 0) else -0.05
num_rotation_steps = int(np.floor(tool_orientation / rotation_step))
for step_iter in range(num_rotation_steps):
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 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 before_grasp(self):
sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
sim_ret, UR5_orientation = vrep.simxGetObjectOrientation(
self.sim_client, UR5_target_handle, -1, vrep.simx_opmode_blocking)
if UR5_orientation[1] > 0:
vrep.simxSetObjectPosition(self.sim_client, UR5_target_handle,
UR5_target_handle, [0, -0.005, 0],
vrep.simx_opmode_blocking)
else:
vrep.simxSetObjectPosition(self.sim_client, UR5_target_handle,
UR5_target_handle, [0, 0.005, 0],
vrep.simx_opmode_blocking)
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 open_griper(self):
gripper_motor_velocity = 0.5
gripper_motor_force = 20
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(self.sim_client, 'RG2_openCloseJoint',
vrep.simx_opmode_blocking)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle,
vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity,
vrep.simx_opmode_blocking)
gripper_fully_opened = False
while gripper_joint_position < 0.0536: # Block until gripper is fully open
sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle,
vrep.simx_opmode_blocking)
if new_gripper_joint_position <= gripper_joint_position:
return gripper_fully_opened
gripper_joint_position = new_gripper_joint_position
gripper_fully_opened = True
return gripper_fully_opened
def close_gripper(self):
gripper_motor_velocity = -0.5
gripper_motor_force = 100
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(self.sim_client, 'RG2_openCloseJoint',
vrep.simx_opmode_blocking)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle,
vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity,
vrep.simx_opmode_blocking)
gripper_fully_closed = False
while gripper_joint_position > -0.047: # Block until gripper is fully closed
sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle,
vrep.simx_opmode_blocking)
# print(gripper_joint_position)
if new_gripper_joint_position >= gripper_joint_position:
return gripper_fully_closed
gripper_joint_position = new_gripper_joint_position
gripper_fully_closed = True
return gripper_fully_closed
def grasp(self):
self.total_try += 1
self.move_down()
# time.sleep(0.5)
self.close_gripper()
self.move_down(-1) # move up
time.sleep(1)
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(
self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
if 0.055 >= gripper_joint_position >= -0.04:
success = True
else:
success = False
if success:
self.total_success += 1
accuracy = float(self.total_success) / self.total_try
self.writer.add_scalar('grasp success rate', accuracy, self.total_try)
return success
def get_target_orientation(self):
sim_ret, self.target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'shape001', vrep.simx_opmode_blocking)
sim_ret, self.target_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.target_handle, -1, vrep.simx_opmode_blocking)
return self.target_orientation[1]
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
