def move_to(self, tool_position, tool_orientation):
# sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking)
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.02 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.02))
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle,
-1,
(UR5_target_position[0] + move_step[0],
UR5_target_position[1] + move_step[1],
UR5_target_position[2] + move_step[2]),
vrep.simx_opmode_blocking)
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
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)
def restart_sim(self):
sim_ret, self.UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1,
(-0.5, 0, 0.3), vrep.simx_opmode_blocking)
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking)
time.sleep(0.5)
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)
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)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client,
vrep.simx_opmode_blocking)
time.sleep(0.5)
sim_ret, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
def move_linear(self, tool_position, num_steps=10):
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]
])
num_move_steps = num_steps
move_step = move_direction / num_move_steps
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle,
-1,
(UR5_target_position[0] + move_step[0],
UR5_target_position[1] + move_step[1],
UR5_target_position[2] + move_step[2]),
vrep.simx_opmode_blocking)
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
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)
time.sleep(0.05)
def setup_sim_camera(self, resolution_x=1024., resolution_y=1024.):
# Get handle to camera
perspectiveAngle = np.deg2rad(54.70)
self.cam_intrinsics = np.asarray(
[[
resolution_x / (2 * np.tan(perspectiveAngle / 2)), 0,
resolution_x / 2
],
[
0, resolution_y / (2 * np.tan(perspectiveAngle / 2)),
resolution_y / 2
], [0, 0, 1]])
_, self.cam_handle = vrep.simxGetObjectHandle(
self.sim_client, 'Vision_sensor_persp_0',
vrep.simx_opmode_blocking)
# Get camera pose and intrinsics in simulation
_, cam_position = vrep.simxGetObjectPosition(self.sim_client,
self.cam_handle, -1,
vrep.simx_opmode_blocking)
_, cam_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking)
cam_trans = np.eye(4, 4)
cam_trans[0:3, 3] = np.asarray(cam_position)
cam_orientation = [
cam_orientation[0], cam_orientation[1], cam_orientation[2]
]
cam_rotm = np.eye(4, 4)
cam_rotm[0:3, 0:3] = utils.euler2rotm(cam_orientation)
self.cam_pose = np.dot(
cam_trans, cam_rotm
) # Compute rigid transformation representating camera pose
self.cam_depth_scale = 1
def setup_sim_camera(self):
# Get handle to camera
sim_ret, self.cam_handle = vrep.simxGetObjectHandle(
self.sim_client, 'Vision_sensor_ortho', vrep.simx_opmode_blocking)
# Get camera pose and intrinsics in simulation
sim_ret, cam_position = vrep.simxGetObjectPosition(
self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking)
sim_ret, cam_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking)
cam_trans = np.eye(4, 4)
cam_trans[0:3, 3] = np.asarray(cam_position)
cam_orientation = [
-cam_orientation[0], -cam_orientation[1], -cam_orientation[2]
]
cam_rotm = np.eye(4, 4)
cam_rotm[0:3, 0:3] = np.linalg.inv(utils.euler2rotm(cam_orientation))
self.cam_pose = np.dot(
cam_trans, cam_rotm
) # Compute rigid transformation representating camera pose
self.cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240],
[0, 0, 1]])
self.cam_depth_scale = 1
# Get background image
self.bg_color_img, self.bg_depth_img = self.get_camera_data()
self.bg_depth_img = self.bg_depth_img * self.cam_depth_scale
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 check_sim(self):
# Check if simulation is stable by checking if gripper is within workspace
sim_ret, gripper_position = vrep.simxGetObjectPosition(self.sim_client, self.RG2_tip_handle, -1, vrep.simx_opmode_blocking)
sim_ok = gripper_position[0] > self.workspace_limits[0][0] - 0.1 and gripper_position[0] < self.workspace_limits[0][1] + 0.1 and gripper_position[1] > self.workspace_limits[1][0] - 0.1 and gripper_position[1] < self.workspace_limits[1][1] + 0.1 and gripper_position[2] > self.workspace_limits[2][0] and gripper_position[2] < self.workspace_limits[2][1]
if not sim_ok:
print('Simulation unstable. Restarting environment.')
self.restart_sim()
def remove_height_object(self):
maxhandle = self.object_handles[0]
sim_ret, obj_position = vrep.simxGetObjectPosition(
self.clientID, self.object_handles[0], -1,
vrep.simx_opmode_blocking)
maxposition = obj_position[2]
for i in range(1, len(self.object_handles)):
sim_ret, obj_position = vrep.simxGetObjectPosition(
self.clientID, self.object_handles[i], -1,
vrep.simx_opmode_blocking)
if obj_position[2] > maxposition:
maxposition = obj_position[2]
maxhandle = self.object_handles[i]
#print(obj_position)
vrep.simxSetObjectPosition(self.clientID, maxhandle, -1,
(0.5, -0.2, 0.3), vrep.simx_opmode_blocking)
def get_obj_positions(self):
obj_positions = []
for object_handle in self.object_handles:
sim_ret, object_position = vrep.simxGetObjectPosition(
self.sim_client, object_handle, -1, vrep.simx_opmode_blocking)
obj_positions.append(object_position)
return obj_positions
def checkState(self):
sim_ret, UR3_target_position = vrep.simxGetObjectPosition(
self.clientID, self.UR3_target_handle, -1,
vrep.simx_opmode_blocking)
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(
self.clientID, 'RG2_openCloseJoint', vrep.simx_opmode_blocking)
sim_ret, RG2_gripper_position = vrep.simxGetObjectPosition(
self.clientID, RG2_gripper_handle, -1, vrep.simx_opmode_blocking)
#print("===================================================================")
#print(UR3_target_position)
#print(RG2_gripper_position)
dist=pow(pow(UR3_target_position[0]-RG2_gripper_position[0],2)+\
pow(UR3_target_position[1] - RG2_gripper_position[1], 2) +\
pow(UR3_target_position[2] - RG2_gripper_position[2], 2),0.5)
if dist > 0.1:
return True #异常状态
else:
return False
def move_abs_world(self, target_position):
# sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking)
sim_ret, UR3_target_position = vrep.simxGetObjectPosition(
self.clientID, self.UR3_target_handle, -1,
vrep.simx_opmode_blocking)
move_direction = np.asarray([
target_position[0] - UR3_target_position[0],
target_position[1] - UR3_target_position[1],
target_position[2] - UR3_target_position[2]
])
self.move_relative(move_direction)
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 setVisionSensor(self):
sim_ret, self.VS_left_handle = vrep.simxGetObjectHandle(
self.clientID, 'Vision_sensor_left', vrep.simx_opmode_blocking)
ret, self.VS_left_position = vrep.simxGetObjectPosition(
self.clientID, self.VS_left_handle, -1, vrep.simx_opmode_blocking)
ret, self.VS_left_orientation = vrep.simxGetObjectOrientation(
self.clientID, self.VS_left_handle, -1, vrep.simx_opmode_blocking)
self.left_cam_intrinsics = np.asarray([[561.82, 0, 320],
[0, 561.82, 240], [0, 0,
1]]) # 内部参数
self.leftmat = utils.Creat_posemat(self.VS_left_orientation,
self.VS_left_position)
sim_ret, self.VS_right_handle = vrep.simxGetObjectHandle(
self.clientID, 'Vision_sensor_right', vrep.simx_opmode_blocking)
ret, self.VS_right_position = vrep.simxGetObjectPosition(
self.clientID, self.VS_right_handle, -1, vrep.simx_opmode_blocking)
ret, self.VS_right_orientation = vrep.simxGetObjectOrientation(
self.clientID, self.VS_right_handle, -1, vrep.simx_opmode_blocking)
self.right_cam_intrinsics = np.asarray([[561.82, 0, 320],
[0, 561.82, 240], [0, 0,
1]]) # 内部参数
self.rightmat = utils.Creat_posemat(self.VS_right_orientation,
self.VS_right_position)
def get_obj_masks(self):
# from scipy.spatial.transform import Rotation as R
obj_contours = []
obj_number = len(self.obj_mesh_ind)
# scene = trimesh.Scene()
for object_idx in range(obj_number):
# Get object pose in simulation
sim_ret, obj_position = vrep.simxGetObjectPosition(
self.sim_client, self.object_handles[object_idx], -1,
vrep.simx_opmode_blocking)
sim_ret, obj_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.object_handles[object_idx], -1,
vrep.simx_opmode_blocking)
obj_trans = np.eye(4, 4)
obj_trans[0:3, 3] = np.asarray(obj_position)
obj_orientation = [
obj_orientation[0], obj_orientation[1], obj_orientation[2]
]
obj_rotm = np.eye(4, 4)
obj_rotm[0:3, 0:3] = utils.obj_euler2rotm(obj_orientation)
obj_pose = np.dot(
obj_trans, obj_rotm
) # Compute rigid transformation representating camera pose
# load .obj files
obj_mesh_file = os.path.join(
self.obj_mesh_dir,
self.mesh_list[self.obj_mesh_ind[object_idx]])
# print(obj_mesh_file)
mesh = trimesh.load_mesh(obj_mesh_file)
if obj_mesh_file.split('/')[-1] == '2.obj' or obj_mesh_file.split(
'/')[-1] == '6.obj':
mesh.apply_transform(obj_pose)
else:
# rest
transformation = np.array([[0, 0, 1, 0], [0, -1, 0, 0],
[1, 0, 0, 0], [0, 0, 0, 1]])
mesh.apply_transform(transformation)
mesh.apply_transform(obj_pose)
# scene.add_geometry(mesh)
obj_contours.append(mesh.vertices[:, 0:2])
# scene.show()
return obj_contours
def get_test_obj_masks(self):
obj_contours = []
obj_number = len(self.test_obj_mesh_files)
for object_idx in range(obj_number):
# Get object pose in simulation
sim_ret, obj_position = vrep.simxGetObjectPosition(
self.sim_client, self.object_handles[object_idx], -1,
vrep.simx_opmode_blocking)
sim_ret, obj_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.object_handles[object_idx], -1,
vrep.simx_opmode_blocking)
obj_trans = np.eye(4, 4)
obj_trans[0:3, 3] = np.asarray(obj_position)
obj_orientation = [
obj_orientation[0], obj_orientation[1], obj_orientation[2]
]
obj_rotm = np.eye(4, 4)
obj_rotm[0:3, 0:3] = utils.obj_euler2rotm(obj_orientation)
obj_pose = np.dot(
obj_trans, obj_rotm
) # Compute rigid transformation representating camera pose
# load .obj files
obj_mesh_file = self.test_obj_mesh_files[object_idx]
# print(obj_mesh_file)
mesh = trimesh.load_mesh(obj_mesh_file)
if obj_mesh_file.split('/')[-1] == '2.obj' or obj_mesh_file.split(
'/')[-1] == '6.obj':
mesh.apply_transform(obj_pose)
else:
# rest
transformation = np.array([[0, 0, 1, 0], [0, -1, 0, 0],
[1, 0, 0, 0], [0, 0, 0, 1]])
mesh.apply_transform(transformation)
mesh.apply_transform(obj_pose)
obj_contours.append(mesh.vertices[:, 0:2])
return obj_contours
def get_obj_mask(self, obj_ind):
# Get object pose in simulation
sim_ret, obj_position = vrep.simxGetObjectPosition(
self.sim_client, self.object_handles[obj_ind], -1,
vrep.simx_opmode_blocking)
sim_ret, obj_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.object_handles[obj_ind], -1,
vrep.simx_opmode_blocking)
obj_trans = np.eye(4, 4)
obj_trans[0:3, 3] = np.asarray(obj_position)
obj_orientation = [
obj_orientation[0], obj_orientation[1], obj_orientation[2]
]
obj_rotm = np.eye(4, 4)
obj_rotm[0:3, 0:3] = utils.obj_euler2rotm(obj_orientation)
obj_pose = np.dot(
obj_trans, obj_rotm
) # Compute rigid transformation representating camera pose
# load .obj files
obj_mesh_file = os.path.join(
self.obj_mesh_dir, self.mesh_list[self.obj_mesh_ind[obj_ind]])
mesh = trimesh.load_mesh(obj_mesh_file)
# transform the mesh to world frame
if obj_mesh_file.split('/')[-1] == '2.obj' or obj_mesh_file.split(
'/')[-1] == '6.obj':
mesh.apply_transform(obj_pose)
else:
# rest
transformation = np.array([[0, 0, 1, 0], [0, -1, 0, 0],
[1, 0, 0, 0], [0, 0, 0, 1]])
mesh.apply_transform(transformation)
mesh.apply_transform(obj_pose)
obj_contour = mesh.vertices[:, 0:2]
return obj_contour
def push(self, postion, angle, dp=0.1):
sim_ret, UR3_target_position = vrep.simxGetObjectPosition(
self.clientID, self.UR3_target_handle, -1,
vrep.simx_opmode_blocking)
#self.move_abs_ownframe([0.15, 0.15, 0.2])
self.move_relative([0, 0, 0.2 - UR3_target_position[2]], step=0.02)
self.move_abs_ownframe([postion[0], postion[1], 0.2])
self.rotate_gripper(angle + 90)
self.move_abs_ownframe([postion[0], postion[1], 0.002])
rad = angle / 180 * math.pi
to_x = postion[0] + dp * math.cos(rad)
to_y = postion[1] + dp * math.sin(rad)
if (to_x > self.WorkSpace_size):
to_x = self.WorkSpace_size
if (to_x < 0):
to_x = 0
if (to_y > self.WorkSpace_size):
to_y = self.WorkSpace_size
if (to_y < 0):
to_y = 0
#print("to_x",to_x,"to_y",to_y)
self.move_abs_ownframe([to_x, to_y, 0.002])
self.move_abs_ownframe([to_x, to_y, 0.2])
def move_relative(self, dp, step=0.02):
'''
:param dp: [dx,dy,dz]
:param step: step lenght
:return:
'''
move_direction = np.asarray(dp)
move_magnitude = np.linalg.norm(move_direction)
move_step = step * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / step))
for step_iter in range(num_move_steps):
sim_ret, UR3_target_position = vrep.simxGetObjectPosition(
self.clientID, self.UR3_target_handle, -1,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.clientID, self.UR3_target_handle,
-1,
(UR3_target_position[0] + move_step[0],
UR3_target_position[1] + move_step[1],
UR3_target_position[2] + move_step[2]),
vrep.simx_opmode_blocking)
def env_prepare(self):
sim_ret, self.UR3_target_handle = vrep.simxGetObjectHandle(
self.clientID, 'UR3_Target', vrep.simx_opmode_blocking)
sim_ret, self.WorkSpace_handle = vrep.simxGetObjectHandle(
self.clientID, 'WorkSpace', vrep.simx_opmode_blocking)
sim_ret, self.WorkSpace_position = vrep.simxGetObjectPosition(
self.clientID, self.WorkSpace_handle, -1,
vrep.simx_opmode_blocking)
print("WorkSpace_position", self.WorkSpace_position)
self.WorkSpace_size = 0.34 # 0.36*0.36
self.workspace_limits = np.array(
[[
self.WorkSpace_position[0] - self.WorkSpace_size / 2,
self.WorkSpace_position[0] + self.WorkSpace_size / 2
],
[
self.WorkSpace_position[1] - self.WorkSpace_size / 2,
self.WorkSpace_position[1] + self.WorkSpace_size / 2
]])
self.move_abs_ownframe(
[self.WorkSpace_size / 2, self.WorkSpace_size / 2, 0.3])
self.setangle_360(0)
self.setVisionSensor()
self.color_space = np.asarray([
[78.0, 121.0, 167.0], # blue
[89.0, 161.0, 79.0], # green
[156, 117, 95], # brown
[242, 142, 43], # orange
[237.0, 201.0, 72.0], # yellow
[186, 176, 172], # gray
[255.0, 87.0, 89.0], # red
[176, 122, 161], # purple
[118, 183, 178], # cyan
[255, 157, 167]
]) / 255 # pink
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 getObjectPosition(sim_client, obj_handle):
sim_ret, obj_position = vrep.simxGetObjectPosition(sim_client, obj_handle,
-1, VREP_BLOCKING)
return sim_ret, np.asarray(obj_position)
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
#Setup
is_sim = True
workspace_limits = np.asarray([
[-0.724, -0.276], [-0.224, 0.224], [-0.0001, 0.4]
]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates)
robot = Robot(is_sim, workspace_limits)
#Variable Creation
heightmap_rotation_angle = 0 #needed for grasp function
workspace_center = [-.5, 0, .03] #where the objects will be placed
#Handle of UR5_target, needed as a home for the robotic arm
sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(
robot.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
robot.sim_client, UR5_target_handle, -1, vrep.simx_opmode_blocking)
#Camera With YOLOv5
robot.setup_sim_camera()
rgb, depth = robot.get_camera_data()
workspace_picture = Image.fromarray(rgb)
workspace_picture.save(
"/home/saeid/Downloads/CoppeliaSim_Edu_V4_1_0_Ubuntu18_04/programming/remoteApiBindings/python/python/data/images/workspace.jpg"
)
#Detect number of objects
objectsPresent = detect.detect(False) #filled with objects detected by cameras
objectsPresent = int(objectsPresent)
print("Number of Objects detected: ", objectsPresent)
towerTip = workspace_center #tip of tower, where next block will be placed
tipRaise = .06 #how much towerTip must be raised by to properly place next object, slightly taller than picked object
def get_ur5_position(self): sim_ret, UR5_target_position = vrep.simxGetObjectPosition(self.sim_client, self.UR5_target_handle, -1, vrep.simx_opmode_blocking) return UR5_target_position
def get_single_obj_position(self, object_handle):
_, obj_position = vrep.simxGetObjectPosition(self.sim_client,
object_handle, -1,
vrep.simx_opmode_blocking)
return np.asarray(obj_position)
