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 close(self, kill=False):
if self.connected:
remote_api.simxStopSimulation(self.api_id, remote_api.simx_opmode_oneshot_wait)
remote_api.simxFinish(self.api_id)
self.connected = False
if kill and self.vrep_proc is not None:
os.killpg(self.vrep_proc.pid, signal.SIGTERM)
def cleanUp(self):
print "About to stop simulation connected to self.simulID: ", self.simulID
vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
vrep.simxFinish(self.simulID)
vrep.simxFinish(-1)
print "Disconnected from V-REP"
def reset_simulation(clientID):
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(1) # um pequeno sleep entre o stop e o start
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
for move in startMoves:
JointControl(clientID, 0, Body, move)
time.sleep(0.1)
def disconnection(self):
"""
Make disconnection with v-rep simulator
"""
# stop the simulation:
vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
# Now close the connection to V-REP:
vrep.simxFinish(self.clientID)
def restart_simulation(self):
mode = vrep.simx_opmode_oneshot_wait
assert vrep.simxStopSimulation(self.clientID, mode) == 0, \
"StopSim Error"
time.sleep(0.1)
self.start_simulation()
self.num_sim_steps = 0
def finishSimulation(clientID):
errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait)
error=errorStop or errorClose
errorFinish=vrep.simxFinish(clientID)
error=error or errorFinish
return error
def stop( self ):
"""
Stops the simulation
"""
err = vrep.simxStopSimulation( self.cid, vrep.simx_opmode_oneshot_wait )
time.sleep(0.01) # Maybe this will prevent V-REP from crashing as often
return hasattr(self, 'failed') # Returns true if this is a failed run
def finishSimulation(self, clientID):
self.getObjectPositionFirstTime = True
errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait)
error=errorStop or errorClose
errorFinish=vrep.simxFinish(clientID)
error=error or errorFinish
return error
def __init__(self, n_robots, base_name):
self.name = "line follower tester"
self.n_robots = n_robots
self.base_name = base_name
self.robot_names = [self.base_name]
for i in range(self.n_robots-1):
self.robot_names.append(self.base_name+'#'+str(i))
# Establish connection to V-REP
vrep.simxFinish(-1) # just in case, close all opened connections
# Connect to the simulation using V-REP's remote API (configured in V-REP, not scene specific)
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
# Use port 19999 and add simExtRemoteApiStart(19999) to some child-script in your scene for scene specific API
# (requires the simulation to be running)
if self.clientID != -1:
print ('Connected to remote API server')
else:
print ('Failed connecting to remote API server')
sys.exit('Could not connect')
# Reset running simulation
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
time.sleep(0.2)
# Get initial robots' positions
self.robot_handles = []
self.robot_pos0 = []
for rbt_name in self.robot_names:
res, rbt_tmp = vrep.simxGetObjectHandle(self.clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
self.robot_handles.append(rbt_tmp)
# Initialize data stream
vrep.simxGetObjectPosition(self.clientID, self.robot_handles[-1], -1, vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_1', vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_2', vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_3', vrep.simx_opmode_streaming)
time.sleep(0.2)
for rbt in self.robot_handles:
res, pos = vrep.simxGetObjectPosition(self.clientID, rbt, -1, vrep.simx_opmode_buffer)
self.robot_pos0.append(pos)
def reset(self):
"""In VREP we reset a simulation by stopping and restarting it"""
eCode = vrep.simxStopSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("Could not stop VREP simulation")
eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("Could not start VREP simulation")
vrep.simxSynchronousTrigger(self._VREP_clientId)
def reset_vrep():
print 'Start to connect vrep'
# Close eventual old connections
vrep.simxFinish(-1)
# Connect to V-REP remote server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
opmode = vrep.simx_opmode_oneshot_wait
# Try to retrieve motors and robot handlers
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectHandle
ret_l, LMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", opmode)
ret_r, RMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", opmode)
ret_a, RobotHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx", opmode)
vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, 0, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, 0, vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(clientID)
print 'Connection to vrep reset-ed!'
def reset( self ):
err = vrep.simxStopSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
time.sleep(1)
self.pos_err = [0,0,0]
self.ori_err = [0,0,0]
self.lin = [0,0,0]
self.ang = [0,0,0]
err = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
if SYNC:
vrep.simxSynchronous( self.cid, True )
def reset_rob(self): """ Sets the bubbleRob to his starting position; mind the hack, simulation has to be stopped """ ##### Set absolute position #stop simulation vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) #100ms delay, this is a hack because server isn't ready either time.sleep(0.3) #set on absolute position err = vrep.simxSetObjectPosition(self.clientID, self.bubbleRobHandle, -1, self.bubbleRobStartPosition, vrep.simx_opmode_oneshot_wait) #start simulation again vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) time.sleep(0.3)
def resetSimulation(self):
returnCode = vrep.simx_return_novalue_flag
while returnCode!=vrep.simx_return_ok:
returnCode=vrep.simxStopSimulation(self.__clientID, vrep.simx_opmode_oneshot)
time.sleep(0.5)
time.sleep(0.5)
returnCode = vrep.simx_return_novalue_flag
while returnCode!=vrep.simx_return_ok:
returnCode=vrep.simxStartSimulation(self.__clientID, vrep.simx_opmode_oneshot)
time.sleep(0.5)
time.sleep(1.0)
def exit_gracefully(signum, frame):
running = False
global clientID
# stop the simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Before closing the connection to V-REP,
# make sure that the last command sent out had time to arrive.
vrep.simxGetPingTime(clientID)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
print('connection closed...')
original_sigint = signal.getsignal(signal.SIGINT)
# restore the original signal handler as otherwise evil things will happen
# in raw_input when CTRL+C is pressed, and our signal handler is not re-entrant
signal.signal(signal.SIGINT, original_sigint)
# restore the exit gracefully handler here
signal.signal(signal.SIGINT, exit_gracefully)
def playing():
c = 0
model.load_weights('saved-models-modelWeight_buffer_3100.h5')
sensor_h = sensorHandles()
#Reading of all the current sensors
while (1):
state = sensorInformation(sensor_h, clientID)
qVal = model.predict(np.array(state).reshape(1, 5), batch_size=1)
action = np.argmax(qVal)
new_state = makeMove(state, action)
with open('Questions.txt', 'a') as writeFile:
writeFile.write(str(state) + "," + str(action) + "\n")
state = new_state
if min(state) < 0.09:
#sys.exit()
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(1)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
c = c + 1
print(c)
def stop_simulation(self):
# issue command to stop simulation
vrep.simxStopSimulation(self.client_id, vrep.simx_opmode_blocking)
# stop all streaming
for handle_idx in range(0, self.joint_count):
_, _ = vrep.simxGetJointPosition(self.client_id,
self.handles[handle_idx],
vrep.simx_opmode_discontinue)
for handle_idx in range(0, self.joint_count):
_, _ = vrep.simxGetObjectFloatParameter(
self.client_id, self.handles[handle_idx], 2012,
vrep.simx_opmode_discontinue)
_, _ = vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1,
vrep.simx_opmode_discontinue)
_, _ = vrep.simxGetObjectOrientation(self.client_id, self.body_handle,
-1, vrep.simx_opmode_discontinue)
# Just to make sure this gets executed
vrep.simxGetPingTime(self.client_id)
self.logger.info("simulation stopped")
def signal_handler(signal, frame):
print('You pressed ctrl C. Stopping the simulation...')
return_code = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot)
x = np.asarray(x_training_labels)
np.save("x_data", x)
y = np.asarray(y_training_labels)
np.save("y_data", y)
time.sleep(1)
vrep.simxFinish(clientID)
sys.exit(0)
def run_simulation(self, simTime):
count = simTime
p = 0
while count < 1: # run for 1 simulated second
self.q = np.zeros(len(self.joint_handles))
self.dq = np.zeros(len(self.joint_handles))
self.pos = np.zeros(len(self.joint_handles))
print("t= %f" % (count))
for ii, joint_handle in enumerate(self.joint_handles):
# get the joint angles
_, self.q[ii] = vrep.simxGetJointPosition(
self.clientID, joint_handle, self.block_mode)
if _ != 0: raise Exception()
# get the joint velocityp
_, self.dq[ii] = vrep.simxGetObjectFloatParameter(
self.clientID, joint_handle, 2012,
self.block_mode) # ID for angular velocity of the joint
if _ != 0: raise Exception()
vrep.simxSetJointPosition(self.clientID, joint_handle, p,
vrep.simx_opmode_oneshot)
print(self.joint_handles)
self.record_demo("Abc.csv")
print("Joint Positions in ", self.q * (180 / 3.14))
count = count + self.dt
print("Time Elapsed in simulation", count)
p = p + (3.14 / 180) * 5
print("p is %f" % (p))
vrep.simxSynchronousTrigger(self.clientID)
# stop our simulation
print("simulation shutting down")
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking)
def reset(self):
vrep.simxStopSimulation(self.clientId, vrep.simx_opmode_oneshot)
quad_pos = [-0.75, 0.1, 0.5]
random.seed(time.time())
ball_pos = [-0.75+random.uniform(-0.05,0.05), 0.1+random.uniform(-0.05,0.05), 1.6]
quad_target_pos = [-0.75, 0.1, 0.5]
vrep.simxSetObjectPosition(self.clientId, self.quad_target, -1, quad_target_pos, vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(self.clientId, self.quad, -1, quad_pos, vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(self.clientId, self.ball, -1, ball_pos, vrep.simx_opmode_oneshot)
time.sleep(0.6)
vrep.simxStartSimulation(self.clientId, vrep.simx_opmode_oneshot)
_, self.ball_position = vrep.simxGetObjectPosition(self.clientId, self.ball, self.quad, vrep.simx_opmode_buffer)
_, self.quad_position = vrep.simxGetObjectPosition(self.clientId, self.quad, -1, vrep.simx_opmode_buffer)
_, self.quad_orientation = vrep.simxGetObjectOrientation(self.clientId, self.quad, -1, vrep.simx_opmode_buffer)
_, self.quad_target_position = vrep.simxGetObjectPosition(self.clientId, self.quad_target, -1, vrep.simx_opmode_buffer)
self.previous_target_pos = np.asarray(self.quad_target_position)
self.prev_ball_pos = self.ball_position
# return np.concatenate((np.asarray(self.quad_position), np.asarray(self.ball_position)),axis=0)
return np.asarray(self.ball_position+ [self.quad_position[2]]+ self.quad_orientation)
def start_new_episode(self, episode, reset_positions):
if reset_positions:
# Stop the simulation if it is running
vrep.simxStopSimulation(self.__client_ID, vrep.simx_opmode_blocking)
time.sleep(2) # One second dela
self.randomize_positions()
time.sleep(1) # One second delay
# Start simulation
vrep.simxStartSimulation(self.__client_ID, vrep.simx_opmode_blocking)
# Set display to enabled or not
vrep.simxSetBooleanParameter(self.__client_ID, vrep.sim_boolparam_display_enabled, self.__show_display, vrep.simx_opmode_oneshot)
# Enable threaded rendering
vrep.simxSetBooleanParameter(self.__client_ID, vrep.sim_boolparam_threaded_rendering_enabled, 1, vrep.simx_opmode_oneshot)
# Catch error
error = vrep.simxSynchronous(self.__client_ID, 1)
assert error == 0, "Could not start simulation in synchronous mode."
# Reset all agents
for agent in self.__agents:
agent.restart()
def reset( self ):
err = vrep.simxStopSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
time.sleep(1)
self.pos_err = [0,0,0]
self.ori_err = [0,0,0]
self.lin = [0,0,0]
self.ang = [0,0,0]
self.vert_prox = 0
self.left_prox = 0
self.right_prox = 0
err = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
if self.sync:
vrep.simxSynchronous( self.cid, True )
def resetSimulation(self):
returnCode = vrep.simx_return_novalue_flag
while returnCode != vrep.simx_return_ok:
returnCode = vrep.simxStopSimulation(self.__clientID,
vrep.simx_opmode_oneshot)
time.sleep(0.5)
time.sleep(0.5)
returnCode = vrep.simx_return_novalue_flag
while returnCode != vrep.simx_return_ok:
returnCode = vrep.simxStartSimulation(self.__clientID,
vrep.simx_opmode_oneshot)
time.sleep(0.5)
time.sleep(1.0)
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 main():
Larm_theta = []
Rarm_theta = []
Larm_flag = False
Rarm_flag = False
print "This is a gripping test simulation for the Baxter robot"
clientID = initialize_sim()
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
joint_library, Larm_jointsdict, Rarm_jointsdict, body_jointsdict, joint_bodynames, joint_Rarm, joint_Larm = declarejointvar(
clientID)
collision_library = getCollisionlib(clientID)
time.sleep(5)
# stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# 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)
# Close the connection to V-REP
vrep.simxFinish(clientID)
def startSim(self, clientID, screen=True):
#I need the simulator stopped in order to be started
retSimStop = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
if retSimStop != simx_return_ok :
print "simulation couldnt be stopped!"
else:
print "simulation stopped!"
#setting the physics engine
retSetPhyEngine = vrep.simxSetIntegerParameter(clientID, vrep.sim_intparam_dynamic_engine, bulletEngine, vrep.simx_opmode_oneshot_wait)
if retSetPhyEngine != simx_return_ok:
print "unable to set the physical engine"
else:
print "physical engine correctly setted"
if int(self.sysConf.getProperty("physics enable?"))==1:
vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,True,vrep.simx_opmode_oneshot)
else:
vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,False,vrep.simx_opmode_oneshot)
#settig simulation speed
if self.speedmodifier > 0:
vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait)
#settig simulation step
retSetTimeStep = vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, self.simulationTimeStepDT, vrep.simx_opmode_oneshot_wait)
if retSetTimeStep != simx_return_ok :
print "problems setting time step"
else:
print "time step setted!"
#vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_realtime_simulation,1,vrep.simx_opmode_oneshot_wait)
#sync mode configuration
if self.synchronous:
vrep.simxSynchronous(clientID,True)
#light mode configuration
if not screen:
vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_visible_layers,2,vrep.simx_opmode_oneshot_wait)
#vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)
#start simulation
error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
if int(self.sysConf.getProperty("blank screen?"))==1:
vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)
return error
def stop_simulation(self):
if not self.sim_running:
raise RuntimeError('Simulation is not running.')
self.RAPI_rc(vrep.simxStopSimulation(self.cID, vrep.simx_opmode_blocking))
# Checking if the server really stopped
try:
while True:
self.RAPI_rc(vrep.simxGetIntegerSignal(self.cID,'sig_debug',vrep.simx_opmode_blocking))
e = vrep.simxGetInMessageInfo(self.cID,vrep.simx_headeroffset_server_state)
still_running = e[1] & 1
if not still_running:
break
except: pass
self.sim_running = False
def StopSimulator(self):
print('Attempting to Stop the Simulator')
if vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait) != 0:
print('Could not stop the simulator. You can stop the simulator manually by pressing the Stop button in VREP.')
else:
print('Successfully stoped the VREP Simulator.')
# Stop streaming modes to each object
vrep.simxGetObjectPosition(self.clientID, self.robotHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectOrientation(self.clientID, self.robotHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectPosition(self.clientID, self.cameraHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectPosition(self.clientID, self.ballHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectPosition(self.clientID, self.blueGoalHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectPosition(self.clientID, self.yellowGoalHandle, -1, vrep.simx_opmode_discontinue)
for handle in self.obstacleHandles:
vrep.simxGetObjectPosition(self.clientID, handle, -1, vrep.simx_opmode_discontinue)
def stop_simulation(self):
"""Stop the simulation"""
start = time.time()
if not hasattr(self, 'clientID'):
raise Exception(
'The simulation has not been started. Check start_simulation function'
)
while time.time() - start < self.timeout:
response = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_blocking)
if response == 0:
print('Simulation stopped')
vrep.simxFinish(-1)
return True
raise Exception("Can't stop the simulation")
def stop_sim(self, verbose=None):
"""Stop V-REP simulation."""
# If necessary, determine whether messages should be displayed
if verbose is None:
verbose = self.verbose
# Stop V-REP simulation
if self._client_id is None:
raise ConnectionError("Could not stop V-REP simulation: not "
"connected to V-REP remote API server.")
res = vrep.simxStopSimulation(self._client_id,
vrep.simx_opmode_blocking)
if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag):
raise ServerError("Could not stop V-REP simulation.")
# If necessary, display confirmation message
if verbose:
print("V-REP simulation stopped at "
"{}.".format(time.strftime("%H:%M:%S")))
def vrepSim(clientID, action):
vrep.simxFinish(-1)
localhost = "127.0.0.1"
clientID=vrep.simxStart(localhost,19997,True,True,1000,5)
if clientID!=-1:
print('Connected to remote API server')
if action=="start":
err = vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
if (not err):
print('Sim Started')
elif action=="stop":
err = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
if (not err):
print('Sim Stopped')
print("Disconnected from remote API server")
else:
print('Failed connecting to remote API server')
vrep.simxFinish(clientID)
return clientID
def main():
# this is the main script where all the code is executed
# access all the VREP elements
vrep.simxFinish(-1)
# just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
# start aconnection
if clientID != -1:
print("Connected to remote API server")
else:
print("Not connected to remote API server")
sys.exit("Could not connect")
_ = vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# make bigger the sensor from the parts producer
#Wait until the objects reach the image sensor then stop!
# perhaps a loop that asks every x second is the sensor activated or not (read signal)
time.sleep(10)
_ = vrep.simxStopSimulation(clientID, simx_opmode_blocking)
'| Reward for this step: %f' % r,
'| Global steps: %i' % STEP_COUNT,
'| Epsilon: %.4f' % epsilon, '| ls: ', osc_ls)
if isFall:
print("The robot falls down.")
break
if isOut:
print("The robot is outside")
break
with open("performance_data/{}.csv".format(t), 'a') as f:
x += 2
f.write(
str(i_episode) + ',' + str(episode_reward) + ',' + str(x) +
',' + str(i_step) + ',' + str(y) + '\n')
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(1)
vrep.simxFinish(clientID)
print("Finish.")
# if __name__ == "__main__":
# for i_episode in range(MAX_EPISODES):
# vrep.simxSynchronous(clientID, 1)
# vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# #print('#################start##################')
# print("Now we are starting a new episode.")
# episode_reward = 0
# #para_vec_old = para_vec
# dx = 0
def foo (portNumb, instructions):
clientID = vrep.simxStart('127.0.0.1', portNumb, True, True, 5000, 5)
if clientID != -1 :
print ("se pudo establecer la conexión con la api del simulador")
#Recover handlers for robot parts
LUM,LLM,RUM,RLM,head = recoverRobotParts(clientID)
#Set Initial Target Velocity to 0
LUMSpeed = 0
LLMSpeed = 0
RUMSpeed = 0
RLMSpeed = 0
setVelocity(clientID,LUM,LUMSpeed)
setVelocity(clientID,LLM,LLMSpeed)
setVelocity(clientID,RUM,RUMSpeed)
setVelocity(clientID,RLM,RLMSpeed)
#Set simulation to be Synchonous instead of Asynchronous
vrep.simxSynchronous(clientID, True)
#Setting Time Step to 50 ms (miliseconds)
dt = 0.05
#WARNING!!! - Time step should NEVER be set to custom because it messes up the simulation behavior!!!!
#vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, dt, vrep.simx_opmode_blocking)
#Start simulation if it didn't start
vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)
#This are for controlling where I'm in the instructions while simulation is running
secuenceIndex = 0
runInfo = []
headSecuenceTrace = []
lowerSpeed, upperSpeed = 0, 0
secuenceTimes = []
for secuence in instructions:
instructionIndex = 0
headTrace = []
extraPoints = 0
runtime = 0
for instruction in secuence:
instructionIndex+=1
moveRobot(clientID,instruction[0], LUM, LLM, RUM, RLM)
#This is what makes the simulation Synchronous
initialTime = 0.0
actualTime = initialTime + dt
runtime += dt
#Condition to stop simulation
hasFallen = False
vrep.simxSynchronousTrigger(clientID)
#Retrive head position
headPosition = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_oneshot)
#headTrace.append((headPosition,runtime))
while((actualTime - initialTime) < (instruction[1]/10)):
#Make de simulation run one step (dt determines how many seconds pass by between step and step)
vrep.simxSynchronousTrigger(clientID)
#Advance time in my internal counter
actualTime = actualTime + dt
runtime += dt
#TODO do I still need the extra points for time?
extraPoints += dt
#Retrive head position
headPosition = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_oneshot)
headTrace.append((headPosition,runtime))
#Verify that the model hasn't fallen
if(headPosition[0] == 0 and headPosition[1][2]<0.65):
#print("Posición de la cabeza:", headPosition[1][2])
#print("tiempo: ", runtime)
hasFallen = True
break
if(hasFallen):
break
if (hasFallen):
print ("Secuence: ", secuenceIndex, " has fallen!!")
else:
print("Secuence: ", secuenceIndex, " has finished without falling")
#print(secuence)
secuenceTimes.append(extraPoints)
#Here I collect the data for the whole secuence
#filter not valid positions
headTrace = list(filter(lambda x: x[0][0] == 0,headTrace))
#add to whole run trace info
headSecuenceTrace.append(headTrace)
fallenFactor = 0
if hasFallen:
fallenFactor = -50
#format: (index, score, headtrace((valid,(x,y,z),time))
runInfo.append((secuenceIndex, sum(map(lambda x:math.log(1/distancia(x[0][1],puntoMovil(x[1]))),headTrace))+fallenFactor,headTrace))
print("puntaje obtenido",runInfo[-1][1])
secuenceIndex+=1
#Stop_Start_Simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
#This sleep is necesary for the simulation to finish stopping before starting again
time.sleep(2)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
#Should always end by finishing connetions
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(clientID)
sortedScore = sorted(runInfo, key=lambda x:x[1], reverse=True)
return sortedScore
else:
print ("No se pudo establecer conexión con la api del simulador en el puerto: ", portNumb)
print ("Verificar que el simulador está abierto")
population.append([awrist, aelbow, ashoulder, rs])
#print "The evolution individual is [awrist, aelbow, ashoulder, dis]: \n" + str(population)
# Get the robot orientation after the movement sequence
oret, robotOrient = vrep.simxGetObjectOrientation(clientID, robotHandle, -1, vrep.simx_opmode_buffer)
msg = "2w1a orientation: (x = " + str(robotOrient[0]) +\
", y = " + str(robotOrient[1]) +\
", z = " + str(robotOrient[2]) + ")"
print msg
logging.info(msg)
msg = "\n----- next move -----\n"
print msg
logging.info(msg)
vrep.simxStopSimulation(clientID, opmode)
time.sleep(1)
print "The evolution individual is [awrist, aelbow, ashoulder, dis]: \n" + str(population)
# evolution for 20 times
for i in xrange(10):
msg = "\n>>>The " + str(i) + " generations: <<<\n"
print msg
logging.info(msg)
population = evolve(population)
# get the distance again
for j in range(0, len(population)-1):
vrep.simxStartSimulation(clientID, opmode)
awrist = population[j][0]
def stop_world(self):
vrep.unwrap_vrep(
vrep.simxStopSimulation(self._clientID, vrep.simx_opmode_blocking))
self.wait_for_ping()
import sys
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:
print ('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID,True)
# start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# Now step a few times:
for i in range(1,10):
if sys.version_info[0] == 3:
input('Press <enter> key to step the simulation!')
else:
raw_input('Press <enter> key to step the simulation!')
vrep.simxSynchronousTrigger(clientID);
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')
def stop(self):
returnCode = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot)
self.robot.reset()
if (returnCode>1):
print "returnCode: ", returnCode
raise Exception('Could not stop')
def clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
sys.exit(0)
ax4.matshow(q_table.T, cmap=plt.cm.gray)
ax4.set_title('Q-table')
ax4.xaxis.set_ticks_position('bottom')
ax4.set_xlabel('state')
ax4.set_ylabel('action')
plt.tight_layout()
plt.show()
plt.draw()
if __name__ == '__main__':
try:
client_id
except NameError:
client_id = -1
e = vrep.simxStopSimulation(client_id, vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(-1)
client_id = vrep.simxStart('127.0.0.1', 19998, True, True, 5000, 5)
assert client_id != -1, 'Failed connecting to remote API server'
# print ping time
sec, msec = vrep.simxGetPingTime(client_id)
print "Ping time: %f" % (sec + msec / 1000.0)
robot = Robot(client_id)
agent = Agent(robot, alpha=0.1, gamma=0.9, epsilon=0.05, q_init=0.3)
num_episodes = 500
len_episode = 100
return_history = []
try:
def stopSimulation(self):
"""
Stop simulation
"""
# stop the simulation:
vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter):
try:
import vrep
except:
print ('--------------------------------------------------------------')
print ('"vrep.py" could not be imported. This means very probably that')
print ('either "vrep.py" or the remoteApi library could not be found.')
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "vrep.py"')
print ('--------------------------------------------------------------')
print ('')
""" =========================================================== """
""" ============ Imported Libraries: ============ """
import time
import numpy as np
from captains_log_v1 import log_data
import os
import cost
import csv
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
""" =================================================================== """
# output limits:
#min_output=0
#max_output=8.335
# program parameters:
global i
i = 0
xe = []
ye = []
ze = []
xs = []
ys = []
zs = []
x_qc = []
y_qc = []
z_qc = []
u = []
v1 = []
v2 = []
v3 = []
v4 = []
#global variables:
cumul=0
last_e=0
pAlphaE=0
pBetaE=0
psp2=0
psp1=0
prevEuler=0
cumulAlpha = 0
cumulBeta = 0
cumulAlphaPos = 0
cumulBetaPos = 0
s_r = 0
particlesTargetVelocities=[0,0,0,0]
#speed weight:
vParam=-2
#parameters for vertical control
Kpv=2
Kiv=0
Kdv=2
#parameters for horizontal control:
Kph=0.4 # TBD
Kih=0.1 # TBD
Kdh=1.5
Kph_pos1=0.4
Kih_pos1=0.001
Kdh_pos1=0.05
Kph_pos0=0.4
Kih_pos0=0.001
Kdh_pos0=0.05
#parameters for rotational control:
Kpr=0.05
Kir=0
Kdr=0.9
""" =========================================================== """
# parameters needed for gradient descent:
t0 = 0
tf = 1
dt = 0.01
sum_h_alpha = []
sum_h_beta = []
sum_h_pos0 = []
sum_h_pos1 = []
last_alpha_angle = 0
last_alpha_pos = 0
last_beta_angle = 0
last_beta_pos = 0
delta_alpha_angle = []
delta_alpha_pos = []
delta_beta_angle = []
delta_beta_pos = []
J_h_alpha = []
J_h_beta = []
J_h_pos0 = []
J_h_pos1 = []
paramX = []
paramY = []
theta_h_angles = [Kph, Kih, Kdh]
theta_h_pos = [Kph_pos0, Kih_pos0, Kdh_pos0] # Kph_pos0 == Kph_pos1 ...
gd = 0
""" =========================================================== """
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:
print ('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID,True)
# start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)
#functional/handle code:
emptyBuff=bytearray()
[returnCode,targetObj]=vrep.simxGetObjectHandle(clientID,Quadricopter_target,vrep.simx_opmode_blocking)
[returnCode,qc_base_handle]=vrep.simxGetObjectHandle(clientID,Quadricopter_base,vrep.simx_opmode_blocking)
[returnCode,qc_handle]=vrep.simxGetObjectHandle(clientID,Quadricopter,vrep.simx_opmode_blocking)
# main loop:
while True:
""" ========== vertical control: ========== """
[returnCode,targetPos]=vrep.simxGetObjectPosition(clientID,targetObj,-1,vrep.simx_opmode_blocking)
[returnCode,pos]=vrep.simxGetObjectPosition(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking)
[returnCode, l, w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle, vrep.simx_opmode_blocking)
e=targetPos[2]-pos[2]
cumul=cumul+e
diff_e=e-last_e
Pvert=Kpv*e
Ivert=Kiv*cumul
Dvert=Kdv*diff_e
thrust=5.335+Pvert+Ivert+Dvert+l[2]*vParam# get thrust
last_e=e
""" =========================================================== """
""" ========== horizontal control: ========== """
[returnCode,sp]=vrep.simxGetObjectPosition(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking)
[rc,rc,vx,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_vx',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
[rc,rc,vy,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_vy',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
[rc,rc,rc,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_Object_Matrix',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
[errorCode,M]=vrep.simxGetStringSignal(clientID,'mtable',vrep.simx_opmode_oneshot_wait);
if (errorCode==vrep.simx_return_ok):
m=vrep.simxUnpackFloats(M)
alphaE=vy[2]-m[11]
cumulAlpha = cumulAlpha + alphaE
diff_alphaE=alphaE-pAlphaE
alphaCorr=Kph*alphaE + Kih*cumulAlpha + Kdh*diff_alphaE #alpha correction
betaE=vx[2]-m[11]
cumulBeta = cumulBeta + betaE
diff_betaE=betaE-pBetaE
betaCorr=-Kph*betaE - Kih*cumulBeta - Kdh*diff_betaE #beta correction
pAlphaE=alphaE
pBetaE=betaE
cumulAlphaPos = cumulAlphaPos + sp[1]
cumulBetaPos = cumulBetaPos + sp[0]
alphaPos=Kph_pos1*(sp[1])+ Kih_pos1*cumulAlphaPos +Kdh_pos1*(sp[1]-psp2) #alpha position correction
betaPos=Kph_pos0*(sp[0])+ Kih_pos0*cumulBetaPos + Kdh_pos0*(sp[0]-psp1) #beta position correction
alphaCorr=alphaCorr+alphaPos
betaCorr=betaCorr-betaPos
psp2=sp[1]
psp1=sp[0]
delta_alpha_angle.append(alphaCorr - last_alpha_angle)
delta_beta_angle.append(betaCorr - last_beta_angle)
delta_alpha_pos.append(alphaPos - last_alpha_pos)
delta_beta_pos.append(betaPos - last_beta_pos)
last_alpha_angle = alphaCorr
last_beta_angle = betaCorr
last_alpha_pos = alphaPos
last_beta_pos = betaPos
""" =========================================================== """
""" ========== rotational control: ========== """
[returnCode,euler]=vrep.simxGetObjectOrientation(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking)
[returnCode,orientation]=vrep.simxGetObjectOrientation(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking)
Prot=Kpr*euler[2]
Drot=Kdr*(euler[2]-prevEuler)
s_r = s_r + euler[2]
rotCorr=Prot+Drot
prevEuler=euler[2]
""" =========================================================== """
""" ========== set propeller velocities: ========== """
propeller1_PTV = thrust*(1-alphaCorr+betaCorr-rotCorr)
propeller2_PTV = thrust*(1-alphaCorr-betaCorr+rotCorr)
propeller3_PTV = thrust*(1+alphaCorr-betaCorr-rotCorr)
propeller4_PTV = thrust*(1+alphaCorr+betaCorr+rotCorr)
particlesTargetVelocities=[propeller1_PTV, propeller2_PTV, propeller3_PTV, propeller4_PTV]
""" =========================================================== """
""" ========== set parameters for logging data: ========== """
C_parameters_vert = [0,0,0]
C_parameters_horr = [0,0,0,0,0,0,0]
C_parameters_rot = [0,0,0]
vert_comp = [0,0,0,0]
horr_comp = [0,0,0,0,[0,0,0],0,0,0,0,0,0]
rot_comp = [[0,0,0],0]
C_parameters_vert[0] = Kpv
C_parameters_vert[1] = Kiv
C_parameters_vert[2] = Kdv
C_parameters_horr[0] = Kph
C_parameters_horr[1] = Kih
C_parameters_horr[2] = Kdh
C_parameters_horr[3] = Kph_pos0
C_parameters_horr[4] = Kdh_pos0
C_parameters_horr[5] = Kph_pos1
C_parameters_horr[6] = Kdh_pos1
C_parameters_rot[0] = Kpr
C_parameters_rot[1] = Kir
C_parameters_rot[2] = Kdr
vert_comp[0] = targetPos
vert_comp[1] = pos
vert_comp[2] = e
vert_comp[3] = thrust
horr_comp[0] = alphaE
horr_comp[1] = betaE
horr_comp[2] = cumulAlpha
horr_comp[3] = cumulBeta
horr_comp[4] = sp
horr_comp[5] = cumulAlphaPos
horr_comp[6] = cumulBetaPos
horr_comp[7] = alphaCorr
horr_comp[8] = betaCorr
horr_comp[9] = vx
horr_comp[10] = vy
rot_comp[0] = euler
rot_comp[1] = rotCorr
""" =========================================================== """
""" ========== PLOTTING: ========== """
## PLOTTING:
ze.append(e) # otstapuvanje od z-oska
xe.append(sp[0]) # otstapuvanje od x-oska
ye.append(sp[1]) # otstapuvawe od y-oska
# xs.append(targetPos[0])
# ys.append(targetPos[1])
# zs.append(targetPos[2])
# x_qc.append(pos[0])
# y_qc.append(pos[1])
# z_qc.append(pos[2])
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')
# ax.plot_wireframe(xs,ys,zs,color='blue')
# ax.plot_wireframe(x_qc,y_qc,z_qc,color='red')
# ax.set_xlim3d(-2.1,2.1)
# ax.set_ylim3d(-2.1,2.1)
# ax.set_zlim3d(0,1.9)
# plt.show()
# u.append(thrust)
# v1.append(propeller1_PTV)
# v2.append(propeller2_PTV)
# v3.append(propeller3_PTV)
# v4.append(propeller4_PTV)
#
plt.plot(xe,color='blue')
plt.hold(True)
plt.plot(ye,color='red')
plt.hold(True)
plt.plot(ze,color='green')
plt.hold(True)
# plt.plot(v4,color='pink')
# plt.hold(True)
# plt.axis([0,25,0,15])
# plt.show()
#
# plt.plot(xe,color='blue')
# plt.axis([0,100,-4,4])
# plt.show()
# plt.plot(ye,color='red')
# plt.axis([0,100,-4,4])
# plt.show()
# plt.plot(ze,color='green')
plt.axis([0,25,-2,2])
plt.show()
""" =========================================================== """
""" ========== Gradient descent: ========== """
sum_h_alpha.append(cumulAlpha)
sum_h_beta.append(cumulBeta)
sum_h_pos1.append(cumulAlphaPos)
sum_h_pos0.append(cumulBetaPos)
J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt)
J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt)
J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt)
J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt)
paramX.append(targetPos[0])
paramY.append(targetPos[1])
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
print "theta_h_angles : ", theta_h_angles
print "theta_h_pos : ", theta_h_pos
print "sum_h_alpha : ", sum_h_alpha[gd]
print "sum_h_beta : ", sum_h_beta[gd]
print "sum_h_pos1 : ", sum_h_pos1[gd]
print "sum_h_pos0 : ", sum_h_pos0[gd]
print "J_h_alpha : ", J_h_alpha[gd]
print "J_h_beta : ", J_h_beta[gd]
print "J_h_pos1 : ", J_h_pos1[gd]
print "J_h_pos0 : ", J_h_pos0[gd]
print "paramX : ", paramX[gd]
print "paramY : ", paramY[gd]
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
gd = gd + 1
print "gd (iterations) : ", gd
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
""" =========================================================== """
if gd == 400:
with open('GD_43.csv','wb') as f1:
writer=csv.writer(f1)
for i in range(0, gd):
writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]])
break
""" =========================================================== """
""" ========== WRITE TO TEXT: ========== """
## WRITE TO TEXT:
log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i)
i +=1
""" =========================================================== """
# send propeller velocities to output:
[res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities,[],emptyBuff,vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
""" =========================================================== """
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
def stop_sim(self):
time.sleep(self.sleep_sec_min)
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
def stopSimulation(self):
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
def Stop_VREP(clientID):
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(clientID)
def main():
Larm_theta = []
Rarm_theta = []
Larm_flag = False
Rarm_flag = False
print "This is a path planning simulation for the Baxter robot"
arm_response1 = raw_input("Would you like to move the left arm? (Y or N)")
#for user reference
#joint_upperbound = [97.494, 60, 174.987, 150, 175.25, 120, 175.25]
#joint_lowerbound = [-97.494, -123, -174.987, -2.864, -175.25, -90, -175.25]
if arm_response1 == 'Y':
Larm_flag = True
print "Input Theta in degrees"
theta1 = raw_input("Theta1 = ")
Larm_theta.append(float(theta1))
theta2 = raw_input("Theta2 = ")
Larm_theta.append(float(theta2))
theta3 = raw_input("Theta3 = ")
Larm_theta.append(float(theta3))
theta4 = raw_input("Theta4 = ")
Larm_theta.append(float(theta4))
theta5 = raw_input("Theta5 = ")
Larm_theta.append(float(theta5))
theta6 = raw_input("Theta6 = ")
Larm_theta.append(float(theta6))
theta7 = raw_input("Theta7 = ")
Larm_theta.append(float(theta7))
arm_response2 = raw_input("Would you like to move the right arm? (Y or N)")
if arm_response2 == 'Y':
Rarm_flag = True
print "Input Theta in degrees"
theta1 = raw_input("Theta1 = ")
Rarm_theta.append(float(theta1))
theta2 = raw_input("Theta2 = ")
Rarm_theta.append(float(theta2))
theta3 = raw_input("Theta3 = ")
Rarm_theta.append(float(theta3))
theta4 = raw_input("Theta4 = ")
Rarm_theta.append(float(theta4))
theta5 = raw_input("Theta5 = ")
Rarm_theta.append(float(theta5))
theta6 = raw_input("Theta6 = ")
Rarm_theta.append(float(theta6))
theta7 = raw_input("Theta7 = ")
Rarm_theta.append(float(theta7))
if Larm_flag == False:
Larm_theta = [0, 0, 0, 0, 0, 0, 0]
if Rarm_flag == False:
Rarm_theta = [0, 0, 0, 0, 0, 0, 0]
#check if thetas are valid
Rarm_theta = checkjointThetas(Rarm_theta)
Larm_theta = checkjointThetas(Larm_theta)
clientID = initialize_sim()
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
joint_library, Larm_jointsdict, Rarm_jointsdict, body_jointsdict, joint_bodynames, joint_Rarm, joint_Larm = declarejointvar(
clientID)
collision_library = getCollisionlib(clientID)
#generate path
#Initial thetas are 0, desired goal thetas are user-input
#path plan for L_joints
executemovement(clientID, Larm_jointsdict, joint_Larm, collision_library,
Larm_theta, 'L')
#path plan for R_joints
#executemovement(clientID, Rarm_jointsdict, joint_Rarm, collision_library, Rarm_theta, 'R')
# stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# 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)
# Close the connection to V-REP
vrep.simxFinish(clientID)
def stop():
errorCode = vrep.simxStopSimulation(clientID,
vrep.simx_opmode_oneshot_wait)
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter,
Quadricopter_target2, Quadricopter_base2, Quadricopter2):
try:
import vrep
except:
print('--------------------------------------------------------------')
print('"vrep.py" could not be imported. This means very probably that')
print('either "vrep.py" or the remoteApi library could not be found.')
print('Make sure both are in the same folder as this file,')
print('or appropriately adjust the file "vrep.py"')
print('--------------------------------------------------------------')
print('')
""" =========================================================== """
""" ============ Imported Libraries: ============ """
import time
import numpy as np
from captains_log_v1 import log_data
import os
import cost
import csv
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import main_PID_controller
""" =================================================================== """
# output limits:
#min_output=0
#max_output=8.335
# program parameters:
global i
i = 0
xf = []
xl = []
yl = []
yf = []
zl = []
zf = []
vxl = []
vxf = []
vyl = []
vyf = []
euler_l = []
euler_f = []
# xe = []
# ye = []
ze = []
# xs = []
# ys = []
# zs = []
x_qc = []
y_qc = []
z_qc = []
x_qc2 = []
y_qc2 = []
z_qc2 = []
# u = []
v1 = []
v2 = []
v3 = []
v4 = []
v12 = []
v22 = []
v32 = []
v42 = []
#global variables:
cumul = 0
last_e = 0
pAlphaE = 0
pBetaE = 0
psp2 = 0
psp1 = 0
prevEuler = 0
cumulAlpha = 0
cumulBeta = 0
cumulAlphaPos = 0
cumulBetaPos = 0
cumul2 = 0
last_e2 = 0
pAlphaE2 = 0
pBetaE2 = 0
psp22 = 0
psp12 = 0
prevEuler2 = 0
cumulAlpha2 = 0
cumulBeta2 = 0
cumulAlphaPos2 = 0
cumulBetaPos2 = 0
cumulPOS = 0
cumulVY = 0
cumulVX = 0
cumulSP0 = 0
cumulSP1 = 0
cumulEULER = 0
cumulPOS2 = 0
cumulVY2 = 0
cumulVX2 = 0
cumulSP02 = 0
cumulSP12 = 0
cumulEULER2 = 0
particlesTargetVelocities = [0, 0, 0, 0]
particlesTargetVelocities2 = [0, 0, 0, 0]
#speed weight:
vParam = -2
#parameters for vertical control
Kpv = 2
Kiv = 0
Kdv = 2
#parameters for horizontal control:
Kph = 0.4 # TBD
Kih = 0.1 # TBD
Kdh = 1.5
Kph_pos1 = 0.4
Kih_pos1 = 0.002
Kdh_pos1 = 0.05
Kph_pos0 = 0.4
Kih_pos0 = 0.002
Kdh_pos0 = 0.05
#parameters for rotational control:
Kpr = 0.05
# Kir=0
Kdr = 0.9
""" =========================================================== """
gd = 0
""" =========================================================== """
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:
print('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, True)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
#functional/handle code:
emptyBuff = bytearray()
[returnCode,
targetObj] = vrep.simxGetObjectHandle(clientID, Quadricopter_target,
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle] = vrep.simxGetObjectHandle(clientID,
Quadricopter_base,
vrep.simx_opmode_blocking)
[returnCode,
qc_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter,
vrep.simx_opmode_blocking)
[returnCode,
targetObj2] = vrep.simxGetObjectHandle(clientID, Quadricopter_target2,
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle2] = vrep.simxGetObjectHandle(clientID,
Quadricopter_base2,
vrep.simx_opmode_blocking)
[returnCode,
qc_handle2] = vrep.simxGetObjectHandle(clientID, Quadricopter2,
vrep.simx_opmode_blocking)
# main loop:
while True:
""" ========== vertical control: ========== """
[returnCode,
targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
[returnCode, l,
w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle,
vrep.simx_opmode_blocking)
[returnCode, targetPos2
] = vrep.simxGetObjectPosition(clientID, targetObj2, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos2] = vrep.simxGetObjectPosition(clientID, qc_base_handle2, -1,
vrep.simx_opmode_blocking)
[returnCode, l2,
w2] = vrep.simxGetObjectVelocity(clientID, qc_base_handle2,
vrep.simx_opmode_blocking)
""" =========================================================== """
""" ========== horizontal control: ========== """
[returnCode,
sp] = vrep.simxGetObjectPosition(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, rc, rc,
rc] = vrep.simxCallScriptFunction(clientID, Quadricopter,
vrep.sim_scripttype_childscript,
'qc_Get_Object_Matrix', [], [],
[], emptyBuff,
vrep.simx_opmode_blocking)
[errorCode,
M] = vrep.simxGetStringSignal(clientID, 'mtable',
vrep.simx_opmode_oneshot_wait)
if (errorCode == vrep.simx_return_ok):
m = vrep.simxUnpackFloats(M)
m1 = m[0]
m2 = m[1]
vx1 = [vx[0], vx[1], vx[2]]
vx2 = [vx[3], vx[4], vx[5]]
vy1 = [vy[0], vy[1], vy[2]]
vy2 = [vy[3], vy[4], vy[5]]
[returnCode,
sp2] = vrep.simxGetObjectPosition(clientID, targetObj2,
qc_base_handle2,
vrep.simx_opmode_blocking)
""" =========================================================== """
""" ========== rotational control: ========== """
[returnCode,
euler] = vrep.simxGetObjectOrientation(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[returnCode, orientation
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
[returnCode,
euler2] = vrep.simxGetObjectOrientation(clientID, targetObj2,
qc_base_handle2,
vrep.simx_opmode_blocking)
[returnCode, orientation2
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle2, -1,
vrep.simx_opmode_blocking)
""" =========================================================== """
""" ========== set propeller velocities: ========== """
particlesTargetVelocities, cumul, last_e, cumulAlpha, pAlphaE, cumulBeta, pBetaE, cumulAlphaPos, cumulBetaPos, psp2, psp1, prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1, cumulEULER = main_PID_controller.PID(
targetPos, pos, cumul, last_e, Kpv, Kiv, Kdv, vParam, l, vy1,
m1, cumulAlpha, pAlphaE, Kph, Kih, Kdh, vx1, cumulBeta, pBetaE,
cumulAlphaPos, cumulBetaPos, sp, Kph_pos1, Kih_pos1, Kdh_pos1,
Kph_pos0, Kih_pos0, Kdh_pos0, psp2, psp1, Kpr, Kdr, euler,
prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1,
cumulEULER, vx, vy, pos, sp, euler)
particlesTargetVelocities2, cumul2, last_e2, cumulAlpha2, pAlphaE2, cumulBeta2, pBetaE2, cumulAlphaPos2, cumulBetaPos2, psp22, psp12, prevEuler2, cumulPOS2, cumulVY2, cumulVX2, cumulSP02, cumulSP12, cumulEULER2 = main_PID_controller.PID(
targetPos2, pos2, cumul2, last_e2, Kpv, Kiv, Kdv, vParam, l2,
vy2, m2, cumulAlpha2, pAlphaE2, Kph, Kih, Kdh, vx2, cumulBeta2,
pBetaE2, cumulAlphaPos2, cumulBetaPos2, sp2, Kph_pos1,
Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0, psp22, psp12,
Kpr, Kdr, euler2, prevEuler2, cumulPOS2, cumulVY2, cumulVX2,
cumulSP02, cumulSP12, cumulEULER2, vx, vy, pos, sp, euler)
particlesTargetVelocities_s = [
particlesTargetVelocities[0], particlesTargetVelocities[1],
particlesTargetVelocities[2], particlesTargetVelocities[3],
particlesTargetVelocities2[0], particlesTargetVelocities2[1],
particlesTargetVelocities2[2], particlesTargetVelocities2[3]
]
""" =========================================================== """
""" =========================================================== """
""" ========== PLOTTING: ========== """
## PLOTTING:
# ze.append(targetPos[2]-pos[2]) # otstapuvanje od z-oska
# xe.append(sp[0]) # otstapuvanje od x-oska
# ye.append(sp[1]) # otstapuvawe od y-oska
# xs.append(targetPos[0])
# ys.append(targetPos[1])
# zs.append(targetPos[2])
# x_qc.append(pos[0])
# y_qc.append(pos[1])
# z_qc.append(pos[2])
# x_qc2.append(pos2[0])
# y_qc2.append(pos2[1])
# z_qc2.append(pos2[2])
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')
# ax.plot_wireframe(x_qc,y_qc,z_qc,color='blue')
# ax.plot_wireframe(x_qc2,y_qc2,z_qc2,color='red')
# ax.set_xlim3d(-2.1,2.1)
# ax.set_ylim3d(-2.1,2.1)
# ax.set_zlim3d(0,1.9)
# plt.show()
# u.append(thrust)
''' !!! treba da se nacrta i pozicijata i da se sporedi so i bez consensus !!! '''
xl.append(pos[0])
xf.append(pos2[0])
yl.append(pos[1])
yf.append(pos2[1])
zl.append(pos[2])
zf.append(pos2[2])
vxl.append(vx1[2])
vxf.append(vx2[2])
vyl.append(vy1[2])
vyf.append(vy2[2])
euler_l.append(euler[2])
euler_f.append(euler2[2])
v1.append(particlesTargetVelocities[0])
v2.append(particlesTargetVelocities[1])
v3.append(particlesTargetVelocities[2])
v4.append(particlesTargetVelocities[3])
v12.append(particlesTargetVelocities2[0])
v22.append(particlesTargetVelocities2[1])
v32.append(particlesTargetVelocities2[2])
v42.append(particlesTargetVelocities2[3])
plt.plot(yl, color='blue')
plt.hold(True)
plt.plot(yf, color='red')
# plt.plot(euler_l,color='blue')
# plt.plot(euler_f,color='red')
# plt.plot(yl,color='green')
# plt.hold(True)
# plt.plot(yf,color='pink')
# plt.hold(True)
# plt.plot(v3,color='green')
# plt.hold(True)
# plt.plot(v4,color='pink')
plt.hold(True)
plt.axis([0, 149, -3, 3])
plt.show()
# print " Zaxis error between drones: ", pos[2]-pos2[2]
ze.append(pos[2] - pos2[2])
# ze.append(abs(euler[2])-abs(euler2[2]))
plt.plot(ze, color='green')
plt.axis([0, 149, -3, 3])
plt.show()
""" =========================================================== """
""" ========== Gradient descent: ========== """
# sum_h_alpha.append(cumulAlpha)
# sum_h_beta.append(cumulBeta)
# sum_h_pos1.append(cumulAlphaPos)
# sum_h_pos0.append(cumulBetaPos)
# J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt)
# J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt)
# J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt)
# J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt)
# paramX.append(targetPos[0])
# paramY.append(targetPos[1])
# print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
# print "theta_h_angles : ", theta_h_angles
# print "theta_h_pos : ", theta_h_pos
#
# print "sum_h_alpha : ", sum_h_alpha[gd]
# print "sum_h_beta : ", sum_h_beta[gd]
# print "sum_h_pos1 : ", sum_h_pos1[gd]
# print "sum_h_pos0 : ", sum_h_pos0[gd]
# print "J_h_alpha : ", J_h_alpha[gd]
# print "J_h_beta : ", J_h_beta[gd]
# print "J_h_pos1 : ", J_h_pos1[gd]
# print "J_h_pos0 : ", J_h_pos0[gd]
# print "paramX : ", paramX[gd]
# print "paramY : ", paramY[gd]
# print "cumulPOS : ", cumulPOS2
# print "cumulVY : ", cumulVY2
# print "cumulVX : ", cumulVX2
# print "cumulSP0 : ", cumulSP02
# print "cumulSP1 : ", cumulSP12
# print "cumulEULER : ", cumulEULER2
# print "cumul : ", cumul
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
gd = gd + 1
print "gd (iterations) : ", gd
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
""" =========================================================== """
if gd == 150:
# with open('GD_43.csv','wb') as f1:
# writer=csv.writer(f1)
# for i in range(0, gd):
# writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]])
break
# """ =========================================================== """
""" ========== WRITE TO TEXT: ========== """
## WRITE TO TEXT:
# log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i)
# i +=1
""" =========================================================== """
# send propeller velocities to output:
[res, retInts, retFloats, retStrings,
retBuffer] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_propeller_v', [], particlesTargetVelocities_s, [],
emptyBuff, vrep.simx_opmode_blocking)
# [res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter2,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities2,[],emptyBuff,vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
""" =========================================================== """
print " Zaxis error between drones: ", ze
# stop the simulation:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
pshoulder = vrep.simxGetJointPosition(clientID, shoulderHandle,
opmode)
print("Motors reached positions: " + str(ashoulder) + " " +
str(aelbow) + " " + str(awrist))
# Get the robot position after the movement sequence
pret, robotPos = vrep.simxGetObjectPosition(
clientID, robotHandle, -1, vrep.simx_opmode_buffer)
print ("2w1a position: (x = " + str(robotPos[0]) +\
", y = " + str(robotPos[1]) + ")")
# Get the robot orientation after the movement sequence
oret, robotOrient = vrep.simxGetObjectOrientation(
clientID, robotHandle, -1, vrep.simx_opmode_buffer)
print ("2w1a orientation: (x = " + str(robotOrient[0]) +\
", y = " + str(robotOrient[1]) +\
", z = " + str(robotOrient[2]) + ")")
# End the simulation, wait to be sure V-REP had the time to stop it entirely
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStopSimulation
vrep.simxStopSimulation(clientID, opmode)
time.sleep(1)
print("----- Simulation ended -----")
# Close the connection to V-REP remote server
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxFinish
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('End')
def reset_simulation(clientID):
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
time.sleep(1) # um pequeno sleep entre o stop e o start
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
printlog('simxSetJointTargetVelocity', returnCode)
elif c == 'i':
returnCode = vrep.simxSetJointTargetVelocity(
clientID, jointHandles[0], vel, vrep.simx_opmode_blocking)
printlog('simxSetJointTargetVelocity', returnCode)
elif c == 'p':
returnCode = vrep.simxSetJointTargetVelocity(
clientID, jointHandles[0], -vel, vrep.simx_opmode_blocking)
printlog('simxSetJointTargetVelocity', returnCode)
elif c == 'o':
returnCode = vrep.simxSetJointTargetVelocity(
clientID, jointHandles[0], 0, vrep.simx_opmode_blocking)
printlog('simxSetJointTargetVelocity', returnCode)
elif c == 'z':
# stop simulation
returnCode = vrep.simxStopSimulation(clientID,
vrep.simx_opmode_blocking)
printlog('simxStopSimulation', returnCode)
running = True
while running:
returnCode, ping = vrep.simxGetPingTime(clientID)
returnCode, serverState = vrep.simxGetInMessageInfo(
clientID, vrep.simx_headeroffset_server_state)
running = serverState
# Start simulation
returnCode = vrep.simxStartSimulation(clientID,
vrep.simx_opmode_blocking)
printlog('simxStartSimulation', returnCode)
elif c == 'q':
break
def stop(self):
vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
sumo = Sumo(agent_1, agent_2)
# If reaches max time, quit
sumo_maxtime = 120
update_freq = 0.05
max_update_count = int(sumo_maxtime / update_freq)
winner = -1
# Run match
for count in range(max_update_count):
# Update all agents
sumo.update()
# Check winner
winner = sumo.getWinner()
if winner != -1:
if winner == 1:
print("Robot 1 wins")
elif winner == 2:
print("Robot 2 wins")
break
# Wait for program to update
time.sleep(update_freq)
# Tie
if winner == -1:
print("Tie")
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(-1)
errorCode, eulerAngles=vrep.simxGetObjectOrientation(clientID,car_handle,-1,vrep.simx_opmode_streaming)
while trainCount < train_frames:
errorCode, eulerAngles=vrep.simxGetObjectOrientation(clientID,car_handle,-1,vrep.simx_opmode_buffer)
print (eulerAngles)
trainCount += 1
print(trainCount)
#Collision handler
errorCode, collisionState1=vrep.simxReadCollision(clientID,collision_handle1,vrep.simx_opmode_buffer)
print(collisionState1)
print(maxroute)
if abs(eulerAngles[0])>2:
returnCode=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot)
time.sleep(5)
reward = PUNISH
flipCount +=1
returnCode=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
if collisionState1 == 1 or min(proximitysensor) <=1:
#returnCode=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot)
#clientID = tv.Initialize()
#returnCode=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
if maxroute < driveCount and trainCount > observe:
maxroute = driveCount
# Log the car's distance at this T.
data_collect.append([trainCount, maxroute])
driveCount = 0
def close(self):
gym.Env.close(self)
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
os.system("TASKKILL /F /IM vrep.exe")
vrep.simxSetJointForce(clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# move simulation ahead one time step
vrep.simxSynchronousTrigger(clientID)
count += dt
else:
raise Exception('Failed connecting to remote API server')
except Exception, e:
print e
finally:
# stop the simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Before closing the connection to V-REP,
# make sure that the last command sent out had time to arrive.
vrep.simxGetPingTime(clientID)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
print 'connection closed...'
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
track_hand = np.array(track_hand)
track_target = np.array(track_target)
def __del__(self):
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking)
def JointControl(clientID, motionProxy, postureProxy, i, Body):
# Head
# velocity_x = vrep.simxGetObjectFloatParameter(clientID,vrep.simxGetObjectHandle(clientID,'NAO#',vrep.simx_opmode_oneshot_wait)[1],11,vrep.simx_opmode_streaming)
pos = vrep.simxGetObjectPosition(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
angularPos = vrep.simxGetObjectOrientation(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
alphaPosRegister = []
betaPosRegister = []
gamaPosRegister = []
j = 0
li = []
while vrep.simxGetConnectionId(clientID) != -1:
for x in range(0, 150):
commandAngles = motionProxy.getAngles('Body', False)
j+=1
pos = vrep.simxGetObjectPosition(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
angularPos = vrep.simxGetObjectOrientation(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
alphaPosRegister.append(angularPos[0])
betaPosRegister.append(angularPos[1])
gamaPosRegister.append(angularPos[2])
vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
# Left Leg
vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming)
# Right Leg
vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming)
# Left Arm
vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming)
# Right Arm
vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming)
# Left Fingers
vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming)
# Right Fingers
vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
motionProxy.stopMove()
postureProxy.stopMove()
# postureProxy.goToPosture("Stand",0.5)
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
break
print 'End of simulation'
print 'Final x pos'
print pos
calculos = {}
calcular(alphaPosRegister,calculos)
desvpadA = calculos['desvio_padrao']
calculos = {}
calcular(betaPosRegister,calculos)
desvpadB = calculos['desvio_padrao']
calculos = {}
calcular(gamaPosRegister,calculos)
desvpadG = calculos['desvio_padrao']
return [(10*pos[0] - (desvpadA + desvpadB + desvpadG)/3), pos[0]]
