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)