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())