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 = [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 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 start_sim(self):
time.sleep(self.sleep_sec_min)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
self.gripper_enabled = False
self.__update_all_object_positions()
self.cylinder_height = self.get_object_height(self.cylinder_handle)
self.cylinder_z_locus = self.get_position(self.cylinder_handle)[2]
self.bin_position = self.get_position(self.bin_handle)
def connect(self):
global SYNC
self.cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP
if self.cid != -1:
print ('Connected to V-REP remote API serv'
'\er, client id: %s' % self.cid)
vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot)
if SYNC:
vrep.simxSynchronous(self.cid, True)
else:
print ('Failed connecting to V-REP remote API server')
exit()
def startsim(self):
"""
Start the simulation in synchronous mode to achieve exact simulation
independent of the framerate. To be able to do this, you have to
connec to the port 19997 of vrep, edit the file
`remoteApiConnections.txt` to contain something like
portIndex1_port = 19997
portIndex1_debug = true
portIndex1_syncSimTrigger = false
"""
self.set_led(8,0)
vrep.simxStartSimulation(self._clientID, vrep.simx_opmode_oneshot)
vrep.simxSynchronous(self._clientID, True)
self.stepsim(1)
def wait_until_simulator_started(clientID):
while True:
try:
if vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait) == vrep.simx_return_ok:
break
except KeyboardInterrupt:
sys.exit('Program Ended')
def __init__(self, sim_dt=0.05, nengo_dt=0.001, sync=True):
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
self.sync = sync
if self.cid != -1:
print ('Connected to V-REP remote API server, client id: %s' % self.cid)
vrep.simxStartSimulation( self.cid, vrep.simx_opmode_oneshot )
if self.sync:
vrep.simxSynchronous( self.cid, True )
else:
print ('Failed connecting to V-REP remote API server')
exit(1)
self.count = 0
self.sim_dt = sim_dt
self.nengo_dt = nengo_dt
def startSim(self, clientID, screen=True):
vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_dynamic_engine,bulletEngine ,vrep.simx_opmode_oneshot_wait)
vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, simulatioTimeStepDT, vrep.simx_opmode_oneshot_wait)
error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
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)
return error
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 run(self):
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
t0 = time.time()
while time.time() - t0 < 30:
for bot in self.bots:
bot.robotCode()
def connection(self):
"""
Make connection with v-rep simulator
"""
print ('Waiting for connection...')
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',self.port,True,True,5000,5) # Connect to V-REP
if self.clientID!=-1:
print ('Connected to remote API server')
# enable the synchronous mode on the client:
if self.synchronous:
vrep.simxSynchronous(self.clientID,True)
# start the simulation:
vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
else:
raise IOError('Failed connecting to remote API server')
def __init__(self):
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(0.5)
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # tenta conectar no simulador, se salva o clientID
# paramos a simulacao, se estiver em andamento, e comecamos denovo
# vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
#time.sleep(0.5)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# modo da API, como eh False, esta no modo assincrono(os ticks da simulacao rodam em velocidade independente)
vrep.simxSynchronous(self.clientID, False)
print("connected with id ", self.clientID)
self.oldtarget = None
self.camera = None
self.setup()
self.lastimageAcquisitionTime = 0
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 run(self):
if self.clientID!=-1:
if not self.multirobots:
# Start simulation if MultiRobotRunner not already starting it
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
self.robotCode()
if not self.multirobots:
# Stop VREP simulation cleanly if MultiRobotRunner not already stopping it
self.clean_exit()
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 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 start(self):
returnCode = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
if (returnCode>1):
print "returnCode: ", returnCode
raise Exception('Could not start')
returnCode=vrep.simxSynchronous(self.clientID,True)
for k in range(10): #Run to steps to step through initial "drop"
returncode = vrep.simxSynchronousTrigger(self.clientID)
if (returnCode!=0):
raise Exception('Could not set synchronous mode')
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 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 VREPConnect(self):
vrep.simxFinish(-1)
"Connect to Simulation"
self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5)
eCode = vrep.simxSynchronous(self.simulID, True)
if eCode != 0:
print "Could not get V-REP to synchronize operation with me"
if not self.simulID == -1:
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
print "my SimulID is ", self.simulID
else:
sys.exit("Failed to connect to VREP simulation. Bailing out")
def run(self):
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
# in zone passer, it was
# get to the 3 starting positions
# while game is still going...
# do things in a while loop
while True:
t=time.time()
for bot in self.bots:
bot.robotCode()
print 'loop time'
print time.time()-t
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 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 run(self):
""" Run method is a Cyclic Exectuor.
robots that are added with `addRobot` should have method
`robotCode` which returns in a small amount of time
If you desire high level planner control, use the structure
as a template.
"""
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
t0 = time.time()
while True:
for bot in self.bots:
bot.robotCode()
self.ballEngine.update()
self.clean_exit()
def connect(self):
#os.chdir(VREP_HOME)
#subprocess.call([os.path.join(VREP_HOME,'vrep.sh'), VREP_scene_file], shell = True, cwd = VREP_HOME)
"Close existing connections"
vrep.simxFinish(-1)
"Connect to Simulation"
self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5)
eCode = vrep.simxSynchronous(self.simulID, True)
if eCode != 0:
print "Could not get V-REP to synchronize operation with me"
if not self.simulID == -1:
eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self.simulID)
print "my SimulID is ", self.simulID
else:
sys.exit("Failed to connect to VREP simulation. Bailing out")
def run(self):
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
# Create threads for each
self.threads = []
for bot in self.bots:
# Robot Thread
t = threading.Thread(
name="%sThread" % bot.bot_name
, target=bot.robotCode)
t.setDaemon(True) # Require it to join!
self.threads.append(t)
t.start()
print("Started robotCode for %s" % bot.bot_name)
# Require threads to join before exiting
for t in self.threads:
t.join()
self.clean_exit()
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 __init__(self,port_number):
vrep.simxFinish(-1)
self.clientID = vrep.simxStart('127.0.0.1', port_number, True, True, 5000, 5)
# start simulation
rc = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
# object handles
res, self.quad_obj = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter', vrep.simx_opmode_oneshot_wait)
res, self.camera = vrep.simxGetObjectHandle(self.clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait)
res, self.target = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter_target', vrep.simx_opmode_oneshot_wait)
# Initialise data streaming from V-REP
err, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.camera, 0, vrep.simx_opmode_streaming)
_,pos = vrep.simxGetObjectPosition(self.clientID, self.quad_obj, -1, vrep.simx_opmode_streaming)
_,target_pos = vrep.simxGetObjectPosition(self.clientID, self.target, -1, vrep.simx_opmode_streaming)
time.sleep(2)
# Variables
_,self.last_pos = vrep.simxGetObjectPosition(self.clientID, self.quad_obj, -1, vrep.simx_opmode_buffer)
def callback(msg):
global count
global dt
global _
if clientID != -1 and count == 0: # if we connected successfully
print('Connected to remote API server')
# --------------------- Setup the simulation
vrep.simxSynchronous(clientID, True)
global joint_names
# joint target velocities discussed below
global joint_target_velocities
joint_target_velocities = np.ones(len(joint_names)) * 10000.0
global joint_handles
# get the handles for each joint and set up streaming
joint_handles = [
vrep.simxGetObjectHandle(clientID, name,
vrep.simx_opmode_blocking)[1]
for name in joint_names
]
# get handle for target and set up streaming
global target_handle
_, target_handle = vrep.simxGetObjectHandle(clientID, 'target',
vrep.simx_opmode_blocking)
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# --------------------- Start the simulation
# start our simulation in lockstep with our code
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
print("Starting Simulation")
if clientID != -1 and count >= 0 and count < 6: # run for 1 simulated second
target_xyz = [msg.x, msg.y, msg.z]
# store for plotting
global track_target
track_target.append(np.copy(target_xyz))
# get the (x,y,z) position of the target
vrep.simxSetObjectPosition(clientID, target_handle, -1, target_xyz,
vrep.simx_opmode_blocking)
if _ != 0: raise Exception("Deu ruim parceiro")
target_xyz = np.asarray(target_xyz)
q = np.zeros(len(joint_handles))
dq = np.zeros(len(joint_handles))
for ii, joint_handle in enumerate(joint_handles):
# get the joint angles
_, q[ii] = vrep.simxGetJointPosition(clientID, joint_handle,
vrep.simx_opmode_blocking)
if _ != 0: raise Exception()
# get the joint velocity
_, dq[ii] = vrep.simxGetObjectFloatParameter(
clientID,
joint_handle,
2012, # parameter ID for angular velocity of the joint
vrep.simx_opmode_blocking)
if _ != 0: raise Exception()
L = np.array([0, .42, .225]) # arm segment lengths
xyz = np.array([L[0]*np.cos(q[0])+L[1]*np.cos(q[0])*np.cos(q[1])+L[2]*np.cos(q[0])*np.cos(q[1])*np.cos(q[2])- \
L[2]*np.cos(q[0])*np.sin(q[1])*np.sin(q[2]),
L[0]*np.cos(q[0])+L[1]*np.cos(q[1])*np.sin(q[0])+L[2]*np.sin(q[0])*np.cos(q[1])*np.cos(q[2])- \
L[2]*np.sin(q[0])*np.sin(q[1])*np.sin(q[2]),
# have to add .1 offset to z position
L[1]*np.sin(q[1])+L[2]*(np.cos(q[1])*np.sin(q[2])+np.cos(q[2])*np.sin(q[1]))+.14])
global track_hand
track_hand.append(np.copy(xyz)) #store for plotting
# calculate the Jacobian for the hand
JEE = np.zeros((3, 3))
JEE[0, 2] = -L[2] * np.sin(q[1] + q[2]) * np.cos(q[0])
JEE[1, 2] = -L[2] * np.sin(q[1] + q[2]) * np.sin(q[0])
JEE[2, 2] = L[2] * np.cos(q[1] + q[2])
JEE[0, 1] = -np.cos(q[0]) * (L[2] * np.sin(q[1] + q[2]) +
L[1] * np.sin(q[1]))
JEE[1, 1] = -np.sin(q[0]) * (L[2] * np.sin(q[1] + q[2]) +
L[1] * np.sin(q[1]))
JEE[2, 1] = L[2] * np.cos(q[1] + q[2]) + L[1] * np.cos(q[1])
JEE[0, 0] = -np.sin(q[0]) * (L[0] + JEE[2][1])
JEE[1, 0] = np.cos(q[0]) * (L[0] + JEE[2][1])
# get the Jacobians for the centres-of-mass for the arm segments
JCOM1 = np.zeros((6, 3))
JCOM1[0, 0] = L[0] * -np.sin(q[0]) / 2
JCOM1[1, 0] = L[0] * np.cos(q[0]) / 2
JCOM1[5, 0] = 1.0
JCOM2 = np.zeros((6, 3))
JCOM2[0, 1] = L[1] * (np.cos(q[1]) * np.sin(q[0]) -
np.cos(q[0]) * np.sin(q[1])) / 2
JCOM2[1, 1] = L[1] * (-np.cos(q[1]) * np.cos(q[0]) -
np.sin(q[1]) * np.sin(q[0])) / 2
JCOM2[4, 1] = 1.0
JCOM2[0, 0] = L[1] * (np.cos(q[0]) * np.sin(q[1]) - np.cos(q[1]) *
np.sin(q[0])) / 2 - L[0] * np.sin(q[0])
JCOM2[1,
0] = L[1] * (np.sin(q[0]) * np.sin(q[1]) +
np.cos(q[0]) * np.cos(q[1])) / 2 + 2 * JCOM1[1][0]
JCOM2[4, 0] = 1.0
JCOM3 = np.zeros((6, 3))
JCOM3[0, 2] = L[2] * (np.cos(q[2]) * np.sin(q[0]) -
np.cos(q[0]) * np.cos(q[1]) * np.sin(q[2])) / 2
JCOM3[1, 2] = L[2] * (-np.cos(q[2]) * np.cos(q[0]) -
np.cos(q[1]) * np.sin(q[0]) * np.sin(q[2])) / 2
JCOM3[2, 2] = -L[2] * np.sin(q[1]) * np.sin(q[2]) / 2
JCOM3[4, 2] = 1.0
JCOM3[0, 1] = L[2] * -np.cos(q[0]) * np.cos(q[2]) * np.sin(
q[1]) / 2 - L[1] * np.cos(q[0]) * np.sin(q[1])
JCOM3[1, 1] = L[2] * -np.cos(q[2]) * np.sin(q[0]) * np.sin(
q[1]) / 2 - L[1] * np.sin(q[0]) * np.sin(q[1])
JCOM3[2, 1] = (L[2] * np.cos(q[1]) * np.cos(q[2]) /
2) + L[1] * np.cos(q[1])
JCOM3[4, 1] = 1.0
JCOM3[0,0] = L[2]*(np.cos(q[0])*np.sin(q[2]) - np.cos(q[1])*np.cos(q[2])*np.sin(q[0]))/2 -L[1]*np.cos(q[1])*np.sin(q[0])- \
L[0]*np.sin(q[0])
JCOM3[1,0] = L[2]*(np.sin(q[0])*np.sin(q[2]) + np.cos(q[0])*np.cos(q[1])*np.cos(q[2]))/2 + \
L[1]*np.cos(q[0])*np.cos(q[1])+2*JCOM1[1][0]
JCOM3[4, 0] = 1.0
m1 = 0.15 #from VREP
M1 = np.diag([m1, m1, m1, .001, .001, .001])
m2 = 1.171 # from VREP
M2 = np.diag([m1, m1, m1, .008, .008, .001])
m3 = .329 # from VREP
M3 = np.diag([m2, m2, m2, 5e-4, 5e-4, 1e-4])
# generate the mass matrix in joint space
Mq = np.dot(JCOM1.T, np.dot(M1, JCOM1)) + \
np.dot(JCOM2.T, np.dot(M2, JCOM2)) + \
np.dot(JCOM3.T, np.dot(M3, JCOM3))
# compensate for gravity
gravity = np.array([
0,
0,
-9.81,
0,
0,
0,
])
Mq_g = np.dot(JCOM1.T, np.dot(M1, gravity)) + \
np.dot(JCOM2.T, np.dot(M2, gravity)) + \
np.dot(JCOM2.T, np.dot(M2, gravity))
Mx_inv = np.dot(JEE, np.dot(np.linalg.inv(Mq), JEE.T))
Mu, Ms, Mv = np.linalg.svd(Mx_inv)
# cut off any np.singular values that could cause control problems
for i in range(len(Ms)):
Ms[i] = 0 if Ms[i] < 1e-5 else 1. / float(Ms[i])
# numpy returns U,S,V.T, so have to transpose both here
Mx = np.dot(Mv.T, np.dot(np.diag(Ms), Mu.T))
# calculate desired movement in operational (hand) space
kp = 100
kv = np.sqrt(kp)
u_xyz = np.dot(Mx, kp * (target_xyz - xyz))
u = np.dot(JEE.T, u_xyz) - np.dot(Mq, kv * dq) - Mq_g
u *= -1 # because the joints on the arm are backwards
for ii, joint_handle in enumerate(joint_handles):
# the way we're going to do force control is by setting
# the target velocities of each joint super high and then
# controlling the max torque allowed (yeah, i know)
# get the current joint torque
_, torque = \
vrep.simxGetJointForce(clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ != 0: raise Exception()
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) < 0:
joint_target_velocities[ii] = \
joint_target_velocities[ii] * -1
vrep.simxSetJointTargetVelocity(
clientID,
joint_handle,
joint_target_velocities[ii], # target velocity
vrep.simx_opmode_blocking)
if _ != 0: raise Exception()
# and now modulate the force
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
print("Cheguei aqui uma vez")
elif clientID != -1 and count >= 6:
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)
else:
raise Exception('Failed connecting to remote API server')
return
def startSimulation(self):
# make sure simulation is stopped, stop if needed
self.stop_if_needed()
vrep.simxSynchronous(self.clientID, self.sync_mode)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
def main():
print("Enter the start point x-coordinate(in cm) --> ")
x_s = int(input())
print("Enter the start point y-coordinate --> ")
y_s = int(input())
print("Enter the goal point x-coordinate --> ")
x_g = int(input())
print("Enter the goal point y-coordinate --> ")
y_g = int(input())
# close opened connections
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# check for successful connection
if clientID != -1:
print('Connected to remote API server')
else:
print('Connection not successful')
sys.exit('Could not connect')
# retrieve motor handles
errorCode, left_motor_handle = vrep.simxGetObjectHandle(
clientID, 'wheel_left_joint', vrep.simx_opmode_oneshot_wait)
errorCode, right_motor_handle = vrep.simxGetObjectHandle(
clientID, 'wheel_right_joint', vrep.simx_opmode_oneshot_wait)
errorCode, handle = vrep.simxGetObjectHandle(clientID, 'Turtlebot2',
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, handle, -1, [((x_s / 100) - 5.1870),
((y_s / 100) - 4.8440),
(0.06)],
vrep.simx_opmode_oneshot_wait)
# print(handle)
emptyBuff = bytearray()
simulation_points = a_star((x_s, y_s), (x_g, y_g))
# start the simulation -->
init = time.time()
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
for i in range(0, (len(simulation_points))):
errorcode, position = vrep.simxGetObjectPosition(
clientID, 189, -1, vrep.simx_opmode_streaming)
# X_Point = (((simulation_points[i][0]) / (100)) - (5.1870))
# Y_Point = (((simulation_points[i][1]) / (100)) - (4.8440))
ul = simulation_points[i][2]
ur = simulation_points[i][3]
errorCode = vrep.simxSetJointTargetVelocity(clientID,
left_motor_handle, ul,
vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(clientID,
right_motor_handle, ur,
vrep.simx_opmode_streaming)
# setting the sampling time for simulation. It will be the same as sampling time used in create_neighbors function
time.sleep(3.8)
# print("Velocities --> ", ur, ul)
ul = 0
ur = 0
errorCode = vrep.simxSetJointTargetVelocity(clientID, left_motor_handle,
ul, vrep.simx_opmode_streaming)
errorCode = vrep.simxSetJointTargetVelocity(clientID, right_motor_handle,
ur, vrep.simx_opmode_streaming)
final = time.time()
print("time taken for simulation --> ", final - init)
def init_client(self):
self.logger = logging.getLogger("learner")
# Stop any previous simulation
vrep.simxStopSimulation(-1, vrep.simx_opmode_oneshot_wait)
# Start a client
self.client_id = vrep.simxStart(self.server_ip, self.server_port, True,
True, 5000, 5)
if self.client_id == -1:
self.logger.critical('Failed connecting to remote API server')
self.logger.critical('EXIT')
thread.exit()
# Enable synchronous mode
e = vrep.simxSynchronous(self.client_id, True)
self.logger.info("simxSynchronous=%d" % e)
if e != 0:
self.logger.critical('Failed enabling remote API synchronous mode')
self.logger.critical('EXIT')
thread.exit()
# Start simulation
e = vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_blocking)
self.logger.info("simxStartSimulation=%d" % e)
if e != 0:
self.logger.critical('Failed to start simulation')
self.logger.critical('EXIT')
thread.exit()
# Print ping time
sec, msec = vrep.simxGetPingTime(self.client_id)
self.logger.info("Started simulation on %s:%d" %
(self.server_ip, self.server_port))
self.logger.info("Ping time: %f" % (sec + msec / 1000.0))
# Obtain handle for body (for orientation, positions etc)
_, self.body_handle = vrep.simxGetObjectHandle(
self.client_id, 'Ant_body', vrep.simx_opmode_blocking)
# Obtain joint handles
_, Ant_joint1Leg1 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint1Leg1',
vrep.simx_opmode_blocking)
_, Ant_joint2Leg1 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint2Leg1',
vrep.simx_opmode_blocking)
_, Ant_joint3Leg1 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint3Leg1',
vrep.simx_opmode_blocking)
_, Ant_joint1Leg2 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint1Leg2',
vrep.simx_opmode_blocking)
_, Ant_joint2Leg2 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint2Leg2',
vrep.simx_opmode_blocking)
_, Ant_joint3Leg2 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint3Leg2',
vrep.simx_opmode_blocking)
_, Ant_joint1Leg3 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint1Leg3',
vrep.simx_opmode_blocking)
_, Ant_joint2Leg3 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint2Leg3',
vrep.simx_opmode_blocking)
_, Ant_joint3Leg3 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint3Leg3',
vrep.simx_opmode_blocking)
_, Ant_joint1Leg4 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint1Leg4',
vrep.simx_opmode_blocking)
_, Ant_joint2Leg4 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint2Leg4',
vrep.simx_opmode_blocking)
_, Ant_joint3Leg4 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint3Leg4',
vrep.simx_opmode_blocking)
_, Ant_joint1Leg5 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint1Leg5',
vrep.simx_opmode_blocking)
_, Ant_joint2Leg5 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint2Leg5',
vrep.simx_opmode_blocking)
_, Ant_joint3Leg5 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint3Leg5',
vrep.simx_opmode_blocking)
_, Ant_joint1Leg6 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint1Leg6',
vrep.simx_opmode_blocking)
_, Ant_joint2Leg6 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint2Leg6',
vrep.simx_opmode_blocking)
_, Ant_joint3Leg6 = vrep.simxGetObjectHandle(self.client_id,
'Ant_joint3Leg6',
vrep.simx_opmode_blocking)
# Obtain body joint handles
_, Ant_neckJoint1 = vrep.simxGetObjectHandle(self.client_id,
'Ant_neckJoint1',
vrep.simx_opmode_blocking)
_, Ant_neckJoint2 = vrep.simxGetObjectHandle(self.client_id,
'Ant_neckJoint2',
vrep.simx_opmode_blocking)
_, Ant_neckJoint3 = vrep.simxGetObjectHandle(self.client_id,
'Ant_neckJoint3',
vrep.simx_opmode_blocking)
_, Ant_leftJawJoint = vrep.simxGetObjectHandle(
self.client_id, 'Ant_leftJawJoint', vrep.simx_opmode_blocking)
_, Ant_rightJawJoint = vrep.simxGetObjectHandle(
self.client_id, 'Ant_rightJawJoint', vrep.simx_opmode_blocking)
# Order : [Ant_joint{1-3}Leg{1-6}, Ant_neckJoint{1-3}, Ant_leftJawJoint, Ant_rightJawJoint]
self.handles = [
Ant_joint1Leg1, Ant_joint2Leg1, Ant_joint3Leg1, Ant_joint1Leg2,
Ant_joint2Leg2, Ant_joint3Leg2, Ant_joint1Leg3, Ant_joint2Leg3,
Ant_joint3Leg3, Ant_joint1Leg4, Ant_joint2Leg4, Ant_joint3Leg4,
Ant_joint1Leg5, Ant_joint2Leg5, Ant_joint3Leg5, Ant_joint1Leg6,
Ant_joint2Leg6, Ant_joint3Leg6, Ant_neckJoint1, Ant_neckJoint2,
Ant_neckJoint3, Ant_leftJawJoint, Ant_rightJawJoint
]
self.joint_count = len(self.handles)
self.signal_values()
self.wait_till_stream()
# log these for consistency
self.start_pos = self.get_position()
self.start_orientation = self.get_orientation()
def reset(clientID):
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(0.1)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
def start(self):
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
def quadcopter_fcn():
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('')
import time
import numpy as np
# output limits:
#min_output=0
#max_output=8.335
# program parameters:
#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
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
Kih = 0.1
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
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]
# 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)
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
]
print '////////////'
print '------------'
print '-Controller parameters-'
print 'Vertical control parameters=', [Kpv, Kiv, Kdv]
print 'Horizontal control parameters=', [
Kph, Kih, Kdh, Kph_pos0, Kdh_pos0, Kph_pos1, Kdh_pos1
]
print 'Rotational control parameters=', [Kpr, Kir, Kdr]
print '------------'
print '-Vertical component-'
print 'targetPos=', targetPos
print 'pos=', pos
print 'e=', e
print 'thrust=', thrust
print '------------'
print '-Horizontal component-'
print 'cumulAlpha=', cumulAlpha
print 'cumulBeta=', cumulBeta
print 'cumulAlphaPos=', cumulAlphaPos
print 'cumulBetaPos=', cumulBetaPos
print 'alphaE=', alphaE
print 'betaE=', betaE
print 'alphaCorr=', alphaCorr
print 'betaCorr=', betaCorr
print 'orientation=', orientation
print 'sp_X=', sp[0]
print 'sp_Y=', sp[1]
print '------------'
print '-Rotational component-'
print 'vx=', vx
print 'vy=', vy
print 'gamma=', euler[2]
print 'rotCorr=', rotCorr
print '------------'
print 'Velocity_propeller_1 = ', particlesTargetVelocities[0]
print 'Velocity_propeller_2 = ', particlesTargetVelocities[1]
print 'Velocity_propeller_3 = ', particlesTargetVelocities[2]
print 'Velocity_propeller_4 = ', particlesTargetVelocities[3]
print '------------'
print '////////////'
# 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 start_simulation(self):
#start the simulation
errorCode = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_blocking)
assert errorCode == 0, "Simulation could not be started"
return
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")
def evoluer(self,clientID, mot1, mot2, cores):
"""Cette fonction evoluera la population à la génération suivante
Les étapes suivantes sont faites:
1-On commence par supprimer tous les individus des éspèces sauf le meilleur
2-On sépare la population en espèces
3-On vide le contenu et on garde la dérnière génération
4-On reproduit les espèces"""
if self.generationCount>0:
indivAl = self.tournamentSelection(self.contenu)
self.contenu = []
else:
indivAl = ut.randomPick(self.contenu)
for e in self.especes:
tailleProgeniture = 1+int(floor(self.length*e.averageFitness()/self.averageFitness))
for i in range(tailleProgeniture):
if len(self.contenu) < self.length:
if i == 0:
self.contenu.append(e.leader)
else:
if len(e.best) > 1:
par1, par2 = e.parents()
enfant = self.crossover(par1, par2)
else:
enfant = deepcopy(e.individu())
enfant.id = self.lastIndId
self.lastIndId += 1
enfant.mutationPoids()
if rand.random() < prob.mutation.connexion:
self.mutationConnexion(enfant)
if rand.random() < prob.mutation.noeud:
self.mutationNoeud(enfant)
self.contenu.append(enfant)
e.lastBestFitness = e.leader.fitness
assert self.length - len(self.contenu) <= 1, "La population est morte"
if self.length - len(self.contenu) == 1:
self.contenu.append(deepcopy(indivAl))
# for i in range(len(self.especes)):
# if e.age > 1:
# e = self.especes[i]
# if e.stagnated():
# e.stagnationAge += 1
# else:
# e.stagnationAge = 0
# if e.stagnationAge >= constants.speciation.stagnationAgeThresh:
# del self.especes[i]
for e in self.especes:
e.flush()
vrep.simxSynchronous(clientID, True)
err, simState = vrep.simxGetIntegerSignal(clientID, "simulationState", vrep.simx_opmode_streaming)
# while simState == 17:
# err, simState = vrep.simxGetIntegerSignal(clientID, "simulationState", vrep.simx_opmode_streaming)
err = vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)
assert err == 0
while vrep.simxGetConnectionId != -1 and vrep.simxGetLastCmdTime(clientID) < 1000:
for i in range(len(self.contenu)):
error, angle1 = vrep.simxGetJointPosition(clientID, mot1[i], vrep.simx_opmode_blocking)
error, angle2 = vrep.simxGetJointPosition(clientID, mot2[i], vrep.simx_opmode_blocking)
ind = self.contenu[i]
ind.phenotype.evaluate(ut.entree('1;'+str((angle1 % pi)/pi)+';'+str((angle2 % pi)/pi)))
out = ind.output()
mot1V = out[0]
mot2V = out[1]
vrep.simxSetJointTargetPosition(clientID, mot1[i], angle1 + 10*mot1V*1e-2, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, mot2[i], angle2 + 10*mot2V*1e-2, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(clientID)
for i in range(len(self.contenu)):
error, pos = vrep.simxGetObjectPosition(clientID, cores[i],-1, vrep.simx_opmode_blocking)
print(pos)
self.contenu[i].fitness = pos[0]
vrep.simxSetJointTargetPosition(clientID, mot1[i], 0, vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, mot2[i], 0, vrep.simx_opmode_oneshot)
print()
vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
self.updateEspeces()
# if len(self.especes) > constants.speciation.nbSpeciesTarget:
# constants.speciation.distThreshold += constants.speciation.distanceThresholdMod
# elif len(self.especes) < constants.speciation.nbSpeciesTarget:
# constants.speciation.distThreshold -= constants.speciation.distanceThresholdMod
for i in self.contenu:
i.phenotype.reinit()
for e in self.especes:
e.ajusterFitness()
e.calculateBest()
# self.updateBest()
self.updateAverageFitness()
self.generationCount += 1
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('')
import time
import numpy as np
from captains_log_v1 import log_data
import os
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
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
Kih = 0.1
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
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
## plot height:
# 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]
# plot vx, vy, x, y:
# 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)
rotCorr = Prot + Drot
prevEuler = euler[2]
## plot rot:
# 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
]
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:
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.xaxis.label.set_color('blue')
# ax.yaxis.label.set_color('blue')
# ax.zaxis.label.set_color('blue')
# ax.tick_params(axis='xs',colors='blue')
# ax.tick_params(axis='ys',colors='blue')
# ax.tick_params(axis='zs',colors='blue')
ax.plot_wireframe(xs, ys, zs, color='blue')
# plt.hold(True)
# ax.xaxis.label.set_color('red')
# ax.yaxis.label.set_color('red')
# ax.zaxis.label.set_color('red')
# ax.tick_params(axis='x_qc',colors='red')
# ax.tick_params(axis='y_qc',colors='red')
# ax.tick_params(axis='z_qc',colors='red')
ax.plot_wireframe(x_qc, y_qc, z_qc, color='red')
plt.show()
u.append(thrust)
v1.append(propeller1_PTV)
v2.append(propeller2_PTV)
v3.append(propeller3_PTV)
v4.append(propeller4_PTV)
plt.plot(v1, color='blue')
plt.hold(True)
plt.plot(v2, color='red')
plt.hold(True)
plt.plot(v3, 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, 100, -4, 4])
plt.show()
# print '////////////'
# print '------------'
# print '-Controller parameters-'
# print 'Vertical control parameters=',[Kpv,Kiv,Kdv]
# print 'Horizontal control parameters=',[Kph,Kih,Kdh,Kph_pos0,Kdh_pos0,Kph_pos1,Kdh_pos1]
# print 'Rotational control parameters=',[Kpr,Kir,Kdr]
# print '------------'
# print '-Vertical component-'
# print 'targetPos=',targetPos
# print 'pos=',pos
# print 'e=',e
# print 'thrust=',thrust
# print '------------'
# print '-Horizontal component-'
# print 'cumulAlpha=',cumulAlpha
# print 'cumulBeta=',cumulBeta
# print 'cumulAlphaPos=',cumulAlphaPos
# print 'cumulBetaPos=',cumulBetaPos
# print 'alphaE=',alphaE
# print 'betaE=',betaE
# print 'alphaCorr=',alphaCorr
# print 'betaCorr=',betaCorr
# print 'orientation=',orientation
# print 'sp_X=',sp[0]
# print 'sp_Y=',sp[1]
# print '------------'
# print '-Rotational component-'
# print 'vx=',vx
# print 'vy=',vy
# print 'gamma=',euler[2]
# print 'rotCorr=',rotCorr
# print '------------'
# print 'Velocity_propeller_1 = ',particlesTargetVelocities[0]
# print 'Velocity_propeller_2 = ',particlesTargetVelocities[1]
# print 'Velocity_propeller_3 = ',particlesTargetVelocities[2]
# print 'Velocity_propeller_4 = ',particlesTargetVelocities[3]
# print '------------'
# print '////////////'
## 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 __init__(self):
# 建立通信
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True,
5000, 5)
if self.clientID != -1:
break
else:
time.sleep(0.1)
print("Failed connecting to remote API server!")
print("Connection success!")
#设置机械臂仿真步长，为保持API端与VREP端相同步长
vrep.simxSetFloatingParameter(self.clientID,
vrep.sim_floatparam_simulation_time_step,
self.dt, vrep.simx_opmode_oneshot)
# 打开同步模式
vrep.simxSynchronous(self.clientID, True)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# 获取句柄joint
# 机械臂1
self.robot1_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_jointHandle[i] = returnHandle
# 机械臂2
self.robot2_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_jointHandle[i] = returnHandle
# 获取末端frame
_, self.end_handle = vrep.simxGetObjectHandle(
self.clientID, 'end', vrep.simx_opmode_blocking)
_, self.end0_handle = vrep.simxGetObjectHandle(
self.clientID, 'end0', vrep.simx_opmode_blocking)
_, self.goal1_handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_1', vrep.simx_opmode_blocking)
_, self.goal2_handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_2', vrep.simx_opmode_blocking)
# 俩机械臂间距
_, self.dist_handle = vrep.simxGetDistanceHandle(
self.clientID, 'robots_dist', vrep.simx_opmode_blocking)
_, self.end_dis_handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot1_goal', vrep.simx_opmode_blocking)
_, self.end0_dis_handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot2_goal', vrep.simx_opmode_blocking)
# 获取link句柄
self.link_handle1 = np.zeros((self.jointNum, ), dtype=np.int) #link 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 1),
vrep.simx_opmode_blocking)
self.link_handle1[i] = returnHandle
self.link_handle2 = np.zeros((self.jointNum, ),
dtype=np.int) # link 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 4),
vrep.simx_opmode_blocking)
self.link_handle2[i] = returnHandle
print('Handles available!')
def main():
global clientID
print('Program started')
emptybuf = bytearray()
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if clientID != -1:
print('Connected to remote API server')
# Start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# start init wheels --------------------------------------------------------------------------------------------
wheelJoints = np.empty(4, dtype=np.int)
wheelJoints.fill(-1) # front left, rear left, rear right, front right
res, wheelJoints[0] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[1] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[2] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[3] = vrep.simxGetObjectHandle(
clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait)
# end init wheels ----------------------------------------------------------------------------------------------
# start init camera --------------------------------------------------------------------------------------------
# change the angle of the camera view (default is pi/4)
res = vrep.simxSetFloatSignal(clientID, 'rgbd_sensor_scan_angle',
math.pi / 2,
vrep.simx_opmode_oneshot_wait)
# turn on camera
res = vrep.simxSetIntegerSignal(clientID, 'handle_rgb_sensor', 2,
vrep.simx_opmode_oneshot_wait)
# get camera object-handle
res, youBotCam = vrep.simxGetObjectHandle(
clientID, 'rgbSensor', vrep.simx_opmode_oneshot_wait)
# get first image
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, youBotCam, 0, vrep.simx_opmode_streaming)
time.sleep(1)
# end init camera ----------------------------------------------------------------------------------------------
# start init range sensor ----------------------------------------------------------------------------------------------
# initialize sensor and get sensor handles:
rangeSen.initializeSensor(clientID)
hokuyo = rangeSen.getSensorHandles(clientID)
# end init range sensor ----------------------------------------------------------------------------------------------
# programmable space -------------------------------------------------------------------------------------------
# implement state machine
# 1 - init | 2 - detect blob | 3 - move to blob | 4 - grab | 5 - follow next explore path | 6 - align to blob | 7 - follow next basket path
# 8 - drop block | 0 - finish | -1 - finish with error
state = 1
# space to store data to share between states
h_matrix = -1
explorePaths = Queue()
basketPaths = Queue()
blobsList = []
visitedBlobsList = []
orientations = Queue()
posBeforeMoveToBlob = move.getPos(clientID)[0][:-1]
blockColors = []
while state != 0:
if state == 1: # init
# init path, H, and next state
state, explorePaths, basketPaths, orientations, blockColors, h_matrix = ex.init_state(
youBotCam, clientID)
elif state == 2: # detect blob
# find all blobs that are 360 degrees around youBot
state, blobsList = ex.findBlobs(clientID, youBotCam, h_matrix,
blobsList, visitedBlobsList)
elif state == 3: # move to blob
# get to next blob state
posBeforeMoveToBlob = move.getPos(clientID)[
0][:-1] # store current position for moving back later
state, blobsList, visitedBlobsList = ex.getToNextBlob(
clientID, blobsList, visitedBlobsList)
elif state == 4: # grab
state = ex.grabBlob(clientID, h_matrix, youBotCam)
elif state == 5: # follow next explore path
state = ex.followExplorePath(clientID, hokuyo, explorePaths,
orientations)
elif state == 6: # align to blob
state = ex.alignToBlob(clientID, youBotCam)
elif state == 7: # follow next basket path
state = ex.followBasketPath(clientID, hokuyo, basketPaths,
blockColors)
elif state == 8: # drop blob
state = ex.dropBlob(clientID)
move.sideway(c.maxDistToBlock, clientID, True)
elif state == -1: # finish with error
print("Current state: fail state!")
print("An error has occurred. Program finished with state -1.")
state = 0
print("End of blob grabing shit")
'''
state, explorePaths, basketPaths, orientations, blockColors, h_matrix = ex.init_state(youBotCam, clientID)
move.forward(0.5, clientID)
ex.alignToBlob(clientID, youBotCam)
ex.grabBlob(clientID, h_matrix, youBotCam)
time.sleep(5)
#ex.moveArm(clientID, -90, 20,70,0,0)
#ex.moveArm(clientID, -90, 90,0,0,0)
#ex.getAngle(clientID)
#ex.moveArm(clientID, 0, 0,0,0,0)
# ex.moveArm(clientID, 180/math.pi*ex.getAngle(clientID), 95,40,35,0)
'''
# end of programmable space --------------------------------------------------------------------------------------------
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# Close connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
# Try to retrieve motors and robot handlers
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectHandle
ret1, wristHandle = vrep.simxGetObjectHandle(clientID, "WristMotor", opmode)
ret2, elbowHandle = vrep.simxGetObjectHandle(clientID, "ElbowMotor", opmode)
ret3, shoulderHandle = vrep.simxGetObjectHandle(clientID, "ShoulderMotor", opmode)
ret4, leftWheelHandle = vrep.simxGetObjectHandle(clientID, "LeftWheelJoint", opmode)
ret5, rightWheelHandle = vrep.simxGetObjectHandle(clientID, "RightWheelJoint", opmode)
ret6, robotHandle = vrep.simxGetObjectHandle(clientID, "2W1A", opmode)
# If handlers are OK, execute three random simulations
if ret1 == 0 and ret2 == 0 and ret3 == 0 and ret4 == 0 and ret5 == 0 and ret6 == 0:
random.seed()
for i in range(0, 3):
# Start the simulation
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStartSimulation
vrep.simxStartSimulation(clientID, opmode)
time.sleep(3)
msg = "----- Simulation started -----"
print msg
logging.info(msg)
# Start getting the robot position
# Unlike other commands, we will use a streaming operating mode
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectPosition
pret, robotPos = vrep.simxGetObjectPosition(clientID, robotHandle, -1, vrep.simx_opmode_streaming)
msg = "2w1a position: (x = " + str(robotPos[0]) +\
", y = " + str(robotPos[1]) + ") res = " + str(pret)
print msg
logging.info(msg)
# Start getting the robot orientation
def main():
# Start V-REP connection
try:
vrep.simxFinish(-1)
print("Connecting to simulator...")
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)
except KeyboardInterrupt:
vrep_exit(clientID)
return
# 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)
# Get quadrotor handle
err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)
print(err,quad_handle)
# 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)
vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
# Get quadrotor initial position and orientation
err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID,
quad_handle, vrep.simx_opmode_buffer)
pos_start = np.asarray(pos_start)
euler_start = np.asarray(euler_start)*10.
vel_start = np.asarray(vel_start)
angvel_start = np.asarray(angvel_start)
pos_old = pos_start
euler_old = euler_start
vel_old = vel_start
angvel_old = angvel_start
pos_new = pos_old
euler_new = euler_old
vel_new = vel_old
angvel_new = angvel_old
pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new
pos_error_all = pos_error
euler_error_all = euler_error
n_input = 6
n_forces=4
init_f=7.
state = [0 for i in range(n_input)]
state = torch.from_numpy(np.asarray(state, dtype=np.float32)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
delta_forces = [0., 0., 0., 0.]
extended_state=torch.cat((state,torch.from_numpy(np.asarray([propeller_vels], dtype=np.float32))),1)
memory = ReplayMemory(ROLLOUT_LEN)
test_num = 1
timestep = 1
while (vrep.simxGetConnectionId(clientID) != -1):
# Set propeller thrusts
print("Setting propeller thrusts...")
# Only PD control bc can't find api function for getting simulation time
propeller_vels = pid(pos_error,euler_new,euler_error[2],vel_error,angvel_error)
#propeller_vels = apply_forces(propeller_vels, delta_forces) # for dqn
# Send propeller thrusts
print("Sending propeller thrusts...")
[vrep.simxSetFloatSignal(clientID, prop, vels, vrep.simx_opmode_oneshot) for prop, vels in
zip(propellers, propeller_vels)]
# Trigger simulator step
print("Stepping simulator...")
vrep.simxSynchronousTrigger(clientID)
# Get quadrotor initial position and orientation
err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, vel_new, angvel_new = vrep.simxGetObjectVelocity(clientID,
quad_handle, vrep.simx_opmode_buffer)
pos_new = np.asarray(pos_new)
euler_new = np.asarray(euler_new)*10
vel_new = np.asarray(vel_new)
angvel_new = np.asarray(angvel_new)
#euler_new[2]/=100
valid = is_valid_state(pos_start, pos_new, euler_new)
if valid:
next_state = torch.FloatTensor(np.concatenate([euler_new, pos_new - pos_old]))
#next_state = torch.FloatTensor(euler_new )
next_extended_state=torch.FloatTensor([np.concatenate([next_state,np.asarray(propeller_vels)])])
else:
next_state = None
next_extended_state = None
reward=np.float32(0)
memory.push(extended_state, torch.from_numpy(np.asarray([delta_forces],dtype=np.float32)), next_extended_state,
torch.from_numpy(np.asarray([[reward]])))
state = next_state
extended_state=next_extended_state
pos_old = pos_new
euler_old = euler_new
vel_old = vel_new
angvel_old = angvel_new
print("Propeller Velocities:")
print(propeller_vels)
print("\n")
pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
euler_error %= 2*np.pi
for i in range(len(euler_error)):
if euler_error[i] > np.pi:
euler_error[i] -= 2*np.pi
vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new
pos_error_all = np.vstack([pos_error_all,pos_error])
euler_error_all = np.vstack([euler_error_all,euler_error])
print("Errors (pos,ang):")
print(pos_error,euler_error)
print("\n")
timestep += 1
if not valid or timestep > ROLLOUT_LEN:
np.savetxt('csv/pos_error{0}.csv'.format(test_num),
pos_error_all,
delimiter=',',
fmt='%8.4f')
np.savetxt('csv/euler_error{0}.csv'.format(test_num),
euler_error_all,
delimiter=',',
fmt='%8.4f')
print('RESET')
# reset
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(0.1)
# 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)
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
# Get quadrotor handle
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)
vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID,
quad_handle, vrep.simx_opmode_buffer)
pos_start = np.asarray(pos_start)
euler_start = np.asarray(euler_start)*10.
vel_start = np.asarray(vel_start)
angvel_start = np.asarray(angvel_start)*10.
pos_old = pos_start
euler_old = euler_start
vel_old = vel_start
angvel_old = angvel_start
pos_new = pos_old
euler_new = euler_old
vel_new = vel_old
angvel_new = angvel_old
pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new
pos_error_all = np.vstack([pos_error_all,pos_error])
euler_error_all = np.vstack([euler_error_all,euler_error])
state = [0 for i in range(n_input)]
state = torch.FloatTensor(np.asarray(state)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
extended_state = torch.cat((state, torch.FloatTensor(np.asarray([propeller_vels]))), 1)
print('duration: ',len(memory))
memory = ReplayMemory(ROLLOUT_LEN)
test_num += 1
timestep = 1
def run_trial(self, genomes):
# Set the signals for each robot
for genome, robot in zip(genomes, self.robot_names):
par = [genome[:4], genome[4:]]
for j in range(len(par)):
# For each motor
for k in range(len(par[j])):
# For each sensor (+ base value)
signal_name = robot + "_" + str(j + 1) + "_" + str(k + 1)
res = vrep.simxSetFloatSignal(self.clientID, signal_name,
par[j][k],
vrep.simx_opmode_oneshot)
if res > 1:
print 'Error setting signal ' + signal_name + ': ' + str(
res)
# Start running simulation
# print 'Running trial'
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
# Initialize output arrays
sim_time = [[] for i in range(self.n_robots)]
robot_x = [[] for i in range(self.n_robots)]
robot_y = [[] for i in range(self.n_robots)]
init_pos = [[] for i in range(self.n_robots)]
fit_y = [0 for i in range(self.n_robots)]
t_step = 0.01 # how often to check the simulation's signals
t_flag = time.time() + t_step
once = True
new_data = [0 for i in range(self.n_robots)]
go_loop = True
while go_loop:
now = time.time()
if now > t_flag:
t_flag = now + t_step
# Get info from robot
for i in range(self.n_robots):
res1, in1 = vrep.simxGetFloatSignal(
self.clientID, self.robot_names[i] + '_1',
vrep.simx_opmode_buffer)
res2, in2 = vrep.simxGetFloatSignal(
self.clientID, self.robot_names[i] + '_2',
vrep.simx_opmode_buffer)
res3, in3 = vrep.simxGetFloatSignal(
self.clientID, self.robot_names[i] + '_3',
vrep.simx_opmode_buffer)
if res1 == 0 and res2 == 0 and res3 == 0:
sim_time[i].append(in1)
robot_x[i].append(in2)
robot_y[i].append(in3)
new_data[i] = 1 # new data arrived
if once:
if sum(new_data) == self.n_robots:
for i in range(self.n_robots):
init_pos[i] = [robot_x[i][-1], robot_y[i][-1]]
once = False
else:
for i in range(self.n_robots):
if new_data[i] == 1:
fit_y[i] += abs(robot_y[i][-1]) * (
sim_time[i][-1] - sim_time[i][-2])
new_data[i] = 0
if sim_time[i][
-1] > 3: # time limit for the simulation
go_loop = False
break
# # Pause the simulation
# vrep.simxPauseSimulation(self.clientID, vrep.simx_opmode_oneshot)
# time.sleep(0.15)
#
# trial_fitness = []
# # Get the trial results for each robot
# for i in range(len(self.robot_handles)):
# res, pos = vrep.simxGetObjectPosition(self.clientID, self.robot_handles[i], -1, vrep.simx_opmode_buffer)
# # print pos_tmp
#
# delta_x = pos[0] - self.robot_pos0[i][0]
# delta_y = pos[1] - self.robot_pos0[i][1]
#
# # trial_fitness.append(delta_x)
# trial_fitness.append([delta_x, 1./(1.+20*abs(delta_y))])
trial_fitness = []
for i in range(self.n_robots):
x_fit = robot_x[i][-1] - init_pos[i][0]
y_fit = 1 / (1 + 20 * fit_y[i])
trial_fitness.append([x_fit, y_fit])
# Stop and reset the simulation
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
time.sleep(0.35)
return trial_fitness
def start(self):
error_code = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
