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 main():
# Attempt to connect to simulator
vrep.simxFinish(-1)
sim_client = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if sim_client == -1:
print 'Failed to connect to simulation. Exiting...'
exit()
else:
print 'Connected to simulation.'
# Create UR5 and restart simulator
gripper = RG2(sim_client)
ur5 = UR5(sim_client, gripper)
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
vrep.simxStartSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
# Move arm to random positon and close/open gripper
target_pose = np.eye(4)
target_pose[:3,-1] = [-0.0, 0.2, 0.2]
ur5.moveTo(target_pose)
ur5.closeGripper()
ur5.openGripper()
# Wait for arm to move the exit
time.sleep(1)
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
exit()
def main():
# Attempt to connect to simulator
vrep.simxFinish(-1)
sim_client = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if sim_client == -1:
print 'Failed to connect to simulation. Exiting...'
exit()
else:
print 'Connected to simulation.'
# Create UR5 and restart simulator
gripper = RG2(sim_client)
ur5 = UR5(sim_client, gripper)
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
vrep.simxStartSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
# Generate a cube
position = [0., 0.5, 0.0]
orientation = [0, 0, np.radians(45)]
color = [255, 0, 0]
cube = utils.generateCube(sim_client, 'cube', position, orientation, color)
time.sleep(2)
# Execute pick on cube
pose = transformations.euler_matrix(np.pi / 2, np.radians(45), np.pi / 2)
pose[:3, -1] = [0, 0.5, 0.0]
offset = 0.2
ur5.pick(pose, offset)
pose = transformations.euler_matrix(np.pi / 2, 0.0, np.pi / 2)
pose[:3, -1] = [0.0, 0.5, 0.5]
ur5.moveTo(pose)
pose = transformations.euler_matrix(np.pi / 2, 0.0, np.pi / 2)
pose[:3, -1] = [0.25, 0.25, 0.0]
ur5.place(pose)
# Wait for arm to move the exit
time.sleep(1)
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
exit()
def __init__(self):
vrep.simxFinish(-1) # just in case, close all opened connections
#self.clientID = vrep.simxStart('10.20.5.229', 19997, True, True, -500000,5) # Connect to V-REP, set a very large time-out for blocking commands
self.clientID = vrep.simxStart(
'127.0.0.1', 19997, True, True, -500000, 5
) # Connect to V-REP, set a very large time-out for blocking commands
if self.clientID != -1:
print('Connected to remote API server')
vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
time.sleep(1)
vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
time.sleep(1)
print('has Start')
self.env_prepare()
else:
print('Failed connecting to remote API server')
def main():
# Attempt to connect to simulator
vrep.simxFinish(-1)
sim_client = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if sim_client == -1:
print 'Failed to connect to simulation. Exiting...'
exit()
else:
print 'Connected to simulation.'
# Create UR5 and sensor and restart simulator
gripper = RG2(sim_client)
ur5 = UR5(sim_client, gripper)
sensor = VisionSensor(sim_client, 'Vision_sensor_orth')
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
vrep.simxStartSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
# Generate a cube
position = [0., 0.5, 0.15]
orientation = [0, 0, 0]
color = [255, 0, 0]
size = [0.1, 0.1, 0.1]
mass = 1
cube = utils.generateCube(sim_client, 'cube', size, position, orientation,
mass, color)
time.sleep(2)
# Get sensor data and display it
data = sensor.getData()
rgb = data['rgb']
depth = data['depth']
plt.imshow(rgb)
plt.show()
plt.imshow(depth)
plt.show()
# Exit
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
exit()
def main():
# Attempt to connect to simulator
sim_client = utils.connectToSimulation('127.0.0.1', 19997)
# Create UR5 and restart simulator
gripper = RG2(sim_client)
ur5 = UR5(sim_client, gripper)
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
vrep.simxStartSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
# Generate a cube
position = [0., 0.5, 0.15]
orientation = [0, 0, 0]
size = [0.05, 0.05, 0.05]
mass = 0.1
color = [255, 0, 0]
cube = utils.generateShape(sim_client, 'cube', 0, size, position,
orientation, mass, color)
time.sleep(2)
# Execute pick on cube
pose = transformations.euler_matrix(np.radians(90), 0, np.radians(90))
pose[:3, -1] = [0, 0.5, 0.0]
offset = 0.2
ur5.pick(pose, offset)
pose = transformations.euler_matrix(np.radians(90), 0, np.radians(90))
pose = transformations.euler_matrix(np.pi / 2, 0.0, np.pi / 2)
pose[:3, -1] = [0.0, 0.5, 0.5]
ur5.moveTo(pose)
pose = transformations.euler_matrix(np.radians(90), 0, np.radians(90))
pose = transformations.euler_matrix(np.pi / 2, 0.0, np.pi / 2)
pose[:3, -1] = [0.25, 0.25, 0.0]
ur5.place(pose, offset)
# Wait for arm to move the exit
time.sleep(1)
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
exit()
def main():
# Attempt to connect to simulator
sim_client = utils.connectToSimulation('127.0.0.1', 19997)
# Create UR5 and sensor and restart simulator
gripper = RG2(sim_client)
ur5 = UR5(sim_client, gripper)
cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0,
1]])
workspace = np.asarray([[-0.25, 0.25], [0.25, 0.75], [0, 0.2]])
sensor = VisionSensor(sim_client, 'Vision_sensor_persp', workspace,
cam_intrinsics)
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
vrep.simxStartSimulation(sim_client, VREP_BLOCKING)
time.sleep(2)
# Generate a cube
position = [0., 0.5, 0.15]
orientation = [0, 0, 0]
size = [0.05, 0.05, 0.05]
mass = 0.1
color = [255, 0, 0]
cube = utils.generateShape(sim_client, 'cube', 0, size, position,
orientation, mass, color)
time.sleep(2)
# Get sensor data and display it
depth_pc, rgb_pc = sensor.getPointCloud()
v = pptk.viewer(depth_pc)
v.attributes(rgb_pc)
depth_heightmap, rgb_heightmap = sensor.getHeightmap()
plt.imshow(depth_heightmap)
plt.show()
plt.imshow(rgb_heightmap)
plt.show()
# Exit
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
exit()
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)
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
#Make list of objects in workspace
cyl_handle = [0] * objectsPresent
cyl_position = [0] * objectsPresent
for j in range(0, objectsPresent):
name = ('Cylinder#%d' % j)
sim_ret, cyl_handle[j] = vrep.simxGetObjectHandle(
robot.sim_client, name, vrep.simx_opmode_blocking)
sim_ret, cyl_position[j] = vrep.simxGetObjectPosition(
robot.sim_client, cyl_handle[j], -1, vrep.simx_opmode_blocking)
for i in range(0, objectsPresent): #execute each grasp for each object
robot.grasp(cyl_position[i], heightmap_rotation_angle, workspace_limits,
towerTip)
robot.move_to(UR5_target_position, None)
towerTip[2] += tipRaise
#End simulation
robot.restart_sim()
vrep.simxStopSimulation(robot.sim_client, vrep.simx_opmode_oneshot_wait)
def stop_sim(self):
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
time.sleep(0.1)
def simrestart(self):
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(1)
print("simulation restart")
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(1)
def restartSimulation(sim_client):
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(sim_client, VREP_BLOCKING)
time.sleep(1)
def stopSimulation(sim_client):
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)