def step(self, action:np.ndarray): """ Angles en vrep go through -180º to 180, 180ª=-180º Early stop for angles that commit the following rule abs(alpha) > 90º, this solve the problem of +-180º Compute sin & cos of Yaw angle avoid the problem of +-180ª orientation: [roll, pitch, sinYAW, cosYAW] """ for act, name in zip(action, self.propsignal): vrep.simxSetFloatSignal(self.clientID, name, act, vrep.simx_opmode_streaming) vrep.simxSynchronousTrigger(self.clientID) vrep.simxGetPingTime(self.clientID) #position, orientation, lin_vel, ang_vel, lin_acel, ang_acel = self._get_observation_state() #position, orientation, lin_vel, ang_vel = self._get_observation_state() rowdata = self._get_observation_state() #rowdata = self._appendtuples_((rotmat, position, angvel, linvel)) """ Roll & Pitch must be lower than 90º, else apply earlystop """ earlystop = np.abs(rowdata['orientation'][:-1]) > np.pi/2.0 """ Use sinYAW & cosYAW as features instead of YAW directly to avoid problem when YAW is near to 180º""" #yawangle = orientation[-1] #orientation[-1] = np.sin(yawangle) #orientation = np.concatenate((orientation, np.array([np.cos(yawangle)]))) # reward = rowdata['position'] distance = np.sqrt((reward * reward).sum()) reward = 4.0 -1.25 * distance done = (distance > 3.2) or (earlystop.sum() > 0.0) #rowdata = np.concatenate((position, orientation, lin_vel, ang_vel), axis=0) rowdata = self._flat_observation(rowdata) #done = earlystop.sum() > 0.0 return (rowdata, reward, done, dict())
def reset(self): vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking) try: while True: vrep.simxGetIntegerSignal(self.clientID, 'signal_debug', vrep.simx_opmode_blocking) e = vrep.simxGetInMessageInfo(self.clientID, vrep.simx_headeroffset_server_state) still_running = e[1] & 1 if not still_running: break except: pass r, self.quad_handler = vrep.simxGetObjectHandle(self.clientID, self.envname, vrep.simx_opmode_oneshot_wait) r, self.target_handler = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter_target', vrep.simx_opmode_oneshot_wait) # start pose init_position, init_ang = self._get_random_pos_ang(max_radius=3.1, max_angle=np.pi, respecto=self.targetpos) vrep.simxSetObjectPosition(self.clientID, self.quad_handler, -1, init_position, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(self.clientID, self.quad_handler, -1, init_ang, vrep.simx_opmode_blocking) ## Set target vrep.simxSetObjectPosition(self.clientID, self.target_handler, -1, self.targetpos, vrep.simx_opmode_oneshot) self.startsimulation() vrep.simxSynchronousTrigger(self.clientID) vrep.simxGetPingTime(self.clientID) #rdata = self._get_observation_state(False) position, orientation, lin_vel, ang_vel, lin_acel, ang_acel = self._get_observation_state(compute_acelleration=False) #rowdata = self._appendtuples_((rotmat, position, angvel, linvel)) """ Use sinYAW & cosYAW as features instead of YAW directly to avoid problem when YAW is near to 180º""" yawangle = orientation[-1] orientation[-1] = np.sin(yawangle) orientation = np.concatenate((orientation, np.array([np.cos(yawangle)])), axis=0) rowdata = np.concatenate((position, orientation, lin_vel, ang_vel, lin_acel, ang_acel), axis=0) return rowdata
def reset(self, init_pos=None, init_ang=None): #print('Reset -ing id> ', self.clientID) # Put code when reset here #r = vrep.simxSetObjectPosition(self.clientID, self.quad_handler, -1, np.array([0.0,0.0,0.5]), vrep.simx_opmode_oneshot_wait) #r = vrep.simxCallScriptFunction(self.clientID, 'Quadricopter_target', vrep.sim_scripttype_childscript, 'sysCall_custom_reset', np.array([]), np.array([]), np.array([]), bytearray(), vrep.simx_opmode_blocking) # pass vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking) try: while True: vrep.simxGetIntegerSignal(self.clientID, 'signal_debug', vrep.simx_opmode_blocking) e = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state) still_running = e[1] & 1 if not still_running: break except: pass #print('Totally stopped> ID> ', self.clientID) # Reset quadrotor r, self.quad_handler = vrep.simxGetObjectHandle( self.clientID, self.envname, vrep.simx_opmode_oneshot_wait) r, self.target_handler = vrep.simxGetObjectHandle( self.clientID, 'Quadricopter_target', vrep.simx_opmode_oneshot_wait) # start posedistance = np.sqrt((reward * reward).sum()) #init_position, init_ang = self._get_random_pos_ang(max_radius=3.1, max_angle=np.pi, respecto=self.targetpos) #vrep.simxSetObjectPosition(self.clientID, self.quad_handler, -1, init_position, vrep.simx_opmode_blocking) #vrep.simxSetObjectOrientation(self.clientID, self.quad_handler, -1, init_ang, vrep.simx_opmode_blocking) self.set_states(init_pos, init_ang) ## Set target vrep.simxSetObjectPosition(self.clientID, self.target_handler, -1, self.targetpos, vrep.simx_opmode_oneshot) self.startsimulation() vrep.simxSynchronousTrigger(self.clientID) vrep.simxGetPingTime(self.clientID) rowdata = self._get_observation_state() return self._flat_observation(rowdata)
def step(self, action: np.ndarray): for act, name in zip(action, self.propsignal): vrep.simxSetFloatSignal(self.clientID, name, act, vrep.simx_opmode_streaming) #vrep.simxSetFloatSignal(self.clientID, self.propsignal1) # sincronyze vrep.simxSynchronousTrigger(self.clientID) vrep.simxGetPingTime(self.clientID) #rotmat, position, angvel, linvel = self._get_observation_state() """ _get_observation_state() must be overloaded! """ rowdata = self._get_observation_state() #rowdata = self._appendtuples_((rotmat, position, angvel, linvel)) """ compute_rewards(rowdata) must be overloaded""" reward = self.compute_rewards(rowdata) """ compute_done(rowdata) must be overloaded """ done = self.compute_done(rowdata) """ Flatten data into numpy vector""" rowdata = self._flat_observation(rowdata) return (rowdata, reward, done, dict())