def move_to(self, tool_position, tool_orientation):
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
try:
num_move_steps = int(np.floor(move_magnitude / 0.02))
except ValueError:
return False
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 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 grasp_centroid_data():
wd = '/home/lou00015/data/gsp/'
cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
eid = 0
nb_grasp = 25
if cid != -1:
pos = [0, 0, 0.15]
while True:
vrep.simxStartSimulation(cid, vrep.simx_opmode_blocking)
panda = Robot(cid)
obj_name, obj_hdl = add_object(cid, 'imported_part_0', pos)
time.sleep(1.0)
cloud = panda.get_pointcloud()
centroid = np.average(cloud, axis=0)
print('centroid: ', centroid)
if len(cloud) == 0:
print('no cloud found')
continue
elif centroid[2] > 0.045:
print('perception error')
continue
np.save(wd + 'cloud_' + str(eid) + '.npy', cloud) # save point cloud
cloud = np.delete(cloud, np.where(cloud[:,2]<0.015), axis=0)
pose_set, pt_set = grasp_pose_generation(45, cloud, nb_grasp)
pose = pose_set[10]
pt = centroid
emptyBuff = bytearray()
landing_mtx = [pose[0][0],pose[0][1],pose[0][2],pt[0],
pose[1][0],pose[1][1], pose[1][2],pt[1],
pose[2][0],pose[2][1],pose[2][2],pt[2]]
np.save(wd + 'action_' + str(eid) + '.npy', landing_mtx) # save action
vrep.simxCallScriptFunction(cid, 'landing', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking)
ending_mtx = [pose[0][0], pose[0][1], pose[0][2], pt[0],
pose[1][0], pose[1][1], pose[1][2], pt[1],
pose[2][0], pose[2][1], pose[2][2], pt[2]+0.15]
vrep.simxCallScriptFunction(cid, 'ending', vrep.sim_scripttype_childscript, 'setending', [], ending_mtx,
[], emptyBuff, vrep.simx_opmode_blocking)
time.sleep(1.0)
print('executing experiment %d: ' % (eid))
print('at: ', pt)
vrep.simxCallScriptFunction(cid, 'Sphere', vrep.sim_scripttype_childscript, 'grasp', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
while True:
res, finish = vrep.simxGetIntegerSignal(cid, "finish", vrep.simx_opmode_oneshot_wait)
if finish == 18:
res, end_pos = vrep.simxGetObjectPosition(cid, obj_hdl, -1, vrep.simx_opmode_blocking)
break
if end_pos[2]>0.05:
label = 1
else:
label = 0
print(label)
f = open(wd + 'label.txt', 'a+')
f.write(str(label))
f.close()
eid += 1
else:
print('Failed to connect to simulation (V-REP remote API server). Exiting.')
exit()
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 = self.workspace_limits[0][0] - 0.1 < gripper_position[0] < self.workspace_limits[0][1] + 0.1 and \
self.workspace_limits[1][0] - 0.1 < gripper_position[1] < self.workspace_limits[1][1] + 0.1 and \
self.workspace_limits[2][0] < gripper_position[2] < self.workspace_limits[2][1]
if not sim_ok:
logging.info('Simulation unstable. Restarting environment.')
self.restart_sim()
self.add_objects()
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)
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)
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)
def setup_sim(self):
# Connect to simulator
self.sim_client = -1
vrep.simxFinish(-1) # Just in case, close all opened connections
logging.info('Connecting to simulation...')
while self.sim_client == -1:
self.sim_client = vrep.simxStart('127.0.0.1', self.sim_port, True,
True, 5000, 5)
if self.sim_client == -1:
logging.error(
'Failed to connect to simulation. Trying again..')
time.sleep(5)
else:
logging.info('Connected to simulation.')
self.restart_sim()
break
# Get handle to camera
sim_ret, self.cam_handle = vrep.simxGetObjectHandle(
self.sim_client, 'Vision_sensor_persp', 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 gsp_test():
wd = '/home/lou00015/data/gsp_test/'
model = GSP3d().cuda()
model.load_state_dict(torch.load('gsp.pt'))
model.eval()
cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
eid = 0
nb_grasp = 300
if cid != -1:
pos = [0, 0, 0.15]
while True:
vrep.simxStartSimulation(cid, vrep.simx_opmode_blocking)
panda = Robot(cid)
obj_name, obj_hdl = add_object(cid, 'imported_part_0', pos)
time.sleep(1.0)
cloud = panda.get_pointcloud()
centroid = np.average(cloud, axis=0)
if len(cloud) == 0:
print('no cloud found')
continue
elif centroid[2] > 0.045:
print('perception error')
continue
# np.save(wd + 'cloud_' + str(eid) + '.npy', cloud) # save point cloud
cloud = np.delete(cloud, np.where(cloud[:, 2] < 0.015), axis=0)
v = voxelize(cloud - centroid, 0.1)
pose_set, pt_set = grasp_pose_generation(45, cloud, nb_grasp)
x1, x2 = [], []
emptyBuff = bytearray()
for i in range(nb_grasp):
pose = pose_set[i]
pt = pt_set[i]
landing_mtx = np.asarray([
pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0],
pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1],
pose[2][2], pt[2]
])
x1.append(v)
x2.append(landing_mtx)
x1, x2 = np.stack(x1), np.stack(x2)
X1 = torch.tensor(x1.reshape((x2.shape[0], 1, 32, 32, 32)),
dtype=torch.float,
device=device)
X2 = torch.tensor(x2.reshape((x2.shape[0], 12)),
dtype=torch.float,
device=device)
yhat = model(X1, X2)
yhat = yhat.detach().cpu().numpy()
scores = np.asarray(yhat)
scores = scores.reshape((nb_grasp, ))
g_index = np.argmax(scores)
print('Highest score: {}, the {}th.'.format(
str(scores[g_index]), str(g_index)))
pose = pose_set[g_index]
pt = centroid
landing_mtx = np.asarray([
pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0],
pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1],
pose[2][2], pt[2]
])
vrep.simxCallScriptFunction(cid, 'landing',
vrep.sim_scripttype_childscript,
'setlanding', [], landing_mtx, [],
emptyBuff, vrep.simx_opmode_blocking)
ending_mtx = [
pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0],
pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1],
pose[2][2], pt[2] + 0.15
]
vrep.simxCallScriptFunction(cid, 'ending',
vrep.sim_scripttype_childscript,
'setending', [], ending_mtx, [],
emptyBuff, vrep.simx_opmode_blocking)
time.sleep(1.0)
print('executing experiment %d: ' % g_index)
print('at: ', pt)
vrep.simxCallScriptFunction(cid, 'Sphere',
vrep.sim_scripttype_childscript,
'grasp', [], [], [], emptyBuff,
vrep.simx_opmode_blocking)
while True:
res, finish = vrep.simxGetIntegerSignal(
cid, "finish", vrep.simx_opmode_oneshot_wait)
if finish == 18:
res, end_pos = vrep.simxGetObjectPosition(
cid, obj_hdl, -1, vrep.simx_opmode_blocking)
break
if end_pos[2] > 0.05:
label = 1
else:
label = 0
print(label)
# f = open(wd + 'label.txt', 'a+')
# f.write(str(label))
# f.close()
eid += 1
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
def push(self,
position,
heightmap_rotation_angle,
push_vertical_offset=0.01,
pushing_point_margin=0.1):
logging.info('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] + push_vertical_offset
# 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
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
try:
num_move_steps = int(np.floor(move_direction[0] / move_step[0]))
except ValueError:
return False
# 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,
self.workspace_limits[0][0]), self.workspace_limits[0][1])
target_y = min(
max(position[1] + push_direction[1] * push_length,
self.workspace_limits[1][0]), self.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)
push_success = True
return push_success
def grasp(
self,
position,
heightmap_rotation_angle,
grasp_vertical_offset=-0.04,
grasp_location_margin=0.15,
):
logging.info('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] + grasp_vertical_offset,
self.workspace_limits[2][0] + 0.02)
# Move gripper to location above grasp target
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
try:
num_move_steps = int(np.floor(move_direction[0] / move_step[0]))
except ValueError:
return False
# 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
if grasp_success:
if self.place_enabled:
self.go_home()
else:
self.num_obj_clear += 1
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]
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
