def testNumericReplicator2(self):
"""Replicator should yield the same results as QR if noise = 0.0"""
for i in xrange(50):
p = Dynamics.RandomDistribution(20)
f = array([random.random() for k in xrange(20)])
r = Dynamics._numeric_QuickReplicator(p, f)
r2 = Dynamics._numeric_Replicator(p, f, 0.0)
self.failUnless(AlmostEqual(r, r2))
def testNumericReplicator4(self):
"""Little noise should only matter a little"""
for i in xrange(50):
p = Dynamics.RandomDistribution(100)
f = array([random.random() for k in xrange(100)])
r = Dynamics._numeric_QuickReplicator(p, f)
r2 = Dynamics._numeric_Replicator(p, f, 0.01)
self.failUnless(AlmostEqual(r, r2, 0.0101))
def testFitnessCorrelation(self):
"""Correlation should make a difference"""
G = ArrayWrapper.array(CorelatedGame)
for k in range(20):
p = Dynamics.RandomDistribution(8)
compare = Dynamics._QuickFitness(p, G, 3)
fitness = Dynamics._Fitness(p, G, 0.2, 3)
self.failIf(AlmostEqual(fitness, compare))
def testNumericReplicator3(self):
"""Noise should matter"""
for i in xrange(50):
p = Dynamics.RandomDistribution(100)
f = array([random.random() for k in xrange(100)])
r = Dynamics._numeric_QuickReplicator(p, f)
r2 = Dynamics._numeric_Replicator(p, f, 0.5)
self.failIf(AlmostEqual(r, r2))
def testFitness2ZeroCorrelation(self):
"""Fitness2 should return the same results QuickFitness2 if e=0.0."""
for i in range(20):
G = array(GenRandomGame(2, 8))
for k in range(5):
p = Dynamics.RandomDistribution(8)
compare = Dynamics._QuickFitness2(p, G)
fitness = Dynamics._Fitness2(p, G, 0.0)
self.failUnless(AlmostEqual(fitness, compare))
def testFitnessZeroCorrelation(self):
"""Fitness should return the same results as QuickFitness if e=0.0."""
for i in range(5):
G = array(GenRandomGame(4, 7))
for k in range(5):
p = Dynamics.RandomDistribution(7)
compare = Dynamics._QuickFitness(p, G, 4)
fitness = Dynamics._Fitness(p, G, 0.0, 4)
self.failUnless(AlmostEqual(fitness, compare))
def testNumericFitness2Players(self):
"""Fitness2 and numeric_Fitness2 should yield the same values"""
for i in range(20):
G = array(GenRandomGame(2, 8))
for k in range(5):
p = Dynamics.RandomDistribution(8)
compare = Dynamics._Fitness2(p, G, 0.2)
fitness = Dynamics._numeric_Fitness2(p, G, 0.2)
self.failUnless(AlmostEqual(fitness, compare))
def testFitness2Players(self):
"""Fitness should be the same as Fitness2 in the 2 player case"""
for i in range(20):
G = array(GenRandomGame(2, 8))
for k in range(5):
p = Dynamics.RandomDistribution(8)
compare = Dynamics._Fitness2(p, G, 0.2)
fitness = Dynamics._Fitness(p, G, 0.2, 2)
self.failUnless(AlmostEqual(fitness, compare))
def testQuickFitness2Players(self):
"""QuickFitness should not make a difference to QF2 in the 2 player case
"""
for i in range(20):
G = array(GenRandomGame(2, 8))
for k in range(5):
p = Dynamics.RandomDistribution(8)
compare = Dynamics._QuickFitness2(p, G)
fitness = Dynamics._QuickFitness(p, G, 2)
self.failUnless(AlmostEqual(fitness, compare))
def testSampledFitness(self):
"""SampledFitness should yield roughly the same results as Fitness"""
for i in range(5):
G = array(GenRandomGame(3, 8))
p = Dynamics.RandomDistribution(8)
fitness = Dynamics._Fitness(p, G, 0.2, 3)
failures = 0
for k in range(5):
sampled = Dynamics._SampledFitness(p, G, 0.2, 3, 5000)
if not AlmostEqual(fitness, sampled, 0.15): failures += 1
self.failIf(failures > 1)
def testSampledQuickFitness(self):
"""SampledQuickFitness should yield similar results as QuickFitness"""
for i in range(5):
G = array(GenRandomGame(4, 7))
p = Dynamics.RandomDistribution(7)
fitness = Dynamics._QuickFitness(p, G, 4)
failures = 0
for k in range(5):
sampled = Dynamics._SampledQuickFitness(p, G, 4, 5000)
if not AlmostEqual(fitness, sampled, 0.1): failures += 1
self.failIf(failures > 1)
def newState(self, sample, prev_sens):
if (self.checkCali(sample)):
return
# else:
# # TODO does this make sense??
# return
# state = [None]*sensor_no
sample = np.subtract(sample, self.offset)
sample[:][0] / self.g_weight
# print("Sample: ", sample)
prev_sens_pos = prev_sens.states[prev_sens.state_loc - 1][4]
state = self.getState(sample, 0, prev_sens_pos)
#saveState
self.states.append(state.tolist())
self.state_loc += 1
# make quat
self.currentQ = dyn.makequaternion(sample)
self.quats.append(self.currentQ)
if (self.state_loc % 100 == 0):
self.printStates()
def update_typical_vals(networks, int_times, rtol = 1e-9, fraction=1.0,
cutoff=1e-14):
"""
Update the typical var values for a group of networks.
Find the maximum of each variable over the integrations. In each network
the typical value is set to fraction of that maximum. If that maximum value
is less than cutoff, the typical value is set to 1.
networks List of networks to work with
int_times List of corresponding integration endpoints
(ie. [(0, 100), (0, 50)])
fraction Relative size of typical value, compared to max value over
the integrations.
rtol Relative tolerance for the integrations.
cutoff Values below this are considered to be zero
"""
max_vals = {}
for net, times in zip(networks, int_times):
traj = Dynamics.integrate(net, times, rtol=rtol, fill_traj=True)
for var_id in net.variables.keys():
curr_max = max(traj.get_var_traj(var_id))
max_vals[var_id] = max(curr_max, max_vals.get(var_id, 0))
for var_id, val in max_vals.items():
for net in networks:
if net.variables.has_key(var_id):
if val > cutoff:
net.set_var_typical_val(var_id, val*fraction)
else:
net.set_var_typical_val(var_id, 1.0)
return max_vals
def testQuickReplicator2(self):
"""Population should not change when fitnesses are equal"""
f = [1.0]*10
for i in xrange(50):
p = Dynamics.RandomDistribution(10)
r = Dynamics._QuickReplicator(p, f)
self.failIf(not AlmostEqual(p, r))
def simulate(self):
self.t = self.start
while self.t < self.end:
self.t += self.t_step
# Desired Trajectory [From Input or Trajectory] and State [From Dynamics]
self.State_d = TR.trajectory(self.t, self.State_d)
# Calculate Actions and Forces from Controller
self.PhiC_last, Actions = C2D.control(self.PhiC_last, self.State,
self.State_d, self.Gains,
self.Params, self.t_step)
# Use Dynamics to Simulate New State [Transform Axes, Estimate Derivatives
self.State = DYN.dynamics(self.State, self.Params, Actions,
self.t_step)
## Calculation time for above measured at around 0.5 milliseconds on average
# Record trajectory against simulation time
self.t_vec.append(self.t)
self.y_vec.append(self.State[0])
self.z_vec.append(self.State[1])
self.phi_vec.append(self.State[4])
self.y_des_vec.append(self.State_d[0])
self.z_des_vec.append(self.State_d[1])
def mpcPlot(self):
dy = Dynamics.VehicleDynamics()
ref = dy.reference_trajectory(
Numpy2Torch(self.state_history[:, -1],
self.state_history[:, -1].shape))
self.state_r_history = self.state_r_history.reshape([-1, 5])
plt.figure(1)
plt.plot(self.state_history[:, -1],
self.state_history[:, 0],
label="trajectory")
# plt.plot(state_r_history[:,-1],state_r_history[:,0], label="$trajectory_r$")
# plt.plot(self.state_history[:,-1],self.config.a_curve * np.sin(self.config.k_curve*self.state_history[:,-1]), label="reference")
plt.plot(self.state_history[:, -1], ref[:, 0], label="reference")
plt.legend(loc="upper right")
plt.figure(2)
plt.plot(self.state_history[:, -1],
self.state_history[:, 2],
label="trajectory")
# plt.plot(state_r_history[:,-1],state_r_history[:,0], label="$trajectory_r$")
# plt.plot(self.state_history[:,-1],self.config.a_curve * np.sin(self.config.k_curve*self.state_history[:,-1]), label="reference")
plt.plot(self.state_history[:, -1], ref[:, 2], label="reference")
plt.legend(loc="upper right")
plt.figure(3)
plt.plot(self.state_history[0:-1, -1], self.control_history)
plt.show()
def update_typical_vals(networks, int_times, rtol = 1e-9, fraction=1.0,
cutoff=1e-14):
"""
Update the typical var values for a group of networks.
Find the maximum of each variable over the integrations. In each network
the typical value is set to fraction of that maximum. If that maximum value
is less than cutoff, the typical value is set to 1.
networks List of networks to work with
int_times List of corresponding integration endpoints
(ie. [(0, 100), (0, 50)])
fraction Relative size of typical value, compared to max value over
the integrations.
rtol Relative tolerance for the integrations.
cutoff Values below this are considered to be zero
"""
max_vals = {}
for net, times in zip(networks, int_times):
traj = Dynamics.integrate(net, times, rtol=rtol, fill_traj=True)
for var_id in net.variables.keys():
curr_max = max(traj.get_var_traj(var_id))
max_vals[var_id] = max(curr_max, max_vals.get(var_id, 0))
for var_id, val in max_vals.items():
for net in networks:
if net.variables.has_key(var_id):
if val > cutoff:
net.set_var_typical_val(var_id, val*fraction)
else:
net.set_var_typical_val(var_id, 1.0)
return max_vals
def testQuickFitnessFairGame(self):
"""Fitness should return equal fitnesses a 'totally fair' game"""
G = ArrayWrapper.array(lambda a,b,c,d: 5.0)
for i in range(10):
r = Dynamics.RandomDistribution(7)
f = Dynamics._QuickFitness(r, G, 4)
for k in range(1,7):
self.failIf(f[k] != f[0])
def testQuickFitness2FairGame(self):
"""QuickFitness2 should yield equal fitnesses in a totally fair game"""
G = ArrayWrapper.array(lambda a,b: 0.3)
for i in range(20):
p = Dynamics.RandomDistribution(5)
f = Dynamics._QuickFitness2(p, G)
for k in range(1,5):
self.failIf(f[k] != f[0])
def testQuickReplicator3(self):
"""Different fitness values ought to have an impact"""
p = Dynamics.UniformDistribution(10)
f = array([1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1])
for i in xrange(5):
r = Dynamics._QuickReplicator(p, f)
for k in range(len(r)-1):
self.failIf(r[k+1]>=r[k])
p = r
def testNumericReplicator1(self):
"""Results of Replicator should be sound"""
for i in xrange(50):
p = Dynamics.RandomDistribution(50)
f = array([random.random() for k in xrange(50)])
r = Dynamics._numeric_Replicator(p, f, random.random())
for x in r:
self.failIf(x < 0.0 or x > 1.0)
self.failUnless(AlmostEqual(sum(r), 1.0))
def discrete_data(net, params, pts, interval, vars=None, random=False,
uncert_func=typ_val_uncert(0.1, 1e-14)):
"""
Return a set of data points for the given network generated at the given
parameters.
net Network to generate data for
params Parameters for this evaluation of the network
pts Number of data points to output
interval Integration interval
vars Variables to output data for, defaults to all species in net
random If False data points are distributed evenly over interval
If True they are spread randomly and uniformly over each
variable
uncert_func Function that takes in a trajectory and a variable id and
returns what uncertainty should be assumed for that variable,
either as a scalar or a list the same length as the trajectory.
"""
# Default for vars
if vars is None:
vars = net.species.keys()
# Assign observed times to each variable
var_times = {}
for var in vars:
if random:
var_times[var] = scipy.rand(pts) * (interval[1]-interval[0]) + interval[0]
else:
var_times[var] = scipy.linspace(interval[0], interval[1], pts)
# Create a sorted list of the unique times in the var_times dict
int_times = sets.Set(scipy.ravel(var_times.values()))
int_times.add(0)
int_times = list(int_times)
int_times.sort()
# Get the trajectory
traj = Dynamics.integrate(net, int_times, params=params, fill_traj=False)
# Build up our data dictionary
data = {}
for var, times in var_times.items():
var_data = {}
data[var] = var_data
# Calculate our uncertainties
var_uncerts = uncert_func(traj, var)
for time in times:
val = traj.get_var_val(var, time)
if scipy.isscalar(var_uncerts):
uncert = var_uncerts
else:
index = traj._get_time_index(time)
uncert = var_uncerts[index]
var_data[time] = (val, uncert)
return data
def discrete_data(net, params, pts, interval, vars=None, random=False,
uncert_func=typ_val_uncert(0.1, 1e-14)):
"""
Return a set of data points for the given network generated at the given
parameters.
net Network to generate data for
params Parameters for this evaluation of the network
pts Number of data points to output
interval Integration interval
vars Variables to output data for, defaults to all species in net
random If False data points are distributed evenly over interval
If True they are spread randomly and uniformly over each
variable
uncert_func Function that takes in a trajectory and a variable id and
returns what uncertainty should be assumed for that variable,
either as a scalar or a list the same length as the trajectory.
"""
# Default for vars
if vars == None:
vars = net.species.keys()
# Assign observed times to each variable
var_times = {}
for var in vars:
if random:
var_times[var] = scipy.rand(pts) * (interval[1]-interval[0]) + interval[0]
else:
var_times[var] = scipy.linspace(interval[0], interval[1], pts)
# Create a sorted list of the unique times in the var_times dict
int_times = sets.Set(scipy.ravel(var_times.values()))
int_times.add(0)
int_times = list(int_times)
int_times.sort()
# Get the trajectory
traj = Dynamics.integrate(net, int_times, params=params, fill_traj=False)
# Build up our data dictionary
data = {};
for var, times in var_times.items():
var_data = {}
data[var] = var_data
# Calculate our uncertainties
var_uncerts = uncert_func(traj, var)
for time in times:
val = traj.get_var_val(var, time)
if scipy.isscalar(var_uncerts):
uncert = var_uncerts
else:
index = traj._get_time_index(time)
uncert = var_uncerts[index]
var_data[time] = (val, uncert)
return data
def testQuickReplicator4(self):
"""Extinct species should stay extinct."""
for i in xrange(50):
f = array([random.uniform(0.5, 1.5) for k in xrange(10)])
x = random.randint(1, 9)
p = Dynamics.UniformDistribution(x, 0.5)
p = concatenate((p, array([0.0] * (10-x))))
idx = [l for l in range(10) if p[l] == 0.0]
r = Dynamics._QuickReplicator(p, f)
for l in idx:
self.failIf(r[l] != 0.0)
def adjustVelocitiesToConstraints(self, velocities=None, block=1):
"""Modifies the velocities to be compatible with
the distance constraints, i.e. projects out the velocity
components along the constrained distances.
This operation is thread-safe; it blocks other threads that
want to access the velocities while the data is being
updated. If this is not desired (e.g. when calling from
a routine that handles locking itself), the optional parameter
|block| should be set to 0."""
import Dynamics
if velocities is None:
velocities = self.velocities()
if velocities is not None:
if block:
self.acquireWriteStateLock()
try:
Dynamics.projectVelocities(self, velocities)
finally:
if block:
self.releaseWriteStateLock()
def calculate_msd():
parameterFile = "parameters.json"
parser = argparse.ArgumentParser()
parser.add_argument("-d",
"--directory",
type=str,
default="./",
help="input directory")
#parser.add_argument("-o", "--output", type=str, help="output directory")
#parser.add_argument("-p", "--prefix", type=str, default="cornea",help="prefix for output file")
parser.add_argument("-s",
"--skip",
type=int,
default=1100,
help="skip this many samples")
parser.add_argument("-m",
"--howmany",
type=int,
default=30,
help="read this many samples")
parser.add_argument("-t",
"--step",
type=int,
default=4,
help="step snapshots with this spacing in flow field")
parser.add_argument("-a",
"--average",
type=int,
default=5,
help="average over these many samples in flow field")
args = parser.parse_args()
param = read_param_CAPMD.Param(
p.paramsFromFile(capmd.Parameters(), parameterFile))
sheet = Dynamics(initype="fromCSV",
param=param,
datapath='output/',
multiopt="many")
skip = 0
step = 1
howmany = 500
sheet.readDataMany("CAPMD", skip, step, howmany, False, readtypes=[1])
sheet.validate_initialise()
sheet.getMSD(False)
plt.show()
return 0
def calibrate(self, calSample):
# print("cal_sample:\n", calSample)
n = self.num_cal_samples
sample = np.empty((3, 2), dtype=float)
sample[0][0] = sum(calSample[:, 0]) / n
sample[1][0] = sum(calSample[:, 1]) / n
sample[2][0] = sum(calSample[:, 2]) / n
sample[0][1] = sum(calSample[:, 3]) / n
sample[1][1] = sum(calSample[:, 4]) / n
sample[2][1] = sum(calSample[:, 5]) / n
#print(sample)
currentQ = dyn.makequaternion0(sample)
a_body = np.quaternion(0, sample[0][0], sample[1][0], sample[2][0])
a_nav = dyn.rotatef(currentQ, a_body) # accel in nav frame
v = quaternion.as_float_array(a_nav)
u = abs(v)
x = np.where(u == np.amax(u))
######## TODO
# x = np.where(v == np.amax(abs(v)))
# print("vx0 ", v[x[0]])
# print(x)
v[x[0]] = v[x[0]] + dyn.g
# if v[x[0]] < 0:
# print("vx < 0")
# v[x[0]] = v[x[0]] + dyn.g
# else:
# print("vx >= 0")
# v[x[0]] = v[x[0]] - dyn.g
#print(v[x[0]])
qNav = quaternion.as_quat_array(v)
qBody = dyn.rotateb(self.currentQ, qNav)
#print(qBody)
Q = quaternion.as_float_array(qBody)
self.offset[:, 0] = Q[1:]
self.offset[:, 1] = sample[:, 1]
#print(offset)
return sample
def get_update(self, i):
start = time.time()
self.t += self.t_step
# Desired Trajectory [From Input or Trajectory] and State [From Dynamics]
self.State_d = TR.trajectory(self.t, self.State_d)
# Calculate Actions and Forces from Controller
self.PhiC_last, Actions = C2D.control(self.PhiC_last, self.State,
self.State_d, self.Gains,
self.Params, self.t_step)
# Use Dynamics to Simulate New State [Transform Axes, Estimate Derivatives
self.State = DYN.dynamics(self.State, self.Params, Actions,
self.t_step)
## Calculation time for above measured at around 0.5 milliseconds on average
end = time.time()
print("Simulation time")
print(end - start)
# Update Plot
self.t_vec.append(self.t)
self.p_vec.append(self.State[0])
self.p_des_vec.append(self.State_d[0])
start = time.time()
self.line.set_data(self.t_vec, self.p_vec)
#self.fig.canvas.draw()
self.fig.canvas.blit(self.ax.bbox)
end = time.time()
print("Plotting time")
print(end - start)
if self.t > 10:
self.terminate()
return self.line,
def unitary_dynamics(self, pulse, tlist, wd, drive_qb=1, dynamics=None):
'''
User may call this function after created solver object
pulse: the pulse envelope of drive
tlist: time array of drive
wd: drive frequency
drive_qb: drive qubit, start from 1, instead of 0
'''
if len(pulse) != len(tlist):
raise ValueError(
'pulse array should have the same length with t array!')
self.generate_drive_term(tlist, wd, drive_qb)
#self.H = np.array([self.Hdiag for i in range(len(tlist))], dtype=qt.Qobj)
self.H = self.H0_int + pulse * self.Hd
if dynamics == None: dynamics = self.dynamics
desolver = Dynamics.Sesolver()
desolver.solve(self.H, tlist, dynamics=dynamics)
self.U = desolver.U
def mpcSolution(self):
# initialize state model
statemodel_plt = Dynamics.VehicleDynamics()
state = self.initial_state
# state = torch.tensor([[0.0, 0.0, self.psi_init, 0.0, 0.0]])
state.requires_grad_(False)
x_ref = statemodel_plt.reference_trajectory(state[:, -1])
state_r = state.detach().clone() # relative state
state_r[:, 0:4] = state_r[:, 0:4] - x_ref
self.state_history = state.detach().numpy()
plot_length = self.config.SIMULATION_STEPS
self.control_history = []
self.state_r_history = state_r
cal_time = 0
plt.figure(0)
for i in range(plot_length):
x = state_r.tolist()[0]
time_start = time.time()
temp, control = self.solver.mpcSolver(x, self.config.NP)
plt.plot(temp[:, -1], temp[:, 0])
cal_time += time.time() - time_start
u = Numpy2Torch(control[0], (-1, self.config.ACTION_DIM))
state, state_r = step_relative(statemodel_plt, state, u)
self.state_history = np.append(self.state_history,
state.detach().numpy(),
axis=0)
self.control_history = np.append(self.control_history,
u.detach().numpy())
self.state_r_history = np.append(self.state_history,
state_r.detach().numpy())
print("MPC calculating time: {:.3f}".format(cal_time) + "s")
self.mpcSaveTraj()
def newStateS0(self, sample):
if (self.checkCali(sample)):
return
# state = [None]*sensor_no
sample = np.subtract(sample, self.offset)
sample[:][0] / self.g_weight
# print("Sample: ", sample)
# init state array
state = self.getState(sample, 1, 0)
# use old getState using accel
#saveState
self.states.append(state.tolist())
self.state_loc += 1
# use old quat method for s0
self.currentQ = dyn.orientation(self.currentQ, sample)
self.quats.append(self.currentQ)
if (self.state_loc % 100 == 0):
self.printStates()
def getState(self, sample, using_a, prev_sens_pos):
state = np.zeros((3, 5), dtype=float)
if (using_a):
a_body = np.quaternion(0, sample[0, 0], sample[1, 0], sample[2, 0])
a_nav = dyn.rotatef(self.currentQ, a_body) # accel in nav frame
a_nav_subg = dyn.subtractg(a_nav)
# Acceleration - body
a_body = quaternion.as_float_array(a_body)
state[:, 4] = a_body[1:] # position
# Acceleration - nav
a_nav_subg = quaternion.as_float_array(a_nav_subg)
state[0, 2] = a_nav_subg[1]
state[1, 2] = a_nav_subg[2]
state[2, 2] = a_nav_subg[3]
# Gyroscope
state[0, 3] = sample[0, 1]
state[1, 3] = sample[1, 1]
state[2, 3] = sample[2, 1]
# Velocity
i = self.state_loc
state[0, 1] = dyn.speed(self.states[i - 1][1][0],
self.states[i - 1][2][0],
dyn.w) #speed(v0 a t)
state[1, 1] = dyn.speed(self.states[i - 1][1][1],
self.states[i - 1][2][1], dyn.w)
state[2, 1] = dyn.speed(self.states[i - 1][1][2],
self.states[i - 1][2][2], dyn.w)
# Position
state[0, 0] = dyn.position(self.states[i - 1][0][0],
self.states[i - 1][1][0],
self.states[i - 1][2][0],
dyn.w) #position(s0,v0,a,t)
state[1, 0] = dyn.position(self.states[i - 1][0][1],
self.states[i - 1][1][1],
self.states[i - 1][2][1], dyn.w)
state[2, 0] = dyn.position(self.states[i - 1][0][2],
self.states[i - 1][1][2],
self.states[i - 1][2][2], dyn.w)
else: # not using accelerometer values
# Gyroscope
state[0, 3] = sample[0, 1]
state[1, 3] = sample[1, 1]
state[2, 3] = sample[2, 1]
# rotate old pos about prev sensor old position
last_pos = self.states[self.state_loc][4]
p = dyn.orbit(np.asarray(last_pos), np.asarray(prev_sens_pos),
self.currentQ)
state[:, 4] = p
# state[0,0] = p[0]
# state[0,1] = p[1]
# state[0,2] = p[2]
# Velocity
# i = self.state_loc
# state[0,1] = dyn.speed(self.states[i-1][1][0], self.states[i-1][2][0], dyn.w) #speed(v0 a t)
# state[1,1] = dyn.speed(self.states[i-1][1][1], self.states[i-1][2][1], dyn.w)
# state[2,1] = dyn.speed(self.states[i-1][1][2], self.states[i-1][2][2], dyn.w)
# # Position
# state[0,0] = dyn.position(self.states[i-1][0][0], self.states[i-1][1][0], self.states[i-1][2][0], dyn.w) #position(s0,v0,a,t)
# state[1,0] = dyn.position(self.states[i-1][0][1], self.states[i-1][1][1], self.states[i-1][2][1], dyn.w)
# state[2,0] = dyn.position(self.states[i-1][0][2], self.states[i-1][1][2], self.states[i-1][2][2], dyn.w)
return state.T
def testQuickFitness2DemandGame(self):
"""Test QuickFitness with the 'demand game'"""
fitness = Dynamics._QuickFitness2(self.pop1, self.DemandGame)
self.failUnless(fitness[2] > fitness[1] and fitness[2] >= fitness[0])
import Dynamics
import Trajectory
import MinimumTime
import Shortcutting
set_printoptions(precision=5)
################# Loading the environment ########################
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('/home/cuong/Dropbox/Code/mintime/robots/twodof.robot.xml')
#env.Load('/home/cuong/Dropbox/Code/mintime/robots/arm.robot.xml')
#env.Load('robots/wam4.robot.xml')
robot = env.GetRobots()[0]
env.GetPhysicsEngine().SetGravity([0, 0, 0])
params = Dynamics.GetParams(robot)
robot.SetDOFValues(zeros(robot.GetDOF()))
robot.SetDOFVelocities(zeros(robot.GetDOF()))
robot.SetDOFValues(zeros(robot.GetDOF()))
m = robot.GetLinks()[1].GetMass() #mass of the links
l = robot.GetLinks()[1].GetGlobalCOM()[0] * 2 #length of the links
coef = 1 / 2. * m * l * l
theta1 = 0
theta2 = 1.1
ctheta2 = cos(theta2)
robot.SetDOFValues([theta1, theta2])
M_ref = coef * array([[2 * (5 / 3. + ctheta2), 2 / 3. + ctheta2],
[2 / 3. + ctheta2, 2 / 3.]])
def get_update(self, i):
start = time.time()
self.t += self.t_step
# Calculate Actions and Forces from Controller
self.PhiC_last, Actions = C2D.control(self.PhiC_last, self.State,
self.State_d, self.Gains,
self.Params, self.t_step)
# Use Dynamics to Simulate New State [Transform Axes, Estimate Derivatives
self.State = DYN.dynamics(self.State, self.Params, Actions,
self.t_step)
## Calculation time for above measured at around 0.5 milliseconds on average
# Update Plot
self.t_vec.append(self.t)
self.y_vec.append(self.State[0])
self.z_vec.append(self.State[1])
self.y_des_vec.append(self.State_d[0])
self.z_des_vec.append(self.State_d[1])
end = time.time()
theta = self.State[4]
attitude2D = np.array(
((math.cos(theta), -1 * math.sin(theta)), (math.sin(theta),
math.cos(theta))))
self.body = attitude2D.dot(self.base_body.T)
self.body = self.body + np.array([[self.State[0]], [self.State[1]]])
base = self.body[:, 0]
top = self.body[:, 1]
wingL = self.body[:, 2]
wingR = self.body[:, 3]
self.head.set_data([base[0], top[0]], [base[1], top[1]])
self.wing.set_data([wingL[0], wingR[0]], [wingL[1], wingR[1]])
print("simulation time")
print(end - start)
self.line_actual.set_data(self.y_vec, self.z_vec)
if self.t > 20:
self.terminate()
return self.line_actual, self.head, self.wing
# control_history = np.empty([0, 1])
# time_init = time.time()
# for i in range(config.NP_TOTAL):
# state, control = solver.mpc_solver(x, 60)
# x = state[1]
# u = control[0]
# state_history = np.append(state_history, x)
# control_history = np.append(control_history, u)
# x = x.tolist()
# print("steps:{:3d}".format(i) + " | state: " + str(x))
# print("MPC calculating time: {:.3f}".format(time.time() - time_init) + "s")
# state_history = state_history.reshape(-1,config.DYNAMICS_DIM)
# # np.savetxt(os.path.join(log_dir, 'structured_MPC_state.txt'), state_history)
# # np.savetxt(os.path.join(log_dir, 'structured_MPC_control.txt'), control_history)
statemodel_plt = Dynamics.VehicleDynamics()
if config.tire_model == 'Fiala':
state = torch.tensor([[1.0, 0.0, config.psi_init, 0.0, config.u, 0.0]])
else:
state = torch.tensor([[0.0, 0.0, config.psi_init, 0.0, 0.0]])
state.requires_grad_(False)
x_ref = statemodel_plt.reference_trajectory(state[:, -1])
state_r = state.detach().clone()
state_r[:, 0:4] = state_r[:, 0:4] - x_ref
state_history = state.detach().numpy()
x = np.array([0.])
plot_length = 300
control_history = []
state_r_history = state_r
cal_time = 0
plt.figure()
def plot_comparison(simu_dir, methods):
'''
Plot comparison figure among ADP, MPC & open-loop solution.
Trajectory, tracking error and control signal plot
Parameters
----------
picture_dir: string
location of figure saved.
'''
num_methods = len(methods)
picture_dir = simu_dir + "/Figures"
config = DynamicsConfig()
trajectory_data = []
dy = Dynamics.VehicleDynamics()
if os.path.exists(os.path.join(simu_dir, 'structured_MPC_state.txt')) == 0:
print('No comparison state data!')
else:
if 'MPC' in methods:
mpc_state = np.loadtxt(
os.path.join(simu_dir, 'structured_MPC_state.txt'))
mpc_trajectory = (mpc_state[:, 4], mpc_state[:, 0])
trajectory_data.append(mpc_trajectory)
if 'ADP' in methods:
adp_state = np.loadtxt(os.path.join(simu_dir, 'ADP_state.txt'))
adp_trajectory = (adp_state[:, 4], adp_state[:, 0])
trajectory_data.append(adp_trajectory)
if 'OP' in methods:
open_loop_state = np.loadtxt(
os.path.join(simu_dir, 'Open_loop_state.txt'))
open_loop_trajectory = (open_loop_state[:, 4], open_loop_state[:,
0])
trajectory_data.append(open_loop_trajectory)
myplot(trajectory_data,
num_methods,
"xy",
fname=os.path.join(picture_dir, 'trajectory.png'),
xlabel="longitudinal position [m]",
ylabel="Lateral position [m]",
legend=methods,
legend_loc="lower left")
heading_angle = []
if 'MPC' in methods:
mpc_state = np.loadtxt(
os.path.join(simu_dir, 'structured_MPC_state.txt'))
mpc_trajectory = (mpc_state[:, 4], 180 / np.pi * mpc_state[:, 2])
heading_angle.append(mpc_trajectory)
if 'ADP' in methods:
adp_state = np.loadtxt(os.path.join(simu_dir, 'ADP_state.txt'))
adp_trajectory = (adp_state[:, 4], 180 / np.pi * adp_state[:, 2])
heading_angle.append(adp_trajectory)
if 'OP' in methods:
open_loop_state = np.loadtxt(
os.path.join(simu_dir, 'Open_loop_state.txt'))
open_loop_trajectory = (open_loop_state[:, 4], open_loop_state[:,
2])
heading_angle.append(open_loop_trajectory)
myplot(heading_angle,
num_methods,
"xy",
fname=os.path.join(picture_dir, 'trajectory_heading_angle.png'),
xlabel="longitudinal position [m]",
ylabel="Heading angle [degree]",
legend=methods,
legend_loc="lower left")
error_data = []
print('\nMAX TRACKING ERROR:')
print(' MAX LATERAL POSITION ERROR:')
if 'MPC' in methods:
mpc_ref = dy.reference_trajectory(
Numpy2Torch(mpc_state[:, 4], mpc_state[:, 4].shape)).numpy()
mpc_error = (mpc_state[:, 4], mpc_state[:, 0] - mpc_ref[:, 0])
print(' Max MPC lateral error: {:0.3E}m'.format(
float(max(abs(mpc_state[:, 0] - mpc_ref[:, 0])))))
error_data.append(mpc_error)
if 'ADP' in methods:
adp_ref = dy.reference_trajectory(
Numpy2Torch(adp_state[:, 4], adp_state[:, 4].shape)).numpy()
adp_error = (adp_state[:, 4], adp_state[:, 0] - adp_ref[:, 0])
print(' Max ADP lateral error: {:0.3E}m'.format(
float(max(abs(adp_state[:, 0] - adp_ref[:, 0])))))
error_data.append(adp_error)
if 'OP' in methods:
open_loop_error = (open_loop_state[:, 4],
open_loop_state[:, 0] - config.a_curve *
np.sin(config.k_curve * open_loop_state[:, 4]))
error_data.append(open_loop_error)
myplot(error_data,
num_methods,
"xy",
fname=os.path.join(picture_dir, 'trajectory_error.png'),
xlabel="longitudinal position error [m]",
ylabel="Lateral position [m]",
legend=methods,
legend_loc="upper left")
# mpc_error = (mpc_state[:, 4], mpc_state[:, 0] - config.a_curve * np.sin(config.k_curve * mpc_state[:, 4]))
# open_loop_error = (open_loop_state[:, 4], open_loop_state[:, 0] - config.a_curve * np.sin(config.k_curve * open_loop_state[:, 4]))
# adp_error = (adp_state[:, 4], 1 * (adp_state[:, 0] - config.a_curve * np.sin(config.k_curve * adp_state[:, 4]))) # TODO: safe delete
#error_data = [mpc_error, adp_error, open_loop_error]
# myplot(error_data, 3, "xy",
# fname=os.path.join(picture_dir,'trajectory_error.png'),
# xlabel="longitudinal position [m]",
# ylabel="Lateral position error [m]",
# legend=["MPC", "ADP", "Open-loop"],
# legend_loc="lower left"
# )
psi_error_data = []
print(' MAX YAW ANGLE ERROR:')
if 'MPC' in methods:
mpc_psi_error = (mpc_state[:, 4],
180 / np.pi * (mpc_state[:, 2] - mpc_ref[:, 2]))
print(' MPC: {:0.3E} degree'.format(
float(max(abs(180 / np.pi *
(mpc_state[:, 2] - mpc_ref[:, 2]))))))
psi_error_data.append(mpc_psi_error)
if 'ADP' in methods:
adp_psi_error = (adp_state[:, 4],
180 / np.pi * (adp_state[:, 2] - adp_ref[:, 2]))
print(' ADP: {:0.3E} degree'.format(
float(max(abs(180 / np.pi *
(adp_state[:, 2] - adp_ref[:, 2]))))))
psi_error_data.append(adp_psi_error)
if 'OP' in methods:
open_loop_psi_error = (
open_loop_state[:, 4], 180 / np.pi *
(open_loop_state[:, 2] -
np.arctan(config.a_curve * config.k_curve *
np.cos(config.k_curve * open_loop_state[:, 4]))))
psi_error_data.append(open_loop_psi_error)
myplot(psi_error_data,
num_methods,
"xy",
fname=os.path.join(picture_dir, 'head_angle_error.png'),
xlabel="longitudinal position [m]",
ylabel="head angle error [degree]",
legend=["MPC", "ADP", "Open-loop"],
legend_loc="lower left")
control_plot_data = []
if 'MPC' in methods:
mpc_control = np.loadtxt(
os.path.join(simu_dir, 'structured_MPC_control.txt'))
mpc_control_tuple = (mpc_state[1:, 4], 180 / np.pi * mpc_control)
control_plot_data.append(mpc_control_tuple)
if 'ADP' in methods:
adp_control = np.loadtxt(os.path.join(simu_dir, 'ADP_control.txt'))
adp_control_tuple = (adp_state[1:, 4], 180 / np.pi * adp_control)
control_plot_data.append(adp_control_tuple)
if 'OP' in methods:
open_loop_control = np.loadtxt(
os.path.join(simu_dir, 'Open_loop_control.txt'))
open_loop_control_tuple = (open_loop_state[1:, 4],
180 / np.pi * open_loop_control)
control_plot_data.append(open_loop_control_tuple)
myplot(control_plot_data,
3,
"xy",
fname=os.path.join(picture_dir, 'control.png'),
xlabel="longitudinal position [m]",
ylabel="steering angle [degree]",
legend=["MPC", "ADP", "Open-loop"],
legend_loc="upper left")
if 'OP' in methods:
y_avs_error = []
for [i, d] in enumerate(error_data):
y_avs_error.append(np.mean(np.abs(d[1])))
print("Tracking error of lateral position:")
print("MPC:{:.3e} | ".format(y_avs_error[0]) +
"ADP:{:.3e} | ".format(y_avs_error[1]) +
"Open-loop:{:.3e} | ".format(y_avs_error[2]))
psi_avs_error = []
for [i, d] in enumerate(psi_error_data):
psi_avs_error.append(np.mean(np.abs(d[1])))
print("Tracking error of heading angle:")
print("MPC:{:.3e} | ".format(psi_avs_error[0]) +
"ADP:{:.3e} | ".format(psi_avs_error[1]) +
"Open-loop:{:.3e} | ".format(psi_avs_error[2]))
mpc_control_error = mpc_control - open_loop_control
adp_control_error = adp_control - open_loop_control
print("Control error:")
print("MPC:{:.3e} | ".format(np.mean(np.abs(mpc_control_error))) +
"ADP:{:.3e} | ".format(np.mean(np.abs(adp_control_error))))
def enforceConstraints(self, configuration=None, velocities=None):
"""Enforces the previously defined distance constraints
by modifying the configuration and velocities."""
import Dynamics
Dynamics.enforceConstraints(self, configuration)
self.adjustVelocitiesToConstraints(velocities)
def simulation(methods, log_dir, simu_dir):
policy = Actor(S_DIM, A_DIM)
value = Critic(S_DIM, A_DIM)
config = DynamicsConfig()
solver = Solver()
load_dir = log_dir
policy.load_parameters(load_dir)
value.load_parameters(load_dir)
statemodel_plt = Dynamics.VehicleDynamics()
plot_length = config.SIMULATION_STEPS
# initial_state = torch.tensor([[0.5, 0.0, config.psi_init, 0.0, 0.0]])
# baseline = Baseline(initial_state, simu_dir)
# baseline.mpcSolution()
# baseline.openLoopSolution()
# Open-loop reference
x_init = [0.0, 0.0, config.psi_init, 0.0, 0.0]
op_state, op_control = solver.openLoopMpcSolver(x_init, config.NP_TOTAL)
np.savetxt(os.path.join(simu_dir, 'Open_loop_control.txt'), op_control)
for method in methods:
cal_time = 0
state = torch.tensor([[0.0, 0.0, config.psi_init, 0.0, 0.0]])
# state = torch.tensor([[0.0, 0.0, 0.0, 0.0, 0.0]])
state.requires_grad_(False)
# x_ref = statemodel_plt.reference_trajectory(state[:, -1])
x_ref = statemodel_plt.reference_trajectory(state[:, -1])
state_r = state.detach().clone()
state_r[:, 0:4] = state_r[:, 0:4] - x_ref
state_history = state.detach().numpy()
control_history = []
print('\nCALCULATION TIME:')
for i in range(plot_length):
if method == 'ADP':
time_start = time.time()
u = policy.forward(state_r[:, 0:4])
cal_time += time.time() - time_start
elif method == 'MPC':
x = state_r.tolist()[0]
time_start = time.time()
_, control = solver.mpcSolver(x, config.NP) # todo:retreve
cal_time += time.time() - time_start
u = np.array(control[0],
dtype='float32').reshape(-1, config.ACTION_DIM)
u = torch.from_numpy(u)
else:
u = np.array(op_control[i],
dtype='float32').reshape(-1, config.ACTION_DIM)
u = torch.from_numpy(u)
state, state_r = step_relative(statemodel_plt, state, u)
# state_next, deri_state, utility, F_y1, F_y2, alpha_1, alpha_2 = statemodel_plt.step(state, u)
# state_r_old, _, _, _, _, _, _ = statemodel_plt.step(state_r, u)
# state_r = state_r_old.detach().clone()
# state_r[:, [0, 2]] = state_next[:, [0, 2]]
# x_ref = statemodel_plt.reference_trajectory(state_next[:, -1])
# state_r[:, 0:4] = state_r[:, 0:4] - x_ref
# state = state_next.clone().detach()
# s = state_next.detach().numpy()
state_history = np.append(state_history,
state.detach().numpy(),
axis=0)
control_history = np.append(control_history, u.detach().numpy())
if method == 'ADP':
print(" ADP: {:.3f}".format(cal_time) + "s")
np.savetxt(os.path.join(simu_dir, 'ADP_state.txt'), state_history)
np.savetxt(os.path.join(simu_dir, 'ADP_control.txt'),
control_history)
elif method == 'MPC':
print(" MPC: {:.3f}".format(cal_time) + "s")
np.savetxt(os.path.join(simu_dir, 'structured_MPC_state.txt'),
state_history)
np.savetxt(os.path.join(simu_dir, 'structured_MPC_control.txt'),
control_history)
else:
np.savetxt(os.path.join(simu_dir, 'Open_loop_state.txt'),
state_history)
adp_simulation_plot(simu_dir)
plot_comparison(simu_dir, methods)
LR_V = 3e-3 # learning rate of value net
# tasks
TRAIN_FLAG = 1
LOAD_PARA_FLAG = 0
SIMULATION_FLAG = 1
# Set random seed
np.random.seed(0)
torch.manual_seed(0)
# initialize policy and value net, model of vehicle dynamics
config = GeneralConfig()
policy = Actor(config.STATE_DIM, config.ACTION_DIM, lr=LR_P)
value = Critic(config.STATE_DIM, 1, lr=LR_V)
vehicleDynamics = Dynamics.VehicleDynamics()
state_batch = vehicleDynamics.initialize_state()
writer = SummaryWriter()
# Training
iteration_index = 0
if LOAD_PARA_FLAG == 1:
print(
"********************************* LOAD PARAMETERS *********************************"
)
# load pre-trained parameters
load_dir = "./Results_dir/2020-10-09-14-42-10000"
policy.load_parameters(load_dir)
value.load_parameters(load_dir)
if TRAIN_FLAG == 1:
parser.add_argument("-c", "--conffile", type=str, default="plane_periodic.conf", help="configuration file")
parser.add_argument("-d", "--directory", type=str, default="/home/sh18581/Documents/Dome/samos_check/periodic_test/data_phi_1/data_v0_0.01/data_nu_0.01/",help="input directory")
parser.add_argument("-s", "--skip", type=int, default=300, help="skip this many samples")
parser.add_argument("-m", "--howmany", type=int, default=500, help="read this many samples")
parser.add_argument("-t", "--step", type=int, default=20, help="step snapshots with this spacing")
parser.add_argument("-o", "--outfile", type=str, default="correlations.p", help="pickle file name")
args = parser.parse_args()
# Choose to show as plots as well:
plotall = True
# Use the Configuration constructor in its readCSV format to generate the topology
# fromCSV(kwargs["parampath"],kwargs["datapath"],kwargs["multiopt"])
parampath0 = args.directory + args.conffile
param = Param(parampath0)
Cells = Dynamics(initype="fromCSV",param=param,datapath=args.directory,multiopt="many")
# Now read in as desired
# def readDataMany(self,skip=0,step=1,howmany='all',Nvariable=False,readtypes = 'all'):
Cells.readDataMany("SAMoS",args.skip,1,args.howmany,False,readtypes = 'all')
Cells.validate_initialise()
# Start collecting all of this in data dictionary
data={'configuration':args.directory,'skip':args.skip,'howmany':args.howmany,'step':args.step}
# 1st basic set of analyses
# Will save as pickle file regardless
# A - Velocity distributions and mean velocity
# vav, vdist,vdist2 = getVelDist(self,bins,bins2,usetype='all',verbose=True):
# Bins are in normalised units (by mean velocity)
def main(): ctrl = Controller() dyn = Dynamics()
class VideoStreamHandler(socketserver.StreamRequestHandler):
command = Dynamics()
identify_features = features()
print("Video Server Active")
def handle(self):
global command
global distance
global previous_distance
stream_bytes = b''
AEB = 0
try:
while True:
stream_bytes += self.rfile.read(1024)
first = stream_bytes.find(b'\xff\xd8')
last = stream_bytes.find(b'\xff\xd9')
if first != -1 and last != -1:
jpg = stream_bytes[first:last + 2]
stream_bytes = stream_bytes[last + 2:]
#read images in stream
img = cv2.imdecode(np.frombuffer(jpg, dtype=np.uint8),
cv2.IMREAD_COLOR)
#Perspective Transform
src = np.float32([[00, 100], [320, 100], [-10, 150],
[330, 150]])
dst = np.float32([[0, 0], [320, 0], [0, 240], [320, 240]])
M = cv2.getPerspectiveTransform(src, dst)
image = cv2.warpPerspective(img, M, (320, 240))
#Image Processing -Identify Features
img, image, stop, average_heading, sigFlag, carFlag = self.identify_features.haar(
img, image)
#Display Processed Images and views
cv2.imshow('Feild Of View', img)
cv2.imshow('Birds Eye', image) #img
#HC-SR04 distance data
a = (previous_distance + distance) / 2
b = previous_distance - distance
if distance < 25: #Object under threshold distance
AEB = 1
#Emergency Brakes Flag
else:
AEB = 0
if cv2.waitKey(3) & 0xFF == ord('q'):
break
if sigFlag == 0: # Signal is Green
#Set Lane Departure limits
if ((average_heading < 150) and
(stop == 0)) and AEB == 0:
print("Turn left")
self.command.fleft()
elif ((average_heading > 170) and
(stop == 0)) and AEB == 0:
print("Turn right")
self.command.fright()
elif ((170 >= average_heading >= 150) and
(stop == 0)) and AEB == 0:
print("In lane")
self.command.forward()
elif stop == 1:
print("BRAKES ACTIVE due to Stop sign")
self.command.brake()
elif AEB == 1:
self.command.brake()
if carFlag == 1: #If car spotted
print("Car Ahead at " + str(distance) + " cm")
else:
print(
"Obstacle Ahead!, Emergency Brakes Active")
else:
pass
elif sigFlag == 1: # Signal is Red
pass
print("Signal Red, waiting to turn Green...")
self.command.brake()
finally:
cv2.destroyAllWindows()
self.command.halt()
self.command.closeconn()
sys.exit()
import sys
import cStringIO
set_printoptions(precision=5)
save_stdout = sys.stdout
################# Loading the environment ########################
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('/home/cuong/Dropbox/Code/mintime/robots/twodof.robot.xml')
robot=env.GetRobots()[0]
params=Dynamics.GetParams(robot)
grav=[0,9.8,0]
################# Initialize a trajectory ########################
P0=poly1d([-2.1])
P1=poly1d([1,-2.7])
pwp_traj=Trajectory.PieceWisePolyTrajectory([[P0,P1]],[1])
T=1
n_discr=100.
t_step=T/n_discr
a=time.clock()
