def restart(self,earlyStop = False):
if (self.cid != None):
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(1)
self.connect()
time.sleep(1)
vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode())
if earlyStop:
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
return
vrep.simxStartSimulation(self.cid, self.mode())
self.runtime = 0
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait)
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait)
err, self.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)
err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming)
self.getTargetStart()
for i in range(4):
self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid,
'Quadricopter_propeller_respondable' + str(i) + str(1),
self.mode())
def __init__(self):
self._pid = [PID(),PID(),PID(),PID(),PID(),PID(),PID(),PID(),PID()]
self._linearVelocity = [0,0,0,0,0,0,0,0,0]
self._theta = [0,0,0,0,0,0,0,0,0]
self._positions =[]
self._convertdeg2rad = 57.295779578552
self._num_steps = 0
self._timestep = 0.05
self._arm_joints = [0,0,0,0,0,0,0,0,0]
self._cube = 0
self._simulator = "VRep"
self._physics_engine = ""
self._experiment = ""
self._current_iteration = 0
vrep.simxFinish(-1) # just in case, close all opened connections
self._clientID=vrep.simxStart('127.0.0.1',19997,True,False,5000,5) # Connect to V-REP
vrep.simxLoadScene(self._clientID, "kinova_description/urdf/m1n6s300.ttt", 1, vrep.simx_opmode_blocking)
errorCode,self._arm_joints[0] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_1', vrep.simx_opmode_blocking)
errorCode,self._arm_joints[1] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_2', vrep.simx_opmode_blocking)
errorCode,self._arm_joints[2] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_3', vrep.simx_opmode_blocking)
errorCode,self._arm_joints[3] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_4', vrep.simx_opmode_blocking)
errorCode,self._arm_joints[4] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_5', vrep.simx_opmode_blocking)
errorCode,self._arm_joints[5] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_6', vrep.simx_opmode_blocking)
errorCode,self._arm_joints[6] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_finger_1', vrep.simx_opmode_blocking)
errorCode,self._arm_joints[7] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_finger_2', vrep.simx_opmode_blocking)
errorCode,self._arm_joints[8] = vrep.simxGetObjectHandle(self._clientID, 'm1n6s300_joint_finger_3', vrep.simx_opmode_blocking)
errorCode,self._cube = vrep.simxGetObjectHandle(self._clientID, 'Cuboid', vrep.simx_opmode_blocking)
def _configure_environment(self, scene_path):
"""
Provides a chance for subclasses to override this method and supply
a different server configuration. By default, we initialize one
offense agent against no defenders.
"""
vrep.simxLoadScene(self.clientID, scene_path, 1,vrep.simx_opmode_blocking)
self._turn_display(False)
def load_scene(self, path):
"""
Load the vrep scene in the specified path
"""
if self.scene_loaded:
raise RuntimeError('Scene is loaded already.')
vrep.simxLoadScene(self.client_id, path, 0, self.modes['blocking'])
self.scene_loaded = True
def reset(self):
super(StandingUpEnvironment, self).reset()
# print('*** Reset ***')
vrep.simxStopSimulation(self.client_id, self.opmode)
vrep.simxCloseScene(self.client_id, self.opmode)
vrep.simxLoadScene(self.client_id, self.model_path, 0, self.opmode)
vrep.simxSynchronous(self.client_id, True)
vrep.simxStartSimulation(self.client_id, self.opmode)
self.bioloid = Bioloid(self.client_id)
def __init__(self):
# ROS Publishers
fin = rospy.Publisher('/finished', Int8, queue_size=1)
report_sim = rospy.Publisher('/simulation', String, queue_size=1)
starting = rospy.Publisher('/starting', Int16, queue_size=1)
# ROS Subscriber
rospy.Subscriber("/restart", Int8, self.receiveStatus, queue_size=1)
# V-REP Client Start
vrep.simxFinish(-1) # clean up the previous stuff
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
print("Could not connect to server")
sys.exit()
# EPISODE TRACKING
counter = 0
while (counter < episodes):
print("Episode Number ", counter + 1)
time.sleep(3)
# Choose simulation randomly from list
simulation_index = np.random.choice(range(len(VREP_SCENES)))
sim_name, sim_path = VREP_SCENES[simulation_index]
msg = String()
msg.data = sim_name
report_sim.publish(msg)
vrep.simxLoadScene(clientID, sim_path, 0, vrep.simx_opmode_blocking)
time.sleep(2)
# Start simulation
r = 1
while r != 0:
r = vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
msg = Int16()
msg.data = counter + 1
starting.publish(msg)
self.restart = False
# Wait for signal to stop
while not self.restart:
x = 1 + 1
# Stop and restart simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
counter += 1
# Finished all episodes
time.sleep(2)
msg = Int8()
msg.data = 1
fin.publish(msg)
def reset(self):
# first load up the original scene
scene_did_load = vrep.simxLoadScene(self._client_id, self._scene_file,
0, vrep.simx_opmode_blocking)
if scene_did_load != vrep.simx_return_ok:
raise SimulatorException('Could not load scene')
self._goal_point = np.array([0.0, 0.0, 0.0])
self._current_action_step = 0
self._current_sensor_step = None
# Tuple of joint handle and current position
self._joint_positions = [(None, 0)] * len(JOINTS)
self._distance_vector_to_goal = np.array([0, 0, 0])
self._proximity_sensor_distances = [sys.maxint
] * len(PROXIMITY_SENSORS)
self._is_colliding = False
self._sensor_data_vector = [0] * Environment.outdim
# get collision handles, joint handles, etc.
self._scene_handles = self._load_scene_handles()
self._generate_goal_position()
vrep.simxStartSimulation(self._client_id, vrep.simx_opmode_blocking)
time.sleep(0.1)
self._start_streaming()
# sleep after streaming starts for data to roll in
time.sleep(1)
# We want to get sensors once so that things like _distance_from_goal are properly initialized
self.getSensors()
def load_scene(self, scene_path):
if self.scene_loaded:
raise RuntimeError('Scene is already loaded.')
self.RAPI_rc(
vrep.simxLoadScene(self.cID, scene_path, 0,
vrep.simx_opmode_blocking))
self.scene_loaded = True
def loadScene(self,
path="/home/rik/sudo/ros/catkin_ws/src/vrep_test/scenes/",
scene="testScene.ttt"):
returnCode = vrep.simxLoadScene(self.clientID, path + scene, 0,
vrep.simx_opmode_oneshot_wait)
if returnCode != 0:
print 'Could not load scene', returnCode
else:
print 'Scene loaded'
# also get the handles for the joints
for j in range(1, 7):
returnCode, handle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(j),
vrep.simx_opmode_oneshot_wait)
if returnCode == 0:
self.jointHandles.append(handle)
else:
print 'Could not get jointHandle', returnCode
# get the handle of the dummy position
returnCode, self.gripperHandle = vrep.simxGetObjectHandle(
self.clientID, "EEF", vrep.simx_opmode_oneshot_wait)
if returnCode == 0:
pass
else:
print 'Could not get EEFhandle', returnCode
returnCode, self.frameHandle = vrep.simxGetObjectHandle(
self.clientID, "Frame", vrep.simx_opmode_oneshot_wait)
if returnCode == 0:
pass
else:
print 'Could not get frameHandle', returnCode
def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
self.scene = './simu.ttt'
self.gain = 2
self.initial_position = [3,3,to_rad(45)]
self.r = 0.096 # wheel radius
self.R = 0.267 # demi-distance entre les r
print('New pioneer simulation started')
vrep.simxFinish(-1)
self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5)
if self.client_id!=-1:
print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
self.set_position(self.initial_position)
vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
else:
print('Unable to connect to %s:%s' % (self.ip, self.port))
def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
self.scene = './simu.ttt'
self.gain = 2
self.initial_position = [3,3,to_rad(45)]
self.r = 0.096 # wheel radius
self.R = 0.267 # demi-distance entre les r
print('New pioneer simulation started')
vrep.simxFinish(-1)
self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5)
if self.client_id!=-1:
print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
self.set_position(self.initial_position)
vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
else:
print('Unable to connect to %s:%s' % (self.ip, self.port))
def __init__(self, seed, rank, ep_len=64, headless=True):
super().__init__(rank, headless)
self.target_norm = self.normalise_target()
self.np_random = np.random.RandomState()
self.np_random.seed(seed + rank)
self.ep_len = ep_len
return_code = vrep.simxSynchronous(self.cid, enable=True)
check_for_errors(return_code)
return_code = vrep.simxLoadScene(self.cid, scene_path, 0, vrep.simx_opmode_blocking)
check_for_errors(return_code)
# Get the initial configuration of the robot (needed to later reset the robot's pose)
self.init_config_tree, _, _, _ = self.call_lua_function('get_configuration_tree')
_, self.init_joint_angles, _, _ = self.call_lua_function('get_joint_angles')
for i in range(7):
return_code, handle = vrep.simxGetObjectHandle(self.cid, 'Sawyer_joint' + str(i + 1),
vrep.simx_opmode_blocking)
check_for_errors(return_code)
self.joint_handles[i] = handle
return_code, self.end_handle = vrep.simxGetObjectHandle(self.cid,
"BaxterGripper_centerJoint", vrep.simx_opmode_blocking)
check_for_errors(return_code)
_, self.target_handle = vrep.simxGetObjectHandle(self.cid,
"Cube", vrep.simx_opmode_blocking)
# Start the simulation (the "Play" button in V-Rep should now be in a "Pressed" state)
return_code = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_blocking)
check_for_errors(return_code)
def initiate():
global __clientID
global lidars
global rotor
global inst
global steer_handle
port_num = 19999
path_vrep = 'C:\Program Files\V-REP3\V-REP_PRO_EDU\\vrep'
args = [path_vrep, '-gREMOTEAPISERVERSERVICE_' + str(port_num) + '_FALSE_TRUE']
try:
inst = sp.Popen(args)
except EnvironmentError:
print('(instance) Error: cannot find executable at', args[0])
raise
vrep.simxFinish(-1)
retries = 0
while True:
print('(vrepper)trying to connect to server on port', port_num, 'retry:', retries)
# vrep.simxFinish(-1) # just in case, close all opened connections
__clientID = vrep.simxStart(
'127.0.0.1', port_num,
waitUntilConnected=True,
doNotReconnectOnceDisconnected=True,
timeOutInMs=1000,
commThreadCycleInMs=0) # Connect to V-REP
if __clientID != -1:
print('(vrepper)Connected to remote API server!')
break
else:
retries += 1
if retries > 15:
vrep.end()
raise RuntimeError('(vrepper)Unable to connect to V-REP after 15 retries.')
vrep.simxLoadScene(__clientID, 'C:\Program Files\V-REP3\V-REP_PRO_EDU\scenes\\not_pristine.ttt',
0, # assume file is at server side
vrep.simx_opmode_blocking)
# vrep.simxFinish(-1)
# __clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to V-REP
vrep.simxSynchronous(__clientID, True)
lidars = ['TriangleA', 'TriangleB', 'TriangleC']
lidars = [getHandle(x) for x in lidars]
rotor = getHandle('motor_joint')
steer_handle = getHandle('steer_joint')
def __init__(self,
seed,
rank,
initial_policy=None,
ep_len=64,
headless=True):
super().__init__(rank, headless)
self.target_pos = np.array([0.3, -0.5, 0.025]) # TODO: Obtain
self.waypoint_pos = np.array([0, -0.5, 0.45]) # TODO: Obtain
self.target_norm = normalise_coords(self.target_pos)
self.np_random = np.random.RandomState()
self.np_random.seed(seed + rank)
self.ep_len = ep_len
self.initial_policy = initial_policy
return_code = vrep.simxSynchronous(self.cid, enable=True)
check_for_errors(return_code)
return_code = vrep.simxLoadScene(self.cid, scene_path, 0,
vrep.simx_opmode_blocking)
check_for_errors(return_code)
# Get the initial configuration of the robot (needed to later reset the robot's pose)
self.init_config_tree, _, _, _ = self.call_lua_function(
'get_configuration_tree')
_, self.init_joint_angles, _, _ = self.call_lua_function(
'get_joint_angles')
self.joint_angles = self.init_joint_angles
self.joint_handles = np.array([None] * len(self.joint_angles))
for i in range(7):
return_code, handle = vrep.simxGetObjectHandle(
self.cid, 'Sawyer_joint' + str(i + 1),
vrep.simx_opmode_blocking)
check_for_errors(return_code)
self.joint_handles[i] = handle
return_code, self.end_handle = vrep.simxGetObjectHandle(
self.cid, "BaxterGripper_centerJoint", vrep.simx_opmode_blocking)
check_for_errors(return_code)
self.end_pose = self.get_end_pose()
_, self.target_handle = vrep.simxGetObjectHandle(
self.cid, "Cube", vrep.simx_opmode_blocking)
_, self.wall_handle = vrep.simxGetObjectHandle(
self.cid, "Wall", vrep.simx_opmode_blocking)
self.init_wall_pos = vrep.simxGetObjectPosition(
self.cid, self.wall_handle, -1, vrep.simx_opmode_blocking)[1]
self.wall_distance = normalise_coords(self.init_wall_pos[0],
lower=0,
upper=1)
self.init_wall_rot = vrep.simxGetObjectOrientation(
self.cid, self.wall_handle, -1, vrep.simx_opmode_blocking)[1]
self.wall_orientation = self.init_wall_rot
# Start the simulation (the "Play" button in V-Rep should now be in a "Pressed" state)
return_code = vrep.simxStartSimulation(self.cid,
vrep.simx_opmode_blocking)
check_for_errors(return_code)
def loadVREPScene(clientID, path_to_scene):
ret = vrep.simxLoadScene(clientID,
path_to_scene,
False,
blocking)
if ret != return_ok:
raise Exception('FAIL: Scene failed to load')
def getRobotPosition(nsecs):
''' get the position of the robot after nsecs seconds '''
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
vrep.simxLoadScene(clientID, './EvolMor.ttt', 0,
vrep.simx_opmode_blocking)
print('Connected to remote API server')
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
retCodeHandle, robotHandle = vrep.simxGetObjectHandle(
clientID, 'robot_shape', vrep.simx_opmode_blocking)
checkDataFetch(retCodeHandle, robotHandle, 'robot handle')
# time.sleep(nsecs)
try:
# give the robot time to move
time.sleep(nsecs)
except KeyboardInterrupt:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
retCodePos, objectPos = vrep.simxGetObjectPosition(
clientID, robotHandle, -1, vrep.simx_opmode_blocking)
checkDataFetch(retCodePos, objectPos, 'position')
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
return objectPos[0]
def start_vrep(self):
self.client_id = vrep.simxStart(self.host, self.port_num, True, True, 5000, 5)
return_code = vrep.simxSynchronous(self.client_id, enable=True)
self.check_for_errors(return_code)
dir_path = os.path.dirname(os.path.realpath(__file__))
scene_path = dir_path + '/my_scene3.ttt'
return_code = vrep.simxLoadScene(self.client_id, scene_path, 0, vrep.simx_opmode_blocking)
self.check_for_errors(return_code)
return_code = vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
self.check_for_errors(return_code)
def __init__(self):
print('Program started')
self.models_path = os.getcwd() + "/../models/"
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
vrep.simxSynchronous(self.clientID, True)
self.youbot_cmd = rospy.Publisher('cmd_vel', Twist, queue_size=100)
if self.clientID != -1:
print('Connected to remote API server')
vrep.simxLoadScene(self.clientID,
os.getcwd() + '/../scene/remote api.ttt', 0xFF,
vrep.simx_opmode_blocking)
self.tables = []
self.robot = Robot(self.clientID, self.models_path)
self.reset = True
self.planner = Planner()
else:
print('Failed connecting to remote API server')
print('Program ended')
def load_scene(self, filename, server_side=True):
"""Load scene from file."""
SERVER_SIDE = 0x00
CLIENT_SIDE = 0x01
if self._client_id is None:
raise ConnectionError(
"Could not load scene from file {}: not connected to V-REP "
"remote API server.".format(filename))
side = SERVER_SIDE if server_side else CLIENT_SIDE
res = vrep.simxLoadScene(self._client_id, filename, side,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not load scene from file {}."
"".format(filename))
def restart(self, earlyStop=False):
if (self.cid != None):
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(1)
self.connect()
time.sleep(1)
vrep.simxLoadScene(
self.cid,
'/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt',
0, self.mode())
if earlyStop:
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
return
vrep.simxStartSimulation(self.cid, self.mode())
self.runtime = 0
err, self.copter = vrep.simxGetObjectHandle(
self.cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait)
err, self.target = vrep.simxGetObjectHandle(
self.cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait)
err, self.front_camera = vrep.simxGetObjectHandle(
self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)
err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter,
vrep.simx_opmode_streaming)
self.getTargetStart()
for i in range(4):
self.propellerScripts[i] = vrep.simxGetObjectHandle(
self.cid,
'Quadricopter_propeller_respondable' + str(i) + str(1),
self.mode())
def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
self.scene = './simu.ttt'
self.gain = 2
self.initial_position = [1, 1, to_rad(45)]
self.r = 0.096 # wheel radius
self.R = 0.267 # demi-distance entre les r
self.activated_sensors = [1, 4, 5, 8, 9, 12, 13, 16]
#print('New pioneer simulation started')
vrep.simxFinish(-1)
self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000,
5)
if self.client_id != -1:
#print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
res = vrep.simxLoadScene(self.client_id, self.scene, 1,
vrep.simx_opmode_oneshot_wait)
res, self.pioneer = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
res, self.left_motor = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_oneshot_wait)
res, self.right_motor = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_oneshot_wait)
self.proximity_sensors = [
0 for i in range(len(self.activated_sensors))
]
for i in range(len(self.activated_sensors)):
res, self.proximity_sensors[i] = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor' +
str(self.activated_sensors[i]),
vrep.simx_opmode_oneshot_wait)
else:
print('Unable to connect to %s:%s' % (self.ip, self.port))
exit()
self.set_position2(self.initial_position)
#self.set_position(self.initial_position)
#vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
firt_load_of_sensors = self.load_proximity_sensors()
def connectToVREP(self, VREP_World=None):
"Connect to VREP and load the correct world if needed"
"FIXME: SHOULD LAUNCH VREP IF NOT RUNNING"
VREP_exec = 'vrep'
if self.VREPSimulation == None:
self.VREPSimulation = VREP_World
'1. check that V-Rep is running and see whether we are already connected to it. Otherwise connect'
if VREP_exec not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]):
raise Exception(("V-REP is not running! Please start V-REP with scene: %s" % self.VREPSimulation))
else:
"Check if we are connected with the passed clientId already"
if self._VREP_clientId is not None:
print "ClientId = " ,self._VREP_clientId
connId = vrep.simxGetConnectionId(self._VREP_clientId)
print "My connId is " , connId
if connId == -1: # we are not: set client Id to none and re-connect
print "Disconnecting all existing connections to V-REP"
vrep.simxFinish(-1)
self._VREP_clientId = None
while self._VREP_clientId is None:
self._VREP_clientId = vrep.simxStart(self._host, self._kheperaPort,True,True, 5000,5)
if not self._VREP_clientId == -1:
eCode = vrep.simxSynchronous(self._VREP_clientId, True)
if eCode != 0:
raise Exception("Failed to connect to VREP simulation. Bailing out")
# print " we are connected with clientId ", self._VREP_clientId
"2. Check the correct world is running"
if self.VREPSimulation is not None:
VREP_Scene = split(self.VREPSimulation)[1]
if VREP_Scene not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]):
eCode = vrep.simxLoadScene(self._VREP_clientId, self.VREPSimulation, 0, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception(("Could not load into V-REP the world", self.VREPSimulation))
"3. Make sure VREP has bees set to the correct directory for saving data"
self.setDataDir(self.dataDir)
'4. Start simulation'
eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("VREP simulation cannot get started")
else:
print "V-REP simulation is running with clientId: ", self._VREP_clientId
return self._VREP_clientId
def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
self.scene = './fred.ttt'
self.gain = [1, 15]
self.initial_position = [-5, 0, to_rad(0)]
self.r = 0.096 # wheel radius
self.R = 0.267 # demi-distance entre les r
print('New FRED simulation started')
vrep.simxFinish(-1)
self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000,
5)
if self.client_id != -1:
print('Connected to remote API server on %s:%s' %
(self.ip, self.port))
res = vrep.simxLoadScene(self.client_id, self.scene, 1,
vrep.simx_opmode_oneshot_wait)
res, self.fred = vrep.simxGetObjectHandle(
self.client_id, 'Fred', vrep.simx_opmode_oneshot_wait)
res, self.tourelle = vrep.simxGetObjectHandle(
self.client_id, 'Plateau', vrep.simx_opmode_oneshot_wait)
res, self.back = vrep.simxGetObjectHandle(
self.client_id, 'Bras_arrire', vrep.simx_opmode_oneshot_wait)
res, self.direction_motor = vrep.simxGetObjectHandle(
self.client_id, 'Direction', vrep.simx_opmode_oneshot_wait)
res, self.traction_motor = vrep.simxGetObjectHandle(
self.client_id, 'Traction', vrep.simx_opmode_oneshot_wait)
self.set_position(self.initial_position)
vrep.simxStartSimulation(self.client_id,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(self.client_id,
self.traction_motor, 0,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(self.client_id,
self.direction_motor, 0,
vrep.simx_opmode_oneshot_wait)
else:
print('Unable to connect to %s:%s' % (self.ip, self.port))
def Connect_vrep(ip = '127.0.0.1', port = 19997, path = vrep_scenes_path):
'''输入vrep地址和端口,连接vrep并返回可用的clientID'''
print('程序开始,准备连接V-rep')
scene_path = path + 'car_cross_sensor.ttt'
vrep.simxFinish(-1)
while True:
cID = vrep.simxStart(ip, port, True, True, 5000, 5)
if cID > -1:
print('连接成功')
err = vrep.simxLoadScene(cID, scene_path, 0, vrep.simx_opmode_blocking)
if err == 0:
print('场景加载完成')
else:
print('场景加载失败')
return cID
else:
time.sleep(0.50)
print('连接失败,尝试重新连接')
def init_vrep_connection(self):
vrep.simxFinish(-1) # just in case, close all opened connections
self.client_id=vrep.simxStart(self.ip,self.port,True,True,5000,5) # Connect to V-REP
if self.client_id!=-1:
print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
#get motor handler
self.motor_handlers = []
for motor in self.robot.motors:
res, motor_handler = vrep.simxGetObjectHandle(
self.client_id,
motor.name,
vrep.simx_opmode_oneshot_wait
)
self.motor_handlers.append(motor_handler)
self.motor_handlers.sort()
print(self.motor_handlers)
#get effector handler
res, self.effector_handler = vrep.simxGetObjectHandle(
self.client_id,
'effector',
vrep.simx_opmode_oneshot_wait
)
#get collision handler
self.collision_handlers = []
for collision in self.collision_list:
res, collision_handler = vrep.simxGetObjectHandle(
self.client_id,
collision,
vrep.simx_opmode_oneshot_wait
)
self.collision_handlers.append(collision_handler)
vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)
def __init__(self, scene_name, rank, headless):
# Launch a V-Rep server
# Read more here: http://www.coppeliarobotics.com/helpFiles/en/commandLine.htm
port_num = base_port_num + rank
remote_api_string = '-gREMOTEAPISERVERSERVICE_' + str(port_num) + '_FALSE_TRUE'
args = [*xvfb_args, vrep_path, '-h' if headless else '', remote_api_string]
self.cid = -1
while self.cid == -1:
self.process = Popen(args, preexec_fn=os.setsid, stdout=DEVNULL)
time.sleep(6)
self.cid = vrep.simxStart(host, port_num, True, True, 5000, 5)
if self.cid == -1:
print(f"{rank} failed to connect to V-REP. Retrying...")
try:
pgrp = os.getpgid(self.process.pid)
os.killpg(pgrp, signal.SIGKILL)
except ProcessLookupError:
pass
catch_errors(vrep.simxSynchronous(self.cid, enable=True))
scene_path = os.path.join(scene_dir_path, f'{scene_name}.ttt')
catch_errors(vrep.simxLoadScene(self.cid, scene_path, 0, vrep.simx_opmode_blocking))
atexit.register(self.close)
def loadscn(self, clientID, model):
error=vrep.simxLoadScene(clientID, model, 2, vrep.simx_opmode_oneshot_wait)
return error
def load_scene(self, scene):
"""Load existing scene by it's name"""
return vrep.simxLoadScene(self.client_id, SCENES_DIR + scene, 0xFF,
BLOCKING_MODE)
nino_ttt=cwd+"/nino.ttt"
print(nino_ttt)
#
# cmd_line = "sudo ./vrep/vrep.sh"
# p = subprocess.Popen(cmd_line, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
# out = p.communicate()[0]
# print out
# time.sleep(1)
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
res = vrep.simxLoadScene(clientID, nino_ttt, 0xFF, vrep.simx_opmode_oneshot)
returnCode=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
print("connection successful!!")
#time.sleep(2)
while True:
try:
pass
except KeyboardInterrupt:
returnCode=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot)
raise
else:
print("connection failed!!")
sys.exit()
def load_scene(self, scene_name):
vrep.simxLoadScene(self.clientID, scene_name + ".ttt", 0xFF,
vrep.simx_opmode_blocking)
return
def reset(self):
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(0.1)
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(self.clientID, True)
vrep.simxSetFloatingParameter(
self.clientID,
vrep.sim_floatparam_simulation_time_step,
self.dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF,
vrep.simx_opmode_blocking)
# Get quadrotor handle
err, self.quad_handle = vrep.simxGetObjectHandle(
self.clientID, self.quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(self.clientID, self.quad_handle,
vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(self.propellers)):
vrep.simxClearFloatSignal(self.clientID, self.propellers[i],
vrep.simx_opmode_oneshot)
# Get quadrotor initial position and orientation
err, self.pos_start = vrep.simxGetObjectPosition(
self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
err, self.euler_start = vrep.simxGetObjectOrientation(
self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
err, self.vel_start, self.angvel_start = vrep.simxGetObjectVelocity(
self.clientID, self.quad_handle, vrep.simx_opmode_buffer)
self.pos_start = np.asarray(self.pos_start)
self.euler_start = np.asarray(self.euler_start) * 10.
self.vel_start = np.asarray(self.vel_start)
self.angvel_start = np.asarray(self.angvel_start)
self.pos_old = self.pos_start
self.euler_old = self.euler_start
self.vel_old = self.vel_start
self.angvel_old = self.angvel_start
self.pos_new = self.pos_old
self.euler_new = self.euler_old
self.vel_new = self.vel_old
self.angvel_new = self.angvel_old
self.pos_error = (self.pos_start + self.setpoint_delta[0:3]) \
- self.pos_new
self.euler_error = (self.euler_start + self.setpoint_delta[3:6]) \
- self.euler_new
self.vel_error = (self.vel_start + self.setpoint_delta[6:9]) \
- self.vel_new
self.angvel_error = (self.angvel_start + self.setpoint_delta[9:12]) \
- self.angvel_new
self.pos_error_all = self.pos_error
self.euler_error_all = self.euler_error
self.init_f = 5.8
self.propeller_vels = [
self.init_f, self.init_f, self.init_f, self.init_f
]
self.timestep = 1
return self.get_state()
def __init__(self, episode_len=None):
self.clientID = None
# Start V-REP connection
try:
vrep.simxFinish(-1)
print("Connecting to simulator...")
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
5000, 5)
if self.clientID == -1:
print("Failed to connect to remote API Server")
self.vrep_exit()
except KeyboardInterrupt:
self.vrep_exit()
return
self.episode_len = episode_len
self.timestep = 0
self.dt = .001
self.propellers = [
'rotor1thrust', 'rotor2thrust', 'rotor3thrust', 'rotor4thrust'
]
self.quad_name = 'Quadricopter'
self.scene_name = 'quad_innerloop.ttt'
self.setpoint_delta = np.array(
[0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.])
self.quad_handle = None
self.pos_start = np.zeros(3)
self.euler_start = np.zeros(3)
self.vel_start = np.zeros(3)
self.angvel_start = np.zeros(3)
self.pos_old = self.pos_start
self.euler_old = self.euler_start
self.vel_old = self.vel_start
self.angvel_old = self.angvel_start
self.pos_new = self.pos_old
self.euler_new = self.euler_old
self.vel_new = self.vel_old
self.angvel_new = self.angvel_old
ob_high = np.array([
10., 10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21., 10.,
10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21.
])
ob_low = -ob_high
self.observation_space = Box(low=ob_low,
high=ob_high,
dtype=np.float32)
ac_high = np.array([20., 20., 20., 20.])
ac_low = np.zeros(4)
self.action_space = Box(low=ac_low, high=ac_high, dtype=np.float32)
print("Setting simulator to async mode...")
vrep.simxSynchronous(self.clientID, True)
vrep.simxSetFloatingParameter(
self.clientID,
vrep.sim_floatparam_simulation_time_step,
self.dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF,
vrep.simx_opmode_blocking)
# Get quadrotor handle
err, self.quad_handle = vrep.simxGetObjectHandle(
self.clientID, self.quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(self.clientID, self.quad_handle,
vrep.simx_opmode_streaming)
#and then three rings at centred .25d, .5d, .75d with radius = 2
k = functions.keys()
k.sort()
iter = 1
for key in k:
state_file = open('Trajectories/{}.txt'.format(key+"state"),'w+')
action_file = open('Trajectories/{}.txt'.format(key+"action"),'w+')
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(1)
cid = connect()
time.sleep(1)
vrep.simxLoadScene(cid,'/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt',0,mode())
vrep.simxStartSimulation(cid,mode())
runtime = 0
err, copter = vrep.simxGetObjectHandle(cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait)
err, target = vrep.simxGetObjectHandle(cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait)
err, front_camera = vrep.simxGetObjectHandle(cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)
err, lin, ang = vrep.simxGetObjectVelocity(cid, copter, vrep.simx_opmode_streaming)
err, orgpos = getTargetStart(cid,target)
real_args(args,orgpos )
def loadscn(self, clientID, model):
time1 = time()
error = vrep.simxLoadScene(clientID, model, 2, vrep.simx_opmode_oneshot_wait)
time2 = time()
print "LOGGING time for loading VREP scene in seconds: ", time2 - time1
return error
R = 0.267 # demi-distance entre les r
# neural network config
ni = 3 # number of input nodes (without bias)
nj = 10 # number of hidden nodes
nk = 2 # number of output nodes
alpha = [1/4,1/4,1/(math.pi)]
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
client_id=vrep.simxStart(ip,port,True,True,5000,5) # Connect to V-REP
if client_id!=-1:
print ('Connected to remote API server on %s:%s' % (ip, port))
res = vrep.simxLoadScene(client_id, scene, 1, vrep.simx_opmode_oneshot_wait)
res, pioneer = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
res, left_motor = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
res, right_motor = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
# set initial position, -1 is absolute
vrep.simxSetObjectPosition(client_id, pioneer, -1, [position_init[0], position_init[1], 0.13879], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(client_id, pioneer, -1, [0, 0, to_deg(position_init[2])], vrep.simx_opmode_oneshot_wait)
network = NN(ni,nj,nk)
vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait)
position = position_init # [x,y,theta]
network_input = [0, 0, 0]
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID == -1:
print("Failed to connect to remote API Server")
vrep_exit(clientID)
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .001
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
except KeyboardInterrupt:
vrep_exit(clientID)
err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
def reset():
vrepProcess = subprocess.Popen(vrep_cmd, shell=False, preexec_fn=os.setsid)
# connect to V-Rep Remote Api Server
vrep.simxFinish(-1) # close all opened connections
clientID = vrep.simxStart('127.0.0.1', portNb, True, False, 5000,
5) # Connect to V-REP
if clientID == -1:
print('Failed connecting to remote API server')
else:
print('Connected to remote API server')
# load scene
time.sleep(5) # to avoid errors
scenePath = 'MicoRobotTest.ttt'
returnCode = vrep.simxLoadScene(
clientID, scenePath, 1, vrep.simx_opmode_oneshot_wait
) # vrep.simx_opmode_blocking is recommended
printlog('simxLoadScene', returnCode)
# when using child script
# if len(sys.argv) >= 10:
# portNb = int(sys.argv[1])
# jointHandles = list(map(int,sys.argv[2:8]))
# fingersH1 = int(sys.argv[8])
# fingersH2 = int(sys.argv[9])
# else:
# print("Indicate following arguments: 'portNumber jointHandles'")
# time.sleep(5.0)
# sys.exit(0)
# get Handles
def loadscn(clientID, model=genetic_bioloid):
error=vrep.simxLoadScene(clientID, model, 2, vrep.simx_opmode_oneshot_wait)
return error