def init_rotors(clientID):
# Clear all signals
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
# Set all propellers to zero
for i in range(len(propellers)):
vrep.simxSetFloatSignal(clientID, propellers[i], 1e-8, vrep.simx_opmode_oneshot)
def clearSignal(self):
"""
clear the signal at the very begining
"""
vrep.simxClearFloatSignal(self.clientID, self.LSignalName,
vrep.simx_opmode_oneshot)
vrep.simxClearFloatSignal(self.clientID, self.RSignalName,
vrep.simx_opmode_oneshot)
def set_parameter(self, sensor_name, parameter_name, parameter_value):
"""Set sensor parameters. Sensor name can be fastHokuyo_sensor1, kinect_depth, kinect_rgb.
To get list of possible params call youbot.parameters_list"""
if parameter_name == 'perspective_angle':
parameter_value = parameter_value / (180 * 2) * math.pi
if parameter_name in self.params_f:
error = vrep.simxSetObjectFloatParameter(
self.client_id,
self.handles[sensor_name + self.postfix],
self.params_f[parameter_name],
parameter_value,
ONE_SHOT_MODE
)
vrep.simxSetFloatSignal(
self.client_id,
'change_params',
parameter_value,
ONE_SHOT_MODE
)
vrep.simxClearFloatSignal(
self.client_id,
'change_params',
ONE_SHOT_MODE
)
return error
elif parameter_name in self.params_i:
error = vrep.simxSetObjectFloatParameter(
self.client_id,
self.handles[sensor_name + self.postfix],
self.params_i[parameter_name],
parameter_value,
ONE_SHOT_MODE
)
vrep.simxSetFloatSignal(
self.client_id,
'change_params',
parameter_value,
ONE_SHOT_MODE
)
vrep.simxClearFloatSignal(
self.client_id,
'change_params',
ONE_SHOT_MODE
)
return error
else:
return 'Parameter not found'
def vrep_change_properties(client_id, object_id, parameter_id,
parameter_value):
"""Changing properties of sensors in vrep
client_id (int): ID of current scene in vrep
object_id (int): ID of sensor to change
parameter_id (int): ID of parameter to change
parameter_value (int/double): value of parameter to change
"""
params_f = {
'near_clipping_plane': 1000,
'far_clipping_plane': 1001,
'perspective_angle': 1004
}
params_i = {
'vision_sensor_resolution_x': 1002,
'vision_sensor_resolution_y': 1003
}
if parameter_id == 'perspective_angle':
parameter_value = parameter_value / (180 * 2) * math.pi
if parameter_id in params_f:
error = vrep.simxSetObjectFloatParameter(client_id, object_id,
params_f[parameter_id],
parameter_value,
vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value,
vrep.simx_opmode_blocking)
vrep.simxClearFloatSignal(client_id, 'change_params',
vrep.simx_opmode_blocking)
return error
elif parameter_id in params_i:
error = vrep.simxSetObjectIntParameter(client_id, object_id,
params_i[parameter_id],
parameter_value,
vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value,
vrep.simx_opmode_blocking)
vrep.simxClearFloatSignal(client_id, 'change_params',
vrep.simx_opmode_blocking)
return error
else:
return 'parameter wasn\'t found'
def waitForRobot(order, patID):
time.sleep(1)
ptime = None
orderID = 16*(order == 'linen') + patID
signalTo = 'order_ToVREP'
signalBack = 'ptime_FromVREP'
# Send order ID to be processed
vrep.simxSetIntegerSignal(clientID,signalTo,orderID,vrep.simx_opmode_oneshot)
# Start the receiving process
err,ptime=vrep.simxGetFloatSignal(clientID,signalBack,vrep.simx_opmode_streaming)
# While we haven't received anything
while not ptime:
err,ptime=vrep.simxGetFloatSignal(clientID,signalBack,vrep.simx_opmode_buffer)
if err != vrep.simx_return_ok:
print('!!!\nAn error occured while receiving data from vrep...\n!!!')
# Clear signal
vrep.simxClearFloatSignal(clientID,signalBack,vrep.simx_opmode_oneshot)
# Return the signal received (processing time)
return ptime
def waitForRobot(order, patID):
time.sleep(1)
ptime = None
orderID = 16 * (order == 'linen') + patID
signalTo = 'order_ToVREP'
signalBack = 'ptime_FromVREP'
# Send order ID to be processed
vrep.simxSetIntegerSignal(clientID, signalTo, orderID,
vrep.simx_opmode_oneshot)
# Start the receiving process
err, ptime = vrep.simxGetFloatSignal(clientID, signalBack,
vrep.simx_opmode_streaming)
# While we haven't received anything
while not ptime:
err, ptime = vrep.simxGetFloatSignal(clientID, signalBack,
vrep.simx_opmode_buffer)
if err != vrep.simx_return_ok:
print('!!!\nAn error occured while receiving data from vrep...\n!!!')
# Clear signal
vrep.simxClearFloatSignal(clientID, signalBack, vrep.simx_opmode_oneshot)
# Return the signal received (processing time)
return ptime
def clearSignal(self):
vrep.simxClearFloatSignal(self.clientID, self.LSignalName,
vrep.simx_opmode_oneshot)
vrep.simxClearFloatSignal(self.clientID, self.RSignalName,
vrep.simx_opmode_oneshot)
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .001
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
except KeyboardInterrupt:
vrep_exit(clientID)
err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
def reset():
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(0.1)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
err, pos = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
def reset(self):
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(0.1)
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(self.clientID, True)
vrep.simxSetFloatingParameter(
self.clientID,
vrep.sim_floatparam_simulation_time_step,
self.dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF,
vrep.simx_opmode_blocking)
# Get quadrotor handle
err, self.quad_handle = vrep.simxGetObjectHandle(
self.clientID, self.quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(self.clientID, self.quad_handle,
vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(self.propellers)):
vrep.simxClearFloatSignal(self.clientID, self.propellers[i],
vrep.simx_opmode_oneshot)
# Get quadrotor initial position and orientation
err, self.pos_start = vrep.simxGetObjectPosition(
self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
err, self.euler_start = vrep.simxGetObjectOrientation(
self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
err, self.vel_start, self.angvel_start = vrep.simxGetObjectVelocity(
self.clientID, self.quad_handle, vrep.simx_opmode_buffer)
self.pos_start = np.asarray(self.pos_start)
self.euler_start = np.asarray(self.euler_start) * 10.
self.vel_start = np.asarray(self.vel_start)
self.angvel_start = np.asarray(self.angvel_start)
self.pos_old = self.pos_start
self.euler_old = self.euler_start
self.vel_old = self.vel_start
self.angvel_old = self.angvel_start
self.pos_new = self.pos_old
self.euler_new = self.euler_old
self.vel_new = self.vel_old
self.angvel_new = self.angvel_old
self.pos_error = (self.pos_start + self.setpoint_delta[0:3]) \
- self.pos_new
self.euler_error = (self.euler_start + self.setpoint_delta[3:6]) \
- self.euler_new
self.vel_error = (self.vel_start + self.setpoint_delta[6:9]) \
- self.vel_new
self.angvel_error = (self.angvel_start + self.setpoint_delta[9:12]) \
- self.angvel_new
self.pos_error_all = self.pos_error
self.euler_error_all = self.euler_error
self.init_f = 5.8
self.propeller_vels = [
self.init_f, self.init_f, self.init_f, self.init_f
]
self.timestep = 1
return self.get_state()
def main():
# Start V-REP connection
try:
vrep.simxFinish(-1)
print("Connecting to simulator...")
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
print("Failed to connect to remote API Server")
vrep_exit(clientID)
except KeyboardInterrupt:
vrep_exit(clientID)
return
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .001
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
# Get quadrotor handle
err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name,
vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i],
vrep.simx_opmode_oneshot)
# Get quadrotor initial position and orientation
err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle, -1,
vrep.simx_opmode_buffer)
err, euler_old = vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
vrep.simx_opmode_buffer)
pos_old = np.asarray(pos_old)
euler_old = np.asarray(euler_old) * 10.
pos_start = pos_old
# hyper parameters
n_input = 6
n_forces = 4
#n_action = 8
hidden = 256
batch_size = 512
learning_rate = .01
eps = 0.1
gamma = 0.9
init_f = 7.
net = DQN(n_input + n_forces, hidden, 1)
optim = torch.optim.Adam(net.parameters(), lr=learning_rate)
state = [0 for i in range(n_input)]
state = torch.from_numpy(np.asarray(state,
dtype=np.float32)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
extended_state = torch.cat(
(state, torch.from_numpy(np.asarray([propeller_vels],
dtype=np.float32))), 1)
memory = ReplayMemory(10000)
while (vrep.simxGetConnectionId(clientID) != -1):
# epsilon greedy
r = random.random()
if r > eps:
delta_forces = generate_forces(net, extended_state, learning_rate)
else:
delta_forces = [float(random.randint(-1, 1)) for i in range(4)]
# Set propeller thrusts
print("Setting propeller thrusts...")
propeller_vels = apply_forces(propeller_vels, delta_forces)
# Send propeller thrusts
print("Sending propeller thrusts...")
[
vrep.simxSetFloatSignal(clientID, prop, vels,
vrep.simx_opmode_oneshot)
for prop, vels in zip(propellers, propeller_vels)
]
# Trigger simulator step
print("Stepping simulator...")
vrep.simxSynchronousTrigger(clientID)
# Get quadrotor initial position and orientation
err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1,
vrep.simx_opmode_buffer)
err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle,
-1,
vrep.simx_opmode_buffer)
pos_new = np.asarray(pos_new)
euler_new = np.asarray(euler_new) * 10
#euler_new[2]/=100
valid = is_valid_state(pos_start, pos_new, euler_new)
if valid:
next_state = torch.FloatTensor(
np.concatenate([euler_new, pos_new - pos_old]))
#next_state = torch.FloatTensor(euler_new )
next_extended_state = torch.FloatTensor(
[np.concatenate([next_state,
np.asarray(propeller_vels)])])
else:
next_state = None
next_extended_state = None
reward_acc = np.float32(-np.linalg.norm(
pos_new - pos_old)) if next_state is not None else np.float32(-10.)
reward_alpha = np.float32(
-np.fabs(euler_new[0])
) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_beta = np.float32(
-np.fabs(euler_new[1])
) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_gamma = np.float32(
-np.fabs(euler_new[2])
) if not check_quad_flipped(euler_new) else np.float32(-5.)
reward_alpha_acc = np.float32(
-np.fabs(euler_new[0] - euler_old[0])
) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_beta_acc = np.float32(
-np.fabs(euler_new[1] - euler_old[1])
) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_gamma_acc = np.float32(
-np.fabs(euler_new[2] - euler_old[2])
) if not check_quad_flipped(euler_new) else np.float32(-5.)
reward = reward_alpha + reward_beta + reward_gamma + reward_acc + reward_alpha_acc * 10. + reward_beta_acc * 10. + reward_gamma_acc * 10.
if not check_quad_flipped(euler_new) and not valid:
print('Success.')
reward = 10.
#reward_pos=-np.linalg.norm(pos_new-pos_start)
if reward < -10.:
reward = -10.
reward = np.float32(reward)
memory.push(
extended_state,
torch.from_numpy(np.asarray([delta_forces], dtype=np.float32)),
next_extended_state, torch.from_numpy(np.asarray([[reward]])))
state = next_state
extended_state = next_extended_state
pos_old = pos_new
euler_old = euler_new
print(propeller_vels)
print("\n")
# Perform one step of the optimization (on the target network)
#DQN_update2(net, memory, batch_size, gamma, optim)
if not valid:
DQN_update2(net, memory, batch_size, gamma, optim)
print('reset')
# reset
reset(clientID)
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF,
vrep.simx_opmode_blocking)
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .0005
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Get quadrotor handle
err, quad_handle = vrep.simxGetObjectHandle(
clientID, quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# Get quadrotor initial position and orientation
err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle,
-1,
vrep.simx_opmode_buffer)
err, euler_old = vrep.simxGetObjectOrientation(
clientID, quad_handle, -1, vrep.simx_opmode_buffer)
euler_old = euler_old
pos_old = np.asarray(pos_old)
euler_old = np.asarray(euler_old) * 10.
pos_start = pos_old
state = [0 for i in range(n_input)]
state = torch.FloatTensor(np.asarray(state)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
extended_state = torch.cat(
(state, torch.FloatTensor(np.asarray([propeller_vels]))), 1)
print('duration: ', len(memory))
memory = ReplayMemory(10000)
def main():
# Start V-REP connection
try:
vrep.simxFinish(-1)
print("Connecting to simulator...")
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID == -1:
print("Failed to connect to remote API Server")
vrep_exit(clientID)
except KeyboardInterrupt:
vrep_exit(clientID)
return
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .001
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
# Get quadrotor handle
err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)
print(err,quad_handle)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
# Get quadrotor initial position and orientation
err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID,
quad_handle, vrep.simx_opmode_buffer)
pos_start = np.asarray(pos_start)
euler_start = np.asarray(euler_start)*10.
vel_start = np.asarray(vel_start)
angvel_start = np.asarray(angvel_start)
pos_old = pos_start
euler_old = euler_start
vel_old = vel_start
angvel_old = angvel_start
pos_new = pos_old
euler_new = euler_old
vel_new = vel_old
angvel_new = angvel_old
pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new
pos_error_all = pos_error
euler_error_all = euler_error
n_input = 6
n_forces=4
init_f=7.
state = [0 for i in range(n_input)]
state = torch.from_numpy(np.asarray(state, dtype=np.float32)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
delta_forces = [0., 0., 0., 0.]
extended_state=torch.cat((state,torch.from_numpy(np.asarray([propeller_vels], dtype=np.float32))),1)
memory = ReplayMemory(ROLLOUT_LEN)
test_num = 1
timestep = 1
while (vrep.simxGetConnectionId(clientID) != -1):
# Set propeller thrusts
print("Setting propeller thrusts...")
# Only PD control bc can't find api function for getting simulation time
propeller_vels = pid(pos_error,euler_new,euler_error[2],vel_error,angvel_error)
#propeller_vels = apply_forces(propeller_vels, delta_forces) # for dqn
# Send propeller thrusts
print("Sending propeller thrusts...")
[vrep.simxSetFloatSignal(clientID, prop, vels, vrep.simx_opmode_oneshot) for prop, vels in
zip(propellers, propeller_vels)]
# Trigger simulator step
print("Stepping simulator...")
vrep.simxSynchronousTrigger(clientID)
# Get quadrotor initial position and orientation
err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, vel_new, angvel_new = vrep.simxGetObjectVelocity(clientID,
quad_handle, vrep.simx_opmode_buffer)
pos_new = np.asarray(pos_new)
euler_new = np.asarray(euler_new)*10
vel_new = np.asarray(vel_new)
angvel_new = np.asarray(angvel_new)
#euler_new[2]/=100
valid = is_valid_state(pos_start, pos_new, euler_new)
if valid:
next_state = torch.FloatTensor(np.concatenate([euler_new, pos_new - pos_old]))
#next_state = torch.FloatTensor(euler_new )
next_extended_state=torch.FloatTensor([np.concatenate([next_state,np.asarray(propeller_vels)])])
else:
next_state = None
next_extended_state = None
reward=np.float32(0)
memory.push(extended_state, torch.from_numpy(np.asarray([delta_forces],dtype=np.float32)), next_extended_state,
torch.from_numpy(np.asarray([[reward]])))
state = next_state
extended_state=next_extended_state
pos_old = pos_new
euler_old = euler_new
vel_old = vel_new
angvel_old = angvel_new
print("Propeller Velocities:")
print(propeller_vels)
print("\n")
pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
euler_error %= 2*np.pi
for i in range(len(euler_error)):
if euler_error[i] > np.pi:
euler_error[i] -= 2*np.pi
vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new
pos_error_all = np.vstack([pos_error_all,pos_error])
euler_error_all = np.vstack([euler_error_all,euler_error])
print("Errors (pos,ang):")
print(pos_error,euler_error)
print("\n")
timestep += 1
if not valid or timestep > ROLLOUT_LEN:
np.savetxt('csv/pos_error{0}.csv'.format(test_num),
pos_error_all,
delimiter=',',
fmt='%8.4f')
np.savetxt('csv/euler_error{0}.csv'.format(test_num),
euler_error_all,
delimiter=',',
fmt='%8.4f')
print('RESET')
# reset
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(0.1)
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .001
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
# Get quadrotor handle
err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(clientID, quad_handle, vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot)
err, pos_start = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler_start = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, vel_start, angvel_start = vrep.simxGetObjectVelocity(clientID,
quad_handle, vrep.simx_opmode_buffer)
pos_start = np.asarray(pos_start)
euler_start = np.asarray(euler_start)*10.
vel_start = np.asarray(vel_start)
angvel_start = np.asarray(angvel_start)*10.
pos_old = pos_start
euler_old = euler_start
vel_old = vel_start
angvel_old = angvel_start
pos_new = pos_old
euler_new = euler_old
vel_new = vel_old
angvel_new = angvel_old
pos_error = (pos_start + setpoint_delta[0:3]) - pos_new
euler_error = (euler_start + setpoint_delta[3:6]) - euler_new
vel_error = (vel_start + setpoint_delta[6:9]) - vel_new
angvel_error = (angvel_start + setpoint_delta[9:12]) - angvel_new
pos_error_all = np.vstack([pos_error_all,pos_error])
euler_error_all = np.vstack([euler_error_all,euler_error])
state = [0 for i in range(n_input)]
state = torch.FloatTensor(np.asarray(state)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
extended_state = torch.cat((state, torch.FloatTensor(np.asarray([propeller_vels]))), 1)
print('duration: ',len(memory))
memory = ReplayMemory(ROLLOUT_LEN)
test_num += 1
timestep = 1