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 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 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 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]