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)
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)
dummyRot, dummy1R, dummy2R, dummy3R, dummy4R, dummy5R, dummy6R, dummy7R,
dummyEndR
])
#arm joint angle limits
rightLimits = [(math.radians(-168), math.radians(-168 + 336)),
(math.radians(-56), math.radians(-56 + 152.5)),
(math.radians(-115), math.radians(-115 + 175)),
(math.radians(-173), math.radians(-173 + 346)),
(math.radians(0), math.radians(0 + 148)),
(math.radians(-173.3), math.radians(-173.3 + 346.5)),
(math.radians(-88), math.radians(-88 + 206)),
(math.radians(-173.3), math.radians(-173.3 + 346.5))]
LeftLimits = [(math.radians(-168), math.radians(-168 + 336)),
(math.radians(-97.5), math.radians(-97.5 + 152.5)),
(math.radians(-115), math.radians(-115 + 175)),
(math.radians(-173), math.radians(-173 + 346)),
(math.radians(0), math.radians(0 + 148)),
(math.radians(-173.3), math.radians(-173.3 + 346.5)),
(math.radians(-88), math.radians(-88 + 206)),
(math.radians(-173.3), math.radians(-173.3 + 346.5))]
(math.radians(-97.5), math.radians(-97.5 + 152.5))
#outsdie obstacle properties
p_obstacle = np.array([[0, 0, 0], [0, 0, 0], [1.6, 1.25, .4]])
r_obstacle = np.array([[.2, .15, .2]])
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
cubeCenters = np.array([[.7, .7, .7], [0, -.3, .3], [.715, .715, .715]])
