def step(self, action, reset):
if sys.version_info[0] == 3:
_ = sim.simxSetObjectPosition(self.clientID, self.target, -1,
self.target_position,
sim.simx_opmode_oneshot)
sensor_data = np.zeros((self.total_sensors), dtype=np.float32)
vel_reading = np.zeros((2), dtype=np.float32)
angular_reading = 0
collision = np.zeros((2), dtype=np.float32)
target_location = np.zeros((3), dtype=np.float32)
target_location = np.round_(
np.subtract(np.array(self.position[:2]),
np.array(self.target_position[:2])), 3)
if (reset == False):
if (self.flag):
_, target_location = sim.simxGetObjectPosition(
self.clientID, self.target, -1, sim.simx_opmode_buffer)
_, bot_location = sim.simxGetObjectPosition(
self.clientID, self.bot, -1, sim.simx_opmode_buffer)
target_location = np.round_([
bot_location[0] - target_location[0],
bot_location[1] - target_location[1]
], 3)
self.flag = True
speed = (self.velocity * action[0]) / 100
turn = (self.angular_velocity * action[1]) / 100
l_wheel_vel = round(
(speed - self.wheel_basis * turn) / self.wheel_radius, 4)
r_wheel_vel = round(
(speed + self.wheel_basis * turn) / self.wheel_radius, 4)
_ = sim.simxSetJointTargetVelocity(self.clientID,
self.left_wheel,
l_wheel_vel,
sim.simx_opmode_streaming)
_ = sim.simxSetJointTargetVelocity(self.clientID,
self.right_wheel,
r_wheel_vel,
sim.simx_opmode_streaming)
#Collision
_, collision[0] = sim.simxGetIntegerSignal(
self.clientID, "collision_wall", sim.simx_opmode_buffer)
_, collision[1] = sim.simxGetIntegerSignal(
self.clientID, "collision_target", sim.simx_opmode_buffer)
sensor_data = self._readsensor_continue()
vel_reading, angular_reading = self._get_velocity_reading_continue(
)
else:
self._create(self.position)
#_ = sim.simxSetObjectPosition(self.clientID, self.target, self.bot, self.target_position, sim.simx_opmode_oneshot)
#target_location = self.position - self.target_position
sim.simxSynchronousTrigger(self.clientID)
return sensor_data, vel_reading, angular_reading, target_location, collision
else:
raw_input('Press <enter> key to step the !')
def stepSimulation():
if client.runInSynchronousMode:
currentStep = client.stepCounter
sim.simxSynchronousTrigger(client.id)
while client.stepCounter == currentStep:
retCode, s = sim.simxGetIntegerSignal(
client.id, client.intSignalName,
sim.simx_opmode_buffer)
if retCode == sim.simx_return_ok:
client.stepCounter = s
retCode, res, img = sim.simxGetVisionSensorImage(
client.id, client.visionSensorHandle, 0,
sim.simx_opmode_buffer)
client.lastImageAcquisitionTime = sim.simxGetLastCmdTime(
client.id)
if retCode == sim.simx_return_ok:
sim.simxSetVisionSensorImage(
client.id, client.passiveVisionSensorHandle, img, 0,
sim.simx_opmode_oneshot)
else:
retCode, res, img = sim.simxGetVisionSensorImage(
client.id, client.visionSensorHandle, 0,
sim.simx_opmode_buffer)
if retCode == sim.simx_return_ok:
imageSimTime = sim.simxGetLastCmdTime(client.id)
if client.lastImageAcquisitionTime != imageSimTime:
client.lastImageAcquisitionTime = imageSimTime
sim.simxSetVisionSensorImage(
client.id, client.passiveVisionSensorHandle, img,
0, sim.simx_opmode_oneshot)
def get_state(self) -> int:
"""Retrieves the state of the button.
@rtype: int
"""
int_sig, val = s.simxGetIntegerSignal(self._id, self._name,
self._def_op_mode)
return val
def stepSimulation():
currentStep=client.stepCounter
sim.simxSynchronousTrigger(client.id);
while client.stepCounter==currentStep:
retCode,s=sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_buffer)
if retCode==sim.simx_return_ok:
client.stepCounter=s
def __init__(self, ip, port, n_genomes):
"""
Constructor
"""
self.health_initial = 5
self.health = self.health_initial
self.ip = ip
self.port = port
self.amount_of_genomes = n_genomes
self.genome_filenames = []
self.genomes = []
self.status = -1
self.__old_status = -2
self.is_online = False
self.was_online = False
self.clientID = sim.simxStart(self.ip, self.port, True, False,
client_connection_timeout,
client_comm_cycle_time)
# subscribe to streaming
sim.simxGetIntegerSignal(self.clientID, 'Evo-Client Status', mode4)
def _intial(self):
#handles
#bot_handle
_, self.bot = sim.simxGetObjectHandle(self.clientID, "Bot_collider",
sim.simx_opmode_blocking)
_, _ = sim.simxGetObjectPosition(self.clientID, self.bot, -1,
sim.simx_opmode_streaming)
#wheels
_, self.left_wheel = sim.simxGetObjectHandle(self.clientID,
"Left_wheel_joint",
sim.simx_opmode_blocking)
_, self.right_wheel = sim.simxGetObjectHandle(self.clientID,
"Right_wheel_joint",
sim.simx_opmode_blocking)
#Wall_collection
_, self.wall = sim.simxGetCollectionHandle(self.clientID, "wall",
sim.simx_opmode_blocking)
#Target
_, self.target = sim.simxGetObjectHandle(self.clientID, "target",
sim.simx_opmode_blocking)
_, _ = sim.simxGetObjectPosition(self.clientID, self.target, -1,
sim.simx_opmode_streaming)
#Sensors
for i in range(self.total_sensors):
proxy = "proxy_" + str(i) + "_"
_, self.sensor[i] = sim.simxGetObjectHandle(
self.clientID, proxy, sim.simx_opmode_blocking)
_, _, _, _, _ = sim.simxReadProximitySensor(
self.clientID, self.sensor[i], sim.simx_opmode_streaming)
#Sensor velocity
_, _, _ = sim.simxGetObjectVelocity(self.clientID, self.bot,
sim.simx_opmode_streaming)
#Collision Data Stream
_, _ = sim.simxGetIntegerSignal(self.clientID, "collision_wall",
sim.simx_opmode_streaming)
_, _ = sim.simxGetIntegerSignal(self.clientID, "collision_target",
sim.simx_opmode_streaming)
self.reset = False
def update_status(self):
"""
check the status of the simulator
"""
self.status_has_changed = False
self.is_online = False
if sim.simxGetConnectionId(
self.clientID) != -1 and self.clientID != -1:
success, self.status = sim.simxGetIntegerSignal(
self.clientID, 'Evo-Client Status', mode4)
self.is_online = True
if self.status != self.__old_status: self.status_has_changed = True
self.__old_status = self.status
# -1 = not ready/error, 0 = ready, 1 = active, 2 = done
return self.status
def block(self):
res = sim.simxGetIntegerSignal(self.clientID, 'Bloqueado', sim.simx_opmode_blocking)
while res[1]:
time.sleep(0.1)
res = sim.simxGetIntegerSignal(self.clientID, 'Bloqueado', sim.simx_opmode_blocking)
stopbits=serial.STOPBITS_ONE)
serUSB.reset_output_buffer()
serUSB.reset_input_buffer()
# FPGA - Base variables
j = 0
from_patmos = ""
to_patmos = ""
patmos_motor = [1000, 1000, 1000, 1000]
END_CMD = 'END_SIM'
# Start communication with the simulator
with ClientCoppelia() as drone:
if drone.id != -1:
print('Connected to Coppelia Simulator')
# Start streaming drone.intSignalName integer signal, that signals when a step is finished:
sim.simxGetIntegerSignal(drone.id, drone.intSignalName,
sim.simx_opmode_streaming)
drone.init()
# ---> main loop <---
while (True):
# 1. Get info from the sensors
drone.sensing()
# 2. Send info to FPGA
to_patmos = ''
# The message format is fixed to have +/- signed and 4 digits
for i in range(3):
temp = int(drone.position[i] * 1000)
temp_abs = abs(temp)
if (temp > 0):
to_patmos += '+'
j6=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.108,1.182,1.249,1.311,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.571,1.571,1.571,1.569,1.561,1.548,1.529,1.503,1.470,1.431,1.388,1.343,1.297,1.251,1.205,1.159,1.113,1.067,1.021,0.975,0.929,0.883,0.837,0.791,0.745,0.699,0.653,0.607,0.561,0.515,0.470,0.424,0.378,0.332,0.286,0.240,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000]
# Start simulation:
sim.simxStartSimulation(clientID,sim.simx_opmode_blocking)
# Wait until ready:
waitForMovementExecuted('ready')
# Send the movement sequence:
targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180]
movementData={"id":"movSeq1","type":"pts","times":times,"j1":j1,"j2":j2,"j3":j3,"j4":j4,"j5":j5,"j6":j6}
packedMovementData=msgpack.packb(movementData)
sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot)
# Execute movement sequence:
sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot)
# Wait until above movement sequence finished executing:
waitForMovementExecuted('movSeq1')
sim.simxStopSimulation(clientID,sim.simx_opmode_blocking)
sim.simxGetIntegerSignal(clientID,integerSignalName,sim.simx_opmode_discontinue)
sim.simxGetPingTime(clientID)
# Now close the connection to CoppeliaSim:
sim.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')
cosAngle = math.cos(errorValue)
errorValue = math.atan2(sinAngle, cosAngle)
ctrl = errorValue * PID_P
# Calculate the velocity needed to reach the position in one dynamic time step:
velocity = ctrl / dynStepSize
if (velocity > velUpperLimit):
velocity = velUpperLimit
if (velocity < -velUpperLimit):
velocity = -velUpperLimit
return velocity
# Start streaming client.intSignalName integer signal, that signals when a step is finished:
sim.simxGetIntegerSignal(client.id, client.intSignalName,
sim.simx_opmode_streaming)
res, client.jointHandle = sim.simxGetObjectHandle(
client.id, 'joint', sim.simx_opmode_blocking)
sim.simxSetJointTargetVelocity(client.id, client.jointHandle,
360 * math.pi / 180,
sim.simx_opmode_oneshot)
sim.simxGetJointPosition(client.id, client.jointHandle,
sim.simx_opmode_streaming)
# enable the synchronous mode on the client:
sim.simxSynchronous(client.id, True)
sim.simxStartSimulation(client.id, sim.simx_opmode_oneshot)
moveToAngle(45 * math.pi / 180)
