def reposition_objects(self, workspace_limits):
# Move gripper out of the way
self.move_to([-0.1, 0, 0.3], None)
# sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking)
# vrep.simxSetObjectPosition(self.sim_client, UR5_target_handle, -1, (-0.5,0,0.3), vrep.simx_opmode_blocking)
# time.sleep(1)
for object_handle in self.object_handles:
# Drop object at random x,y location and random orientation in robot workspace
drop_x = (workspace_limits[0][1] - workspace_limits[0][0] - 0.2) * np.random.random_sample() + \
workspace_limits[0][0] + 0.1
drop_y = (workspace_limits[1][1] - workspace_limits[1][0] - 0.2) * np.random.random_sample() + \
workspace_limits[1][0] + 0.1
object_position = [drop_x, drop_y, 0.15]
object_orientation = [
2 * np.pi * np.random.random_sample(),
2 * np.pi * np.random.random_sample(),
2 * np.pi * np.random.random_sample()
]
vrep.simxSetObjectPosition(self.sim_client, object_handle, -1,
object_position,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.sim_client, object_handle, -1,
object_orientation,
vrep.simx_opmode_blocking)
time.sleep(2)
def _rotate_gripper_z(self, target_angle, num_step=None):
_, current_angle = vrep.simxGetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
# if target_angle > np.pi:
# target_angle -= np.pi
# if target_angle < -np.pi:
# target_angle += np.pi
if target_angle > np.pi + 0.2:
target_angle -= 2 * np.pi
if target_angle < -np.pi - 0.2:
target_angle += 2 * np.pi
ori_direction = np.asarray([0., 0., target_angle - current_angle[2]])
if num_step is not None:
num_move_step = num_step
else:
diff = abs(np.rad2deg(ori_direction[2]))
num_move_step = int((diff // 5) + 1)
ori_move_step = ori_direction / num_move_step
for step_iter in range(num_move_step):
vrep.simxSetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
(current_angle[0] + ori_move_step[0], current_angle[1] +
ori_move_step[1], current_angle[2] + ori_move_step[2]),
vrep.simx_opmode_blocking)
_, current_angle = vrep.simxGetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
(current_angle[0], current_angle[1], target_angle),
vrep.simx_opmode_blocking)
time.sleep(0.1)
def setangle_360(self, angle):
angle = angle / 180 * math.pi
if (angle >= 0):
vrep.simxSetObjectOrientation(
self.clientID, self.UR3_target_handle, -1,
(math.pi / 2, angle - math.pi / 2, math.pi / 2),
vrep.simx_opmode_blocking)
else:
vrep.simxSetObjectOrientation(
self.clientID, self.UR3_target_handle, -1,
(-math.pi / 2, -angle - math.pi / 2, -math.pi / 2),
vrep.simx_opmode_blocking)
def rotate_object(self, axis, heightmap_rotation_angle, workspace_limits): #tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2 tool_rotation_angle = math.radians(np.float(heightmap_rotation_angle)) sim_ret, UR5_target_position = vrep.simxGetObjectPosition(self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) # Compute gripper orientation and rotation increments sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) rotation_step = 0.15 if (tool_rotation_angle - gripper_orientation[axis] > 0) else -0.15 num_rotation_steps = int(np.floor((tool_rotation_angle - gripper_orientation[axis]) / rotation_step)) # Simultaneously move and rotate gripper if axis==0: for step_iter in range(num_rotation_steps): vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, ( gripper_orientation[axis] + rotation_step * min(step_iter, num_rotation_steps), gripper_orientation[1], gripper_orientation[2]), vrep.simx_opmode_blocking) elif axis==1: for step_iter in range(num_rotation_steps): vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, ( gripper_orientation[0], gripper_orientation[axis] + rotation_step * min(step_iter, num_rotation_steps), gripper_orientation[2]), vrep.simx_opmode_blocking) elif axis==2: for step_iter in range(num_rotation_steps): vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle, -1, ( gripper_orientation[0], gripper_orientation[1], gripper_orientation[axis] + rotation_step * min(step_iter, num_rotation_steps)), vrep.simx_opmode_blocking)
def restart_sim(self):
_, self.UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
_, self.UR5_tip_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1,
self.gripper_home_pos,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle,
-1, self.gripper_home_ori,
vrep.simx_opmode_blocking)
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking)
time.sleep(0.3)
_, self.RG2_tip_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking)
_, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
while gripper_position[2] > self.gripper_home_pos[
2] + 0.01: # 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)
self.open_RG2_gripper()
self.obj_target_handles = []
if self.is_insert_task:
self.hole_handles = []
self.add_hole()
self.add_objects()
time.sleep(0.8)
def push(self, position, heightmap_rotation_angle, workspace_limits):
print('Executing: push at (%f, %f, %f)' %
(position[0], position[1], position[2]))
# Compute tool orientation from heightmap rotation angle
tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2
# Adjust pushing point to be on tip of finger
position[2] = position[2] + 0.015
# Compute pushing direction
push_orientation = [1.0, 0.0]
push_direction = np.asarray([
push_orientation[0] * np.cos(heightmap_rotation_angle) -
push_orientation[1] * np.sin(heightmap_rotation_angle),
push_orientation[0] * np.sin(heightmap_rotation_angle) +
push_orientation[1] * np.cos(heightmap_rotation_angle)
])
# Move gripper to location above pushing point
pushing_point_margin = 0.1
location_above_pushing_point = (position[0], position[1],
position[2] + pushing_point_margin)
# Compute gripper position and linear movement increments
tool_position = location_above_pushing_point
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
move_direction = np.asarray([
tool_position[0] - UR5_target_position[0],
tool_position[1] - UR5_target_position[1],
tool_position[2] - UR5_target_position[2]
])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.05 * move_direction / move_magnitude + 1e-8
num_move_steps = int(np.floor(move_direction[0] / move_step[0]))
# Compute gripper orientation and rotation increments
sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
rotation_step = 0.3 if (
tool_rotation_angle - gripper_orientation[1] > 0) else -0.3
num_rotation_steps = int(
np.floor((tool_rotation_angle - gripper_orientation[1]) /
rotation_step))
# Simultaneously move and rotate gripper
for step_iter in range(max(num_move_steps, num_rotation_steps)):
vrep.simxSetObjectPosition(
self.sim_client, self.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, self.UR5_target_handle, -1,
(np.pi / 2, gripper_orientation[1] +
rotation_step * min(step_iter, num_rotation_steps),
np.pi / 2), vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
(tool_position[0], tool_position[1], tool_position[2]),
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
(np.pi / 2, tool_rotation_angle, np.pi / 2),
vrep.simx_opmode_blocking)
# Ensure gripper is closed
self.close_gripper()
# Approach pushing point
self.move_to(position, None)
# Compute target location (push to the right)
push_length = 0.1
target_x = min(
max(position[0] + push_direction[0] * push_length,
workspace_limits[0][0]), workspace_limits[0][1])
target_y = min(
max(position[1] + push_direction[1] * push_length,
workspace_limits[1][0]), workspace_limits[1][1])
push_length = np.sqrt(
np.power(target_x - position[0], 2) +
np.power(target_y - position[1], 2))
# Move in pushing direction towards target location
self.move_to([target_x, target_y, position[2]], None)
# Move gripper to location above grasp target
self.move_to([target_x, target_y, location_above_pushing_point[2]],
None)
def grasp(self, position, heightmap_rotation_angle, workspace_limits):
print('Executing: grasp at (%f, %f, %f)' %
(position[0], position[1], position[2]))
# Compute tool orientation from heightmap rotation angle
tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2
# Avoid collision with floor
position = np.asarray(position).copy()
position[2] = max(position[2] - 0.04, workspace_limits[2][0] + 0.02)
# Move gripper to location above grasp target
grasp_location_margin = 0.15
# sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking)
location_above_grasp_target = (position[0], position[1],
position[2] + grasp_location_margin)
# Compute gripper position and linear movement increments
tool_position = location_above_grasp_target
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
move_direction = np.asarray([
tool_position[0] - UR5_target_position[0],
tool_position[1] - UR5_target_position[1],
tool_position[2] - UR5_target_position[2]
])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.05 * move_direction / move_magnitude + 1e-8
num_move_steps = int(np.floor(move_direction[0] / move_step[0]))
# Compute gripper orientation and rotation increments
sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
rotation_step = 0.3 if (
tool_rotation_angle - gripper_orientation[1] > 0) else -0.3
num_rotation_steps = int(
np.floor((tool_rotation_angle - gripper_orientation[1]) /
rotation_step))
# Simultaneously move and rotate gripper
for step_iter in range(max(num_move_steps, num_rotation_steps)):
vrep.simxSetObjectPosition(
self.sim_client, self.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, self.UR5_target_handle, -1,
(np.pi / 2, gripper_orientation[1] +
rotation_step * min(step_iter, num_rotation_steps),
np.pi / 2), vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
(tool_position[0], tool_position[1], tool_position[2]),
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
(np.pi / 2, tool_rotation_angle, np.pi / 2),
vrep.simx_opmode_blocking)
# Ensure gripper is open
self.open_gripper()
# Approach grasp target
self.move_to(position, None)
# Close gripper to grasp target
gripper_full_closed = self.close_gripper()
# Move gripper to location above grasp target
self.move_to(location_above_grasp_target, None)
# Check if grasp is successful
gripper_full_closed = self.close_gripper()
grasp_success = not gripper_full_closed
# Move the grasped object elsewhere
grasped_object_name = None
if grasp_success:
object_positions = np.asarray(self.get_obj_positions())
object_positions = object_positions[:, 2]
grasped_object_ind = np.argmax(object_positions)
grasped_object_handle = self.object_handles[grasped_object_ind]
grasped_object_name = self.object_names[grasped_object_ind]
del self.object_handles[grasped_object_ind]
del self.object_names[grasped_object_ind]
vrep.simxSetObjectPosition(
self.sim_client, grasped_object_handle, -1,
(-1, -1 + 0.05 * float(grasped_object_ind), 0.1),
vrep.simx_opmode_blocking)
return grasped_object_name
def set_single_obj_orientation(self, object_handle, goal_ori):
_ = vrep.simxSetObjectOrientation(self.sim_client, object_handle, -1,
goal_ori, vrep.simx_opmode_blocking)
def grasp(self, position, heightmap_rotation_angle, workspace_limits):
print('Executing: grasp at (%f, %f, %f)' %
(position[0], position[1], position[2]))
# Compute tool orientation from heightmap rotation angle
tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2
# Avoid collision with floor
position = np.asarray(position).copy()
position[2] = max(position[2] - 0.04, workspace_limits[2][0] + 0.02)
# Move gripper to location above grasp target
grasp_location_margin = 0.15
# sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking)
location_above_grasp_target = (position[0], position[1],
position[2] + grasp_location_margin)
# Compute gripper position and linear movement increments
tool_position = location_above_grasp_target
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
move_direction = np.asarray([
tool_position[0] - UR5_target_position[0],
tool_position[1] - UR5_target_position[1],
tool_position[2] - UR5_target_position[2]
])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.05 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_direction[0] / move_step[0]))
# Compute gripper orientation and rotation increments
sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
rotation_step = 0.3 if (
tool_rotation_angle - gripper_orientation[1] > 0) else -0.3
num_rotation_steps = int(
np.floor((tool_rotation_angle - gripper_orientation[1]) /
rotation_step))
# Simultaneously move and rotate gripper
for step_iter in range(max(num_move_steps, num_rotation_steps)):
vrep.simxSetObjectPosition(
self.sim_client, self.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, self.UR5_target_handle, -1,
(np.pi / 2, gripper_orientation[1] +
rotation_step * min(step_iter, num_rotation_steps),
np.pi / 2), vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
(tool_position[0], tool_position[1], tool_position[2]),
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(
self.sim_client, self.UR5_target_handle, -1,
(np.pi / 2, tool_rotation_angle, np.pi / 2),
vrep.simx_opmode_blocking)
# Ensure gripper is open
self.open_gripper()
# Approach grasp target
self.move_to(position, None)
# Get images before grasping
color_img, depth_img = self.get_camera_data()
depth_img = depth_img * self.cam_depth_scale # Apply depth scale from calibration
# Get heightmaps beforew grasping
color_heightmap, depth_heightmap = utils.get_heightmap(
color_img, depth_img, self.cam_intrinsics, self.cam_pose,
workspace_limits, 0.002) # heightmap resolution from args
valid_depth_heightmap = depth_heightmap.copy()
valid_depth_heightmap[np.isnan(valid_depth_heightmap)] = 0
# Close gripper to grasp target
gripper_full_closed = self.close_gripper()
# Move gripper to location above grasp target
self.move_to(location_above_grasp_target, None)
# Check if grasp is successful
gripper_full_closed = self.close_gripper()
grasp_success = not gripper_full_closed
# Move the grasped object elsewhere
if grasp_success:
object_positions = np.asarray(self.get_obj_positions())
object_positions = object_positions[:, 2]
grasped_object_ind = np.argmax(object_positions)
print('grasp obj z position', max(object_positions))
grasped_object_handle = self.object_handles[grasped_object_ind]
vrep.simxSetObjectPosition(
self.sim_client, grasped_object_handle, -1,
(-0.5, 0.5 + 0.05 * float(grasped_object_ind), 0.1),
vrep.simx_opmode_blocking)
return grasp_success, color_img, depth_img, color_heightmap, valid_depth_heightmap, grasped_object_ind
else:
return grasp_success, None, None, None, None, None
def setObjectOrientation(sim_client, obj_handle, orientation):
return vrep.simxSetObjectOrientation(sim_client, obj_handle, -1,
orientation, VREP_BLOCKING)
