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 __init__(self, ip='127.0.0.1', port=19997, envname='Quadricopter', targetpos=np.zeros(3, dtype=np.float32)):
super(VREPQuadSimple, self).__init__()
# Initialize vrep
self.envname = envname
clientID = vrep.simxStart(ip, port, True, True, 5000, 0)
if clientID != -1:
print('Connection Established Successfully to IP> {} - Port> {} - ID: {}'.format(ip, port, clientID))
self.clientID = clientID
self.targetpos = targetpos
_, self.dt = vrep.simxGetFloatingParameter(self.clientID, vrep.sim_floatparam_simulation_time_step, vrep.simx_opmode_oneshot_wait)
self.prev_linvel = np.zeros(3, dtype=np.float32)
self.prev_angvel = np.zeros(3, dtype=np.float32)
#self.prev_pos
print('Initialized with tstep>\t{}'.format(vrep.simxGetFloatingParameter(self.clientID, vrep.sim_floatparam_simulation_time_step, vrep.simx_opmode_oneshot_wait)))
else:
raise ConnectionError("Can't Connect with the envinronment at IP:{}, Port:{}".format(ip, port))
## Detach object target_get_random_pos_ang
r, self.target_handler = vrep.simxGetObjectHandle(clientID, 'Quadricopter_target', vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectParent(clientID, self.target_handler, -1, True, vrep.simx_opmode_oneshot_wait)
# Set signal debug:
vrep.simxSetIntegerSignal(self.clientID, 'signal_debug', 1337, vrep.simx_opmode_oneshot)
r, self.quad_handler = vrep.simxGetObjectHandle(clientID, self.envname, vrep.simx_opmode_oneshot_wait)
print(r, self.quad_handler)
# Define gym variables
self.action_space = spaces.Box(low=0.0, high=100.0, shape=(4,), dtype=np.float32)
self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(13,), dtype=np.float32)
# Get scripts propellers Here...!
#self.propsignal = ['joint' + str(i+1) for i in range(0, 4)]
self.propsignal = ['speedprop' + str(i+1) for i in range(0, 4)]