def __init__(self, obj_mesh_dir, workspace_limits, is_testing, ip_vrep = '127.0.0.1'):
self.workspace_limits = workspace_limits
# Read files in object mesh directory
self.obj_mesh_dir = obj_mesh_dir
self.mesh_list = os.listdir(self.obj_mesh_dir)
vrep.simxFinish(-1) # Just in case, close all opened connections
self.sim_client = vrep.simxStart(ip_vrep, 19997, True, True, 5000, 5) # Connect to V-REP on port 19997
if self.sim_client == -1:
print('Failed to connect to simulation (V-REP remote API server). Exiting.')
exit()
else:
print('Connected to simulation.')
self.restart_sim()
self.is_testing = is_testing
# Randomly choose objects to add to scene
self.num_obj = 5
# Define colors for object meshes (Tableau palette)
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.0 #pink
self.obj_mesh_ind = np.random.randint(0, len(self.mesh_list), size=self.num_obj)
self.obj_mesh_color = self.color_space[np.asarray(range(self.num_obj)) % 10, :]
self.add_objects()
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 __init__(self, workspace_limits, is_testing, test_preset_cases,
config_file):
self.workspace_limits = workspace_limits
# If in simulation...
self.obj_mesh_dir = os.path.abspath('./simulation/blocks')
self.mesh_list = [
f for f in os.listdir(self.obj_mesh_dir) if f.endswith('.obj')
]
# Define colors for object meshes (Tableau palette)
self.color_labels = [
'block', 'red', 'orange', 'yellow', 'green', 'blue', 'indigo',
'violet'
]
self.color_space = np.asarray(
[[184, 179, 168], [255, 0, 0], [255, 127, 0], [255, 255, 0],
[0, 255, 0], [0, 0, 255], [38, 0, 51], [148, 0, 211]]) / 255.0
# Make sure to have the server side running in V-REP:
# in a child script of a V-REP scene, add following command
# to be executed just once, at simulation start:
#
# simExtRemoteApiStart(19999)
#
# then start simulation, and run this program.
#
# IMPORTANT: for each successful call to simxStart, there
# should be a corresponding call to simxFinish at the end!
# MODIFY remoteApiConnections.txt
# Connect to simulator
vrep.simxFinish(-1) # Just in case, close all opened connections
self.sim_client = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP on port 19997
if self.sim_client == -1:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
else:
print('Connected to simulation.')
self.restart_sim()
self.is_testing = is_testing
self.test_preset_cases = test_preset_cases
# Setup virtual camera in simulation
self.setup_sim_camera()
# Read config file
self.sim_read_config_file(config_file)
# Add objects to simulation environment
self.objects_reset = False
self.add_objects()
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 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 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 __init__(self,
obj_mesh_dir,
workspace_limits,
is_testing,
ip_vrep='127.0.0.1'):
self.workspace_limits = workspace_limits
# Read files in object mesh directory
self.obj_mesh_dir = obj_mesh_dir
self.mesh_list = os.listdir(self.obj_mesh_dir)
vrep.simxFinish(-1) # Just in case, close all opened connections
self.sim_client = vrep.simxStart(ip_vrep, 19997, True, True, 5000,
5) # Connect to V-REP on port 19997
if self.sim_client == -1:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
else:
print('Connected to simulation.')
self.restart_sim()
# Setup virtual camera in simulation
self.setup_sim_camera()
self.is_testing = is_testing
def __init__(self,
workspace_limits,
num_of_obj,
obj_dir,
is_insert_task=False,
num_of_holes=0,
is_eval=False):
self.workspace_limits = np.asarray(workspace_limits)
self.workspace_center = np.asarray([
(workspace_limits[0][1] + workspace_limits[0][0]) / 2,
(workspace_limits[1][1] + workspace_limits[1][0]) / 2, 0.1
])
if not is_insert_task:
self.obj_target_home_pos = self.workspace_center
else:
self.obj_target_home_pos = np.asarray([
self.workspace_center[0] - 0.1, self.workspace_center[1], 0.08
])
self.obj_target_dir = os.path.join(obj_dir, 'objects')
self.obj_files = [
f for f in listdir(self.obj_target_dir)
if isfile(join(self.obj_target_dir, f)) and ('.ttm' in f)
]
self.obj_files.sort()
self.obj_target_handles = []
if is_insert_task:
self.hole_file_dir = os.path.join(obj_dir, 'holes')
self.hole_files = [
f for f in listdir(self.hole_file_dir)
if isfile(join(self.hole_file_dir, f)) and ('.ttm' in f)
]
self.hole_files.sort()
self.hole_home_pos = np.asarray([
self.workspace_center[0] + 0.15, self.workspace_center[1], 0.08
])
self.hole_handles = []
# These parameters are related to the vrep scenes
self.num_of_obj = num_of_obj
if is_insert_task:
self.num_of_holes = num_of_holes
self.gripper_home_pos = np.asarray([0.4, 0.0, 0.4]) # Gripper home pos
self.gripper_home_ori = np.asarray(
[np.pi, 0., 0.]) # Grasp target Ori (for gripper rotation = 0)
self.is_insert_task = is_insert_task
self.cam_intrinsics = None
self.UR5_target_handle = None
self.UR5_tip_handle = None
self.is_eval = is_eval
self.add_obj_count = 0
# Connect to simulator
vrep.simxFinish(-1) # Just in case, close all opened connections
self.sim_client = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP on port 19997
if self.sim_client == -1:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
else:
print('Connected to simulation.')
self.stop_sim()
self.restart_sim()
# Setup virtual camera in simulation
self.setup_sim_camera()
self.open_RG2_gripper()
self.rotate_gripper_z(0.)
def __init__(self, stage, obj_mesh_dir, num_obj, workspace_limits,
is_testing, test_preset_cases, test_preset_file,
goal_conditioned, grasp_goal_conditioned):
self.workspace_limits = workspace_limits
self.num_obj = num_obj
self.stage = stage
self.goal_conditioned = goal_conditioned
self.grasp_goal_conditioned = grasp_goal_conditioned
# Define colors for object meshes (Tableau palette)
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
], # purpl
[118, 183, 178], # cyan
[255, 157, 167]
]) / 255.0 #pink
# Read files in object mesh directory
self.obj_mesh_dir = obj_mesh_dir
self.mesh_list = os.listdir(self.obj_mesh_dir)
# Randomly choose objects to add to scene
# self.obj_mesh_ind = 5 * np.ones(len(self.mesh_list), dtype=np.int32)
self.obj_mesh_ind = np.random.randint(0,
len(self.mesh_list),
size=self.num_obj)
self.obj_mesh_color = self.color_space[
np.asarray(range(self.num_obj)) % 10, :]
# Connect to simulator
vrep.simxFinish(
-1
) # Just in case, close all opened connections # reason for only one vrep opening?????
self.sim_client = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP on port 19997
if self.sim_client == -1:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
else:
print('Connected to simulation.')
self.restart_sim()
self.is_testing = is_testing
self.test_preset_cases = test_preset_cases
self.test_preset_file = test_preset_file
# Setup virtual camera in simulation
self.setup_sim_camera()
# If testing, read object meshes and poses from test case file
if self.is_testing and self.test_preset_cases:
file = open(self.test_preset_file, 'r')
file_content = file.readlines()
self.test_obj_mesh_files = []
self.test_obj_mesh_colors = []
self.test_obj_positions = []
self.test_obj_orientations = []
for object_idx in range(self.num_obj):
file_content_curr_object = file_content[object_idx].split()
self.test_obj_mesh_files.append(
os.path.join(self.obj_mesh_dir,
file_content_curr_object[0]))
self.test_obj_mesh_colors.append([
float(file_content_curr_object[1]),
float(file_content_curr_object[2]),
float(file_content_curr_object[3])
])
self.test_obj_positions.append([
float(file_content_curr_object[4]),
float(file_content_curr_object[5]),
float(file_content_curr_object[6])
])
self.test_obj_orientations.append([
float(file_content_curr_object[7]),
float(file_content_curr_object[8]),
float(file_content_curr_object[9])
])
file.close()
self.obj_mesh_color = np.asarray(self.test_obj_mesh_colors)
# Add objects to simulation environment
self.add_objects()
def disconnectToSimulation(sim_client):
vrep.simxFinish(sim_client)
import numpy as np
import scipy as sp
from numpy import linalg as nl
from scipy import linalg as sl
import math
from math import factorial
from simulation import vrep
import time
import random
import copy
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
## define Baxter's joints
# define the body joints
bodyJoints = np.zeros(3)
bodyError = np.zeros(3)
vertError, vertJoint = vrep.simxGetObjectHandle(clientID,
'Baxter_verticalJoint',
vrep.simx_opmode_blocking)
rotError, rotJoint = vrep.simxGetObjectHandle(clientID, 'Baxter_rotationJoint',
vrep.simx_opmode_blocking)
monitorError, monitorJoint = vrep.simxGetObjectHandle(
clientID, 'Baxter_monitorJoint', vrep.simx_opmode_blocking)
block0err, block0 = vrep.simxGetObjectHandle(clientID, "cube0",
vrep.simx_opmode_blocking)