def __init__(self, clientID):
self.cid = clientID
self.dummybyte = bytearray()
sim_ret, cam_depth_handle = vrep.simxGetObjectHandle(
self.cid, 'kinect_depth', vrep.simx_opmode_blocking)
self.depth_handle = cam_depth_handle
sim_ret, cam_rgb_handle = vrep.simxGetObjectHandle(
self.cid, 'kinect_rgb', vrep.simx_opmode_blocking)
self.rgb_handle = cam_rgb_handle
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.cid, 'kinect_depth', vrep.sim_scripttype_childscript,
'getMatrix', [], [], [], self.dummybyte, vrep.simx_opmode_blocking)
self.depth_m = np.asarray(
[[retFloats[0], retFloats[1], retFloats[2], retFloats[3]],
[retFloats[4], retFloats[5], retFloats[6], retFloats[7]],
[retFloats[8], retFloats[9], retFloats[10], retFloats[11]]])
def place(self,
position,
heightmap_rotation_angle,
place_vertical_offset=0.04):
logging.info('Executing: place at (%f, %f, %f)' %
(position[0], position[1], position[2]))
# Ensure gripper is closed
gripper_fully_closed = self.close_gripper()
if gripper_fully_closed:
# There is no object present, so we cannot possibly place!
return False
# Compute tool orientation from heightmap rotation angle
tool_rotation_angle = (heightmap_rotation_angle % np.pi) - np.pi / 2
# Avoid collision with floor
position[2] = max(position[2] + place_vertical_offset,
self.workspace_limits[2][0] + 0.02)
# Move gripper to location above place target
place_location_margin = 0.1
sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
location_above_place_target = (position[0], position[1],
position[2] + place_location_margin)
self.move_to(location_above_place_target, None)
sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(
self.sim_client, UR5_target_handle, -1, vrep.simx_opmode_blocking)
if tool_rotation_angle - gripper_orientation[1] > 0:
increment = 0.2
else:
increment = -0.2
while abs(tool_rotation_angle - gripper_orientation[1]) >= 0.2:
vrep.simxSetObjectOrientation(
self.sim_client, UR5_target_handle, -1,
(np.pi / 2, gripper_orientation[1] + increment, np.pi / 2),
vrep.simx_opmode_blocking)
time.sleep(0.01)
sim_ret, gripper_orientation = vrep.simxGetObjectOrientation(
self.sim_client, UR5_target_handle, -1,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(
self.sim_client, UR5_target_handle, -1,
(np.pi / 2, tool_rotation_angle, np.pi / 2),
vrep.simx_opmode_blocking)
# Approach place target
self.move_to(position, None)
# Ensure gripper is open
self.open_gripper()
# Move gripper to location above place target
self.move_to(location_above_place_target, None)
return True
def add_structure(cid):
object_name = 'box_large'
res, object_handle = vrep.simxGetObjectHandle(cid, object_name, vrep.simx_opmode_oneshot_wait)
# object_pos = [random.uniform(-0.15, 0.15), random.uniform(-0.15, 0.15), 0.20]
object_pos = [0, 0, 0.075]
object_angle = [-1.5708, random.uniform(-1.5707, 1.5707), -1.5708]
vrep.simxSetObjectOrientation(cid, object_handle, -1, object_angle, vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(cid, object_handle, -1, object_pos, vrep.simx_opmode_oneshot)
time.sleep(1.0)
return object_name, object_handle
def add_object(cid, object_name, object_pos):
# object_name = random.choice(['apple', 'banana', 'sugar_box', 'cracker_box', 'mustard_bottle', 'lemon', 'orange',
# 'tomato_soup_can'])
res, object_handle = vrep.simxGetObjectHandle(cid, object_name, vrep.simx_opmode_oneshot_wait)
# object_pos = [random.uniform(-0.15, 0.15), random.uniform(-0.15, 0.15), 0.20]
object_angle = [random.uniform(-np.pi, np.pi), random.uniform(-np.pi, np.pi), random.uniform(-np.pi, np.pi)]
# object_angle = [0, 0, 0]
vrep.simxSetObjectOrientation(cid, object_handle, -1, object_angle, vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(cid, object_handle, -1, object_pos, vrep.simx_opmode_oneshot)
time.sleep(1.0)
return object_name, object_handle
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 scenario_one(cid):
name_list = []
locations = [[0.15, 0, 0.1], [-0.15, 0, 0.1], [0, 0.15, 0.15],[0, -0.15, 0.1]]
handle_list = []
for i in range(0, 4):
object_name = 'imported_part_' + str(i)
res, object_handle = vrep.simxGetObjectHandle(cid, object_name, vrep.simx_opmode_oneshot_wait)
# object_angle = [random.uniform(-np.pi, np.pi), random.uniform(-np.pi, np.pi), random.uniform(-np.pi, np.pi)]
object_angle = [0, 0, 0]
vrep.simxSetObjectOrientation(cid, object_handle, -1, object_angle, vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(cid, object_handle, -1, locations[i], vrep.simx_opmode_oneshot)
time.sleep(1.0)
name_list.append(object_name)
handle_list.append(object_handle)
return name_list, handle_list
def open_gripper(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)
while gripper_joint_position < 0.03: # Block until gripper is fully open
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, 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 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.045: # Block until gripper is fully closed
sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
# logging.info(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