def _pattern_intersection(self, key):
"""
regarde la cellule voisine pointee par le pattern. Si l'intersection des
traces associees a ce pattern avec l'un des patterns de la cellule pointee,
alors on peut lier les 2 points d'equilibre (Center of Mass) des 2 patterns
pour obtenir un segment du chemin.
"""
print "CELL:"
for pattern, locs in self.all_nodes[key].significant_patterns.iteritems():
if len(locs) > 0:
print "current pattern: ", pattern
direction_out = pattern[1]
dest_cell = self.location_index.neighbor_on_direction(self.all_nodes[key]._center_of_mass(),direction_out)
# if dest_cell:
print "dest cell: ", dest_cell._center_of_mass()
print "current cell: ", self.all_nodes[key]._center_of_mass()
for l in locs:
pass
#print l.latitude, l.longitude
dest_cell_patterns = dest_cell.patterns_by_in_direction(Trajectory.reverse_direction(direction_out))
print "dcp: ", dest_cell_patterns
# self.pattern_edges.append((self.all_nodes[key]._center_of_mass(), dest_cell._center_of_mass()))
if dest_cell_patterns:
for _p in dest_cell_patterns:
dest_midpoint = Trajectory.center_of_mass(dest_cell.significant_patterns[_p])
source_midpoint = Trajectory.center_of_mass(locs)
self.pattern_edges.append((source_midpoint, dest_midpoint))
print "segment added", (source_midpoint, dest_midpoint)
def _initialize_netcdf(self):
"""
Initialize the netCDF file for storage.
"""
# Open NetCDF file for writing
ncfile = netcdf.Dataset(self.fn_storage, "w") # for netCDF4
# Store netcdf file handle.
self.ncfile = ncfile
# Set global attributes.
setattr(ncfile, "title", "Multi-State-Transition-Interface-Sampling")
setattr(ncfile, "application", "Host-Guest-System")
setattr(ncfile, "program", "run.py")
setattr(ncfile, "programVersion", __version__)
setattr(ncfile, "Conventions", "Multi-State Transition Interface TPS")
setattr(ncfile, "ConventionVersion", "0.1")
# initialize arrays used for snapshots
Snapshot._init_netcdf(self)
# initialize arrays used for trajectories
Trajectory._init_netcdf(self)
# Force sync to disk to avoid data loss.
ncfile.sync()
return
class Car(object):
def __init__(self, dyn, x0, color='yellow', T=5):
self.data0 = {'x0': x0}
self.bounds = [(-1., 1.), (-1., 1.)]
self.T = T
self.dyn = dyn
self.traj = Trajectory(T, dyn)
self.traj.x0.set_value(x0)
self.linear = Trajectory(T, dyn)
self.linear.x0.set_value(x0)
self.color = color
self.default_u = np.zeros(self.dyn.nu)
def reset(self):
self.traj.x0.set_value(self.data0['x0'])
self.linear.x0.set_value(self.data0['x0'])
for t in range(self.T):
self.traj.u[t].set_value(np.zeros(self.dyn.nu))
self.linear.u[t].set_value(self.default_u)
def move(self):
self.traj.tick()
self.linear.x0.set_value(self.traj.x0.get_value())
@property
def x(self):
return self.traj.x0.get_value()
@property
def u(self):
return self.traj.u[0].get_value()
@u.setter
def u(self, value):
self.traj.u[0].set_value(value)
def control(self, steer, gas):
pass
class NestedOptimizerCar(Car):
def __init__(self, *args, **vargs):
Car.__init__(self, *args, **vargs)
self.bounds = [(-3., 3.), (-2., 2.)]
@property
def human(self):
return self._human
@human.setter
def human(self, value):
self._human = value
self.traj_h = Trajectory(self.T, self.human.dyn)
def move(self):
Car.move(self)
self.traj_h.tick()
@property
def rewards(self):
return self._rewards
@rewards.setter
def rewards(self, vals):
self._rewards = vals
self.optimizer = None
def control(self, steer, gas):
if self.optimizer is None:
reward_h, reward_r = self.rewards
reward_h = self.traj_h.reward(reward_h)
reward_r = self.traj.reward(reward_r)
self.optimizer = utils.NestedMaximizer(reward_h, self.traj_h.u, reward_r, self.traj.u)
self.traj_h.x0.set_value(self.human.x)
self.optimizer.maximize(bounds = self.bounds)
def generateTrajectory(self, x0, nframes):
"""
Generate a velocity Verlet trajectory consisting of ntau segments of tau_steps in between storage of Snapshots and randomization of velocities.
ARGUMENTS
x0 (coordinate set) - initial coordinates; velocities will be assigned from Maxwell-Boltzmann distribution
nframes (int) - number of trajectory segments to generate
RETURNS
trajectory (list of Snapshot) - generated trajectory of initial conditions, including initial coordinate set
NOTES
This exists in OpenMM. Check with John on how to best get snapshots
This routine generates a velocity Verlet trajectory for systems without constraints by wrapping the OpenMM 'VerletIntegrator' in two half-kicks of the velocity.
"""
# Set initial positions
self.context.setPositions(x0)
# Store initial state for each trajectory segment in trajectory.
trajectory = Trajectory()
# Construct mass vector.
nparticles = self.system.getNumParticles()
mass = Quantity(numpy.zeros([nparticles, 3], numpy.float64), amu)
for particle_index in range(nparticles):
mass[particle_index, :] = self.system.getParticleMass(particle_index)
# Assign velocities from Maxwell-Boltzmann distribution
self.context.setVelocitiesToTemperature(self.temperature)
# Store initial snapshot of trajectory segment.
snapshot = Snapshot(context=self.context)
trajectory.forward(snapshot)
# Propagate dynamics by velocity Verlet.
in_void = True
self.frame = 0
while self.frame < self.xframes and in_void == True:
# Do integrator x steps
self.integrator.step()
# Check if reached a core set
in_void = self.check_void()
# Store final snapshot of trajectory.
snapshot = Snapshot(self.context)
trajectory.forward(snapshot)
return trajectory
def reset(event):
trajectories[:] = []
Trajectory.resetGlobID()
# clear canvas
w.delete('all')
#redraw background
w.create_image(300, 300, image=bg)
def buttonUp(event):
global newT, xold, yold
newT = True
xold = None
yold = None
# Check if last trajectory has 0 length
if trajectories[len(trajectories) - 1].length() == 0.0:
trajectories.pop()
Trajectory.decGlobID()
def __init__(self, traj=None):
if traj:
self.molecule = qtk.Molecule(traj)
stem, ext = os.path.splitext(traj)
pdb = re.sub('\.xyz', '', traj) + '_qtk_tmp.pdb'
self.molecule.write(pdb, format='pdb')
Trajectory.__init__(self, traj, pdb)
os.remove(pdb)
self.type_list = self.molecule.type_list
self.Z = self.molecule.Z
def solve_with_duration(T):
b = 2. / T * (3. * (Dq / T - v0) - Dv)
a = (Dv / T - b) / T
qdd_fun = lambda t: 2 * a * t + b
qd_fun = lambda t: a * t ** 2. + b * t + v0
q_fun = lambda t: a * t ** 3. / 3. + b * t ** 2. / 2 + v0 * t + q0
traj = Trajectory(T, q_fun, qd_fun, qdd_fun)
max_duration = robot.check_traj_dynamics(traj)
if max_duration > 2 * dt:
traj.duration = max_duration
return traj
return None
def __init__(self,trajectory_node,start,end):
Trajectory.__init__(self,trajectory_node)
self.start_point = start
self.end_point = end
self.tg = TrajectoryGenerator()
self.dist = self.tg.get_distance(self.start_point, self.end_point)
n = [0.,0.,0.]
for i in range(0,3):
n[i] = self.end_point[i] - self.start_point[i]
self.e_t = self.tg.get_direction(n)
self.t_f = math.sqrt(6*self.dist/0.9*self.a_max)
self.constant = -2.0/self.t_f**3.0 * self.dist
def move_thread(self, limb, position, queue, timeout=15.0): """ Adds the points sent by threads to the trajectory path """ try: trajectory = Trajectory(limb) trajectory.add_point(position, 2.0) trajectory.start() queue.put(None) except Exception, exception: queue.put(traceback.format_exc()) queue.put(exception)
def __init__(self,trajectory_node,mid,start,velo,psi): Trajectory.__init__(self,trajectory_node) self.tg = TrajectoryGenerator() self.midpoint = mid self.start = start n = [self.start[0]-self.midpoint[0],self.start[1]-self.midpoint[1],self.start[0]-self.midpoint[2]] self.radius = self.tg.get_distance(self.start,self.midpoint) self.initial_velo = velo self.velo = self.tg.get_norm(self.initial_velo) #define new coordinates self.e_n = self.tg.get_direction(n) self.yp = self.tg.get_direction(self.initial_velo) self.zp = numpy.cross(self.e_n,self.yp) self.psi = psi #angle of rotation about initial e_n direction self.w = self.radius*self.velo self.theta_z = self.tg.get_projection([0,0,1],self.e_n)
def _restore_netcdf(self):
"""
Restore the storage from the netCDF file
"""
# Open NetCDF file for appending
ncfile = netcdf.Dataset(self.fn_storage, "a")
# Store netcdf file handle.
self.ncfile = ncfile
# initialize arrays used for snapshots
Snapshot._restore_netcdf(self)
# initialize arrays used for trajectories
Trajectory._restore_netcdf(self)
return
def _identify_pattern(self, previous, current, next, cell):
"""
Given three consecutive locations(from the same trace) identify a pair of numbers
indicating the source_cell and the destination_cell relative the the following diagram
where current would be 'x'.
|---|---|---|
| 0 | 1 | 2 |
|---|---|---|
| 3 | x | 4 |
|---|---|---|
| 5 | 6 | 7 |
|---|---|---|
If a pattern is found then the current position is added to a list in the patterns dictionary
"""
#get the total distance traveled
total_distance = Trajectory.distance(previous.latitude, previous.longitude, current.latitude, current.longitude) + Trajectory.distance(next.latitude, next.longitude, current.latitude, current.longitude)
in_speed = Trajectory.velocity(previous.latitude, previous.longitude, current.latitude, current.longitude, previous.time, current.time)
out_speed = Trajectory.velocity(current.latitude, current.longitude, next.latitude, next.longitude, current.time, next.time)
# If the distance traveled between the three points is moer than 15 mts
if total_distance > 3 and in_speed < 30 and out_speed < 30:
in_bearing = Trajectory.initial_heading(previous.latitude, previous.longitude, current.latitude, current.longitude)
out_bearing = Trajectory.initial_heading(current.latitude, current.longitude, next.latitude, next.longitude)
#Get a cardinal representation of the bearing
in_direction = Trajectory.direction((in_bearing + 180) % 360) #reverse the bearing to get the incoming direction
out_direction = Trajectory.direction(out_bearing)
#Create a new key on the dictionary if the pattern hasn't been encountered before
if (in_direction, out_direction) not in cell.patterns:
cell.patterns[in_direction,out_direction] = []
cell.patterns[in_direction,out_direction].append(current)
def test_add_heading(self):
df = pd.DataFrame([{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(6, 0),
't': datetime(2018, 1, 1, 12, 10, 0)
}, {
'geometry': Point(6, -6),
't': datetime(2018, 1, 1, 12, 20, 0)
}, {
'geometry': Point(-6, -6),
't': datetime(2018, 1, 1, 12, 30, 0)
}]).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
traj.add_direction()
result = traj.df[DIRECTION_COL_NAME].tolist()
expected_result = [90.0, 90.0, 180.0, 270]
self.assertEqual(expected_result, result)
def __init__(self, dyn, x0, color='yellow', T=5):
self.data0 = {'x0': x0}
self.bounds = [(-1., 1.), (-1., 1.)]
self.T = T
self.dyn = dyn
self.traj = Trajectory(T, dyn)
self.traj.x0.set_value(x0)
self.linear = Trajectory(T, dyn)
self.linear.x0.set_value(x0)
self.color = color
self.default_u = np.zeros(self.dyn.nu)
def test_split_by_daybreak_same_day_of_year(self):
df = pd.DataFrame([{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(-6, 10),
't': datetime(2018, 1, 1, 12, 1, 0)
}, {
'geometry': Point(6, 6),
't': datetime(2019, 1, 1, 12, 0, 1)
}, {
'geometry': Point(6, 16),
't': datetime(2019, 1, 1, 12, 5, 1)
}]).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
split = traj.split_by_date()
result = len(split)
expected_result = 2
self.assertEqual(expected_result, result)
def test_split_by_observation_gap(self):
df = pd.DataFrame([{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(-6, 10),
't': datetime(2018, 1, 1, 12, 1, 0)
}, {
'geometry': Point(6, 6),
't': datetime(2018, 1, 1, 12, 5, 0)
}, {
'geometry': Point(6, 16),
't': datetime(2018, 1, 1, 12, 6, 30)
}]).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
split = traj.split_by_observation_gap(timedelta(seconds=120))
result = len(split)
expected_result = 2
self.assertEqual(expected_result, result)
def run(self):
print("Input task started")
print("[q = 1,2,3,4,5,6,7,8 or]\n[q0 = pi/2]\n\n")
#Thread is daemonic, so this loop will only stop when the main thread is stopped
while(True):
try:
#Waits for an input.
#If the input is joint angles, create a trajectory and add it to the queue
q_f = parser.parse_input()
trajectory = Trajectory(q_f)
if trajectory.is_valid():
self.trajectory_queue.put(trajectory)
else:
print("Trajectory not valid")
#trajectory_queue.join()
#self.master_board.terminate()
#Should stop the robot where it is, but not shut down the master board
#If parser detects one of these specified inputs, the specified action will happen
except ParseError as e:
if e.msg == "Exit":
self.master_board_task.terminate()
if e.msg == "Position":
self.master_board_task.set_print()
if e.msg == "Calibrate":
if self.generated == False:
print("Calibration initated, type c to go to next point")
calibration_generator = config.Calibration_parameters(0)
self.generated = True
else:
try:
trajectory = Trajectory(next(calibration_generator))
self.trajectory_queue.put(trajectory)
except StopIteration:
print("Calibration Done")
self.generated = False
def setUp(self):
start_position = data['start_position']
end_position = data['end_position']
self.start_position = start_position
self.end_position = end_position
self.start_time = data['start_time']
self.end_time = data['end_time']
self.continuity_limit = data['continuity_limit']
robot = "Dummy data"
self.third_time = (self.end_time - self.start_time) / 3.
self.tr = Trajectory(start_position, end_position, robot,
self.start_time, self.end_time)
def test_get_position_interpolated_at_timestamp(self):
df = pd.DataFrame([{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(6, 0),
't': datetime(2018, 1, 1, 12, 10, 0)
}, {
'geometry': Point(10, 0),
't': datetime(2018, 1, 1, 12, 20, 0)
}]).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
result = traj.get_position_at(datetime(2018, 1, 1, 12, 14, 0),
method="interpolated")
expected_result = Point(6 + 4 / 10 * 4, 0)
self.assertEqual(expected_result, result)
result = traj.get_position_at(datetime(2018, 1, 1, 12, 15, 0),
method="interpolated")
expected_result = Point(6 + 4 / 10 * 5, 0)
self.assertEqual(expected_result, result)
def test_f_multidim(self):
from trajectory import Trajectory
from measurements import get_measurements, create_anchors
anchors = create_anchors(dim=2, n_anchors=5)
trajectory = Trajectory(n_complexity=4, dim=2)
basis, D_topright = get_measurements(trajectory, anchors, n_samples=10)
eps = 1e-10
self.assertTrue(np.all(abs(f_multidim(anchors, basis, D_topright, trajectory.coeffs)) < eps))
self.assertTrue(abs(f_onedim(anchors, basis, D_topright, trajectory.coeffs)) < eps)
coeffs_hat = np.squeeze(compute_exact(D_topright, anchors, basis))
np.testing.assert_allclose(coeffs_hat, trajectory.coeffs)
def test_intersection_with_numerical_time_issues(self):
xmin, xmax, ymin, ymax = 116.36850352835575, 116.37029459899574, 39.904675309969896, 39.90772814977718
polygon = Polygon([(xmin, ymin), (xmin, ymax), (xmax, ymax),
(xmax, ymin), (xmin, ymin)])
data = [{
'geometry': Point(116.36855, 39.904926),
't': datetime(2009, 3, 10, 11, 3, 35)
}, {
'geometry': Point(116.368612, 39.904877),
't': datetime(2009, 3, 10, 11, 3, 37)
}, {
'geometry': Point(116.368644, 39.90484),
't': datetime(2009, 3, 10, 11, 3, 39)
}]
df = pd.DataFrame(data).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
intersection = traj.intersection(polygon)[0]
result = intersection.to_linestring().wkt
expected_result = "LINESTRING (116.36855 39.904926, 116.368612 39.904877, 116.368644 39.90484)"
self.assertEqual(result, expected_result)
def test_get_linestring_between(self):
df = pd.DataFrame([{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(10, 0),
't': datetime(2018, 1, 1, 12, 10, 0)
}, {
'geometry': Point(20, 0),
't': datetime(2018, 1, 1, 12, 20, 0)
}, {
'geometry': Point(30, 0),
't': datetime(2018, 1, 1, 12, 30, 0)
}]).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
result = traj.get_linestring_between(
datetime(2018, 1, 1, 12, 5, 0), datetime(2018, 1, 1, 12, 25, 0,
50)).wkt
expected_result = "LINESTRING (10 0, 20 0)"
self.assertEqual(result, expected_result)
def __init__(self, env, K, T):
self.env = env
self.T = T # time steps per sequence
self.predictor = Predictor(1, 2, 1, self.T)
# self.predictor = Predictor(1, 1, 2, self.T)
self.trajectory = Trajectory(self.env)
self.trajectory.reset()
self.K = K # K sample action sequences
# self.T = T # time steps per sequence
self.lambd = 1
# self.dim_u = self.env.action_space.sample().shape[0]
self.dim_u = 2
self.U = np.zeros([self.T, self.dim_u])
self.time_limit = self.env._max_episode_steps
self.u_init = np.zeros([self.dim_u])
self.cost = np.zeros([self.K])
self.noise = np.random.normal(loc=0,
scale=1,
size=(self.K, self.T, self.dim_u))
def associate_path(track_path):
result = None
preds_list = get_path_predicate_score(track_path)
for each_node in track_path:
if each_node.duration != [0, 0]:
sub, obj = each_node.so_labels
pstart, pend = each_node.duration
conf_score = each_node.score
straj = Trajectory(pstart, pend, each_node.subj_tracklet,
conf_score)
otraj = Trajectory(pstart, pend, each_node.obj_tracklet,
conf_score)
for each_pred, pred_score in preds_list:
if result is None:
result = VideoRelation(sub, each_pred, obj, straj, otraj,
conf_score + pred_score)
else:
result.extend(straj, otraj, conf_score + pred_score)
return result
def compute_metric(traj_filename1, traj_filename2):
print("loading %s" % traj_filename1)
traj1 = Trajectory()
traj1.load_from_file(traj_filename1)
# print(traj1.trajectory_array)
# print(len(traj1.get_length_normalized(15, as_array=True)))
# exit()
print("loading %s" % traj_filename2)
traj2 = Trajectory()
traj2.load_from_file(traj_filename2)
print(".....................................")
print("DTW-MT between two trajectories: %f" % compute_dtw_mt(traj1, traj2))
def test_douglas_peucker(self):
df = pd.DataFrame([{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(1, 0.1),
't': datetime(2018, 1, 1, 12, 6, 0)
}, {
'geometry': Point(2, 0.2),
't': datetime(2018, 1, 1, 12, 10, 0)
}, {
'geometry': Point(3, 0),
't': datetime(2018, 1, 1, 12, 30, 0)
}, {
'geometry': Point(3, 3),
't': datetime(2018, 1, 1, 13, 0, 0)
}]).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
result = traj.generalize(mode='douglas-peucker', tolerance=1)
self.assertEqual('LINESTRING (0 0, 3 0, 3 3)',
result.to_linestring().wkt)
class TestSolvers(unittest.TestCase):
def setUp(self):
self.n_anchors = 4
self.traj = Trajectory(n_complexity=5, dim=2)
self.basis = []
self.D_topright = []
def set_measurements(self, seed=1):
# random trajectory and anchors
self.traj.set_coeffs(seed=seed)
np.random.seed(seed)
self.anchors = create_anchors(self.traj.dim, self.n_anchors)
# get measurements
self.basis, self.D_topright = get_measurements(self.traj,
self.anchors,
seed=seed,
n_samples=20)
def test_semidef_relaxation_noiseless(self):
""" Check noiseless error. """
for i in range(10):
self.set_measurements(seed=i)
# check noiseless methods.
X = semidef_relaxation_noiseless(self.D_topright,
self.anchors,
self.basis,
chosen_solver=CVXOPT,
verbose=False)
coeffs_est = X[:DIM:, DIM:]
np.testing.assert_array_almost_equal(X[:DIM:, :DIM],
np.eye(DIM),
decimal=1)
np.testing.assert_array_almost_equal(coeffs_est,
self.traj.coeffs,
decimal=1)
def integrate(self, ic, tInt, t0=0., propagator=None):
"""
Call to the model propagator
Launcher.integrate(ic, tInt)
ic : initial condition <numpy.ndarray>
tInt: integration time <float>
"""
if not isinstance(ic, np.ndarray):
raise TypeError("ic <numpy.ndarray>")
if ic.ndim <> 1 or ic.size <> self.grid.N:
raise ValueError("ic.shape = (grid.N,)")
if ic.dtype<>'float64':
raise ValueError('Potential loss of precision')
self._nDt=int(tInt/self.dt)
if self._nDt*self.dt < tInt :
self._nDt+=1
self._tInt=self._nDt*self.dt
# Initialisation
traj=Trajectory(self.grid)
traj.initialize(ic.copy(), self._nDt, self.dt)
# calling the propagator
if propagator==None:
traj=self.propagator(traj.ic, traj, t0=t0)
else:
traj=propagator(traj.ic, traj, t0=t0)
if traj.getData().dtype<>'float64':
raise RuntimeError('Potential loss of precision')
return traj
def load_trajectories(file_name: str) -> List[Trajectory]:
# create trajectories
ids = load_trajectories_from_file(file_name)
values = id_value_dict()
out: List[Trajectory] = []
# Create nodes
# trajectory_id = index
for i in range(len(ids)):
nodes = [Node(id=id, value=values.get(id), trajectory_id=i, index=index) \
for index, id in enumerate(ids[i])]
out.append(Trajectory(id=i, nodes=nodes))
return out
def test_two_intersections_with_same_polygon(self):
polygon = Polygon([(5, -5), (7, -5), (7, 12), (5, 12), (5, -5)])
data = [{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(6, 0),
't': datetime(2018, 1, 1, 12, 6, 0)
}, {
'geometry': Point(10, 0),
't': datetime(2018, 1, 1, 12, 10, 0)
}, {
'geometry': Point(10, 10),
't': datetime(2018, 1, 1, 12, 30, 0)
}, {
'geometry': Point(0, 10),
't': datetime(2018, 1, 1, 13, 0, 0)
}]
df = pd.DataFrame(data).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
intersections = traj.intersection(polygon)
# spatial
result = []
for x in intersections:
result.append(x.to_linestring().wkt)
expected_result = [
"LINESTRING (5 0, 6 0, 7 0)", "LINESTRING (7 10, 5 10)"
]
self.assertEqual(result, expected_result)
# temporal
result = []
for x in intersections:
result.append((x.get_start_time(), x.get_end_time()))
expected_result = [(datetime(2018, 1, 1, 12, 5,
0), datetime(2018, 1, 1, 12, 7, 0)),
(datetime(2018, 1, 1, 12, 39,
0), datetime(2018, 1, 1, 12, 45, 0))]
self.assertEqual(result, expected_result)
def integrate(self, ic, tInt, t0=0., propagator=None):
"""
Call to the model propagator
Launcher.integrate(ic, tInt)
ic : initial condition <numpy.ndarray>
tInt: integration time <float>
"""
if not isinstance(ic, np.ndarray):
raise TypeError("ic <numpy.ndarray>")
if ic.ndim <> 1 or ic.size <> self.grid.N:
raise ValueError("ic.shape = (grid.N,)")
if ic.dtype <> 'float64':
raise ValueError('Potential loss of precision')
self._nDt = int(tInt / self.dt)
if self._nDt * self.dt < tInt:
self._nDt += 1
self._tInt = self._nDt * self.dt
# Initialisation
traj = Trajectory(self.grid)
traj.initialize(ic.copy(), self._nDt, self.dt)
# calling the propagator
if propagator == None:
traj = self.propagator(traj.ic, traj, t0=t0)
else:
traj = propagator(traj.ic, traj, t0=t0)
if traj.getData().dtype <> 'float64':
raise RuntimeError('Potential loss of precision')
return traj
def main():
"""Initialize a list of trajectory objects"""
segments = [{
'segID': 0,
'length': 25.,
'year': 2019,
'month': 6,
'day': 17,
'hour': 12,
'minutes': 1,
'seconds': 30,
'latitude': 40.,
'longitude': -100.,
'freeboard': 1.4,
}, {
'segID': 1,
'length': 30.,
'year': 2019,
'month': 6,
'day': 17,
'hour': 12,
'minutes': 2,
'seconds': 35,
'latitude': 40.5,
'longitude': -100.6,
'freeboard': 1.2,
}, {
'segID': 2,
'length': 17.,
'year': 2019,
'month': 6,
'day': 17,
'hour': 13,
'minutes': 2,
'seconds': 35,
'latitude': 45,
'longitude': -101,
'freeboard': 2.1,
}]
trajectory_list = [
Trajectory(
seg['segID'], seg['length'],
dt.datetime(seg['year'], seg['month'], seg['day'], seg['hour'],
seg['minutes'], seg['seconds']), seg['latitude'],
seg['longitude'], seg['freeboard']) for seg in segments
]
for t in trajectory_list:
print(t)
class NestedOptimizerCar(Car):
def __init__(self, *args, **vargs):
Car.__init__(self, *args, **vargs)
self.bounds = [(-3., 3.), (-2., 2.)]
@property
def human(self):
return self._human
@human.setter
def human(self, value):
self._human = value
self.traj_h = Trajectory(self.T, self.human.dyn)
def move(self):
Car.move(self)
self.traj_h.tick()
@property
def rewards(self):
return self._rewards
@rewards.setter
def rewards(self, vals):
self._rewards = vals
self.optimizer = None
def control(self, steer, gas):
import ipdb
ipdb.set_trace()
if self.optimizer is None:
reward_h, reward_r = self.rewards
reward_h = self.traj_h.total(reward_h)
reward_r = self.traj.total(reward_r)
self.optimizer = utils.NestedMaximizer(reward_h, self.traj_h.u,
reward_r, self.traj.u)
self.traj_h.x0.set_value(self.human.x)
self.optimizer.maximize(bounds=self.bounds)
def test_intersection_with_duplicate_traj_points(self):
polygon = Polygon([(5, -5), (7, -5), (7, 5), (5, 5), (5, -5)])
data = [{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(6, 0),
't': datetime(2018, 1, 1, 12, 6, 0)
}, {
'geometry': Point(6, 0),
't': datetime(2018, 1, 1, 12, 7, 0)
}, {
'geometry': Point(10, 0),
't': datetime(2018, 1, 1, 12, 11, 0)
}, {
'geometry': Point(10, 10),
't': datetime(2018, 1, 1, 12, 30, 0)
}, {
'geometry': Point(0, 10),
't': datetime(2018, 1, 1, 13, 0, 0)
}]
df = pd.DataFrame(data).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
intersections = traj.intersection(polygon)
# spatial
result = []
for x in intersections:
result.append(x.to_linestring())
expected_result = [LineString([(5, 0), (6, 0), (6, 0), (7, 0)])]
self.assertEqual(result, expected_result)
# temporal
result = []
for x in intersections:
result.append((x.get_start_time(), x.get_end_time()))
expected_result = [(datetime(2018, 1, 1, 12, 5,
0), datetime(2018, 1, 1, 12, 8, 0))]
self.assertEqual(result, expected_result)
class Car(object):
def __init__(self, dyn, x0, color='yellow', T=5):
self.data0 = {'x0': x0}
self.bounds = [(-1., 1.), (-1., 1.)]
self.T = T
self.dyn = dyn
self.traj = Trajectory(T, dyn)
self.traj.x0.set_value(x0)
self.linear = Trajectory(T, dyn)
self.linear.x0.set_value(x0)
self.color = color
self.default_u = np.zeros(self.dyn.nu)
def reset(self):
self.traj.x0.set_value(self.data0['x0'])
self.linear.x0.set_value(self.data0['x0'])
for t in range(self.T):
self.traj.u[t].set_value(np.zeros(self.dyn.nu))
self.linear.u[t].set_value(self.default_u)
def move(self):
self.traj.tick()
self.linear.x0.set_value(self.traj.x0.get_value())
@property
def x(self):
return self.traj.x0.get_value()
@property
def u(self):
return self.traj.u[0].get_value()
@u.setter
def u(self, value):
self.traj.u[0].set_value(value)
def control(self, steer, gas):
pass
def __init__(self,
time,
duration=1,
left_trajectory=None,
right_trajectory=None):
provided_dt = np.average(np.diff(time[:, 0]))
self._dt = provided_dt * duration
self._trajectories = {}
if left_trajectory is not None:
self._trajectories['left'] = Trajectory('left')
rospy.on_shutdown(self._trajectories['left'].stop)
TrajectoryExecutor._add_points(time, self._dt,
self._trajectories['left'],
left_trajectory)
if right_trajectory is not None:
self._trajectories['right'] = Trajectory('right')
rospy.on_shutdown(self._trajectories['right'].stop)
TrajectoryExecutor._add_points(time, self._dt,
self._trajectories['right'],
right_trajectory)
def test_get_segment_between_existing_timestamps(self):
df = pd.DataFrame([
{'geometry': Point(0, 0), 't': datetime(2018, 1, 1, 12, 0, 0)},
{'geometry': Point(6, 0), 't': datetime(2018, 1, 1, 12, 10, 0)},
{'geometry': Point(10, 0), 't': datetime(2018, 1, 1, 12, 15, 0)},
{'geometry': Point(10, 10), 't': datetime(2018, 1, 1, 12, 30, 0)},
{'geometry': Point(0, 10), 't': datetime(2018, 1, 1, 13, 0, 0)}
]).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
result = traj.get_segment_between(datetime(2018, 1, 1, 12, 10, 0), datetime(2018, 1, 1, 12, 30, 0)).df
expected_result = pd.DataFrame([
{'geometry': Point(6, 0), 't': datetime(2018, 1, 1, 12, 10, 0)},
{'geometry': Point(10, 0), 't': datetime(2018, 1, 1, 12, 15, 0)},
{'geometry': Point(10, 10), 't': datetime(2018, 1, 1, 12, 30, 0)}
]).set_index('t')
pd.testing.assert_frame_equal(result, expected_result)
expected_result = pd.DataFrame([
{'geometry': Point(6, 0), 't': datetime(2018, 1, 1, 12, 10, 0)},
{'geometry': Point(10, 0), 't': datetime(2018, 1, 1, 12, 15, 0)},
{'geometry': Point(10, 10), 't': datetime(2018, 1, 1, 12, 30, 1)}
]).set_index('t')
self.assertNotEqual(expected_result.to_dict(), result.to_dict())
def polyfit_trajectory(self, pos_data, t):
x_pos = np.array([el.pos.x for el in pos_data])
y_pos = np.array([el.pos.y for el in pos_data])
times = np.array([el.time for el in pos_data])
# needed for trajectory rendering
self.t_st = times[0]
self.t_fn = max(times[-1], t)
# calculate quadratic approximation of the reference trajectory
x_poly = np.polyfit(times, x_pos, deg=2)
y_poly = np.polyfit(times, y_pos, deg=2)
known = Trajectory.from_poly(x_poly, y_poly)
return known, x_pos[-1], y_pos[-1], times[-1]
class NestedOptimizerCar(Car):
# skippa sa lange, dubbelkolla med elis om vi ska ha med den.
def __init__(self, *args, **vargs):
Car.__init__(self, *args, **vargs)
self.bounds = [(-3., 3.), (-2., 2.)]
@property
def human(self):
return self._human
@human.setter
def human(self, value):
self._human = value
self.traj_h = Trajectory(self.T, self.human.dyn)
def move(self):
Car.move(self)
self.traj_h.tick()
@property
def rewards(self):
return self._rewards
@rewards.setter
def rewards(self, vals):
self._rewards = vals
self.optimizer = None
def control(self, steer, gas):
if self.optimizer is None:
reward_h, reward_r = self.rewards
reward_h = self.traj_h.reward(reward_h)
reward_r = self.traj.reward(reward_r)
self.optimizer = utils.NestedMaximizer(reward_h, self.traj_h.u,
reward_r, self.traj.u)
self.traj_h.x0.set_value(self.human.x)
self.optimizer.maximize(bounds=self.bounds)
def test_clip_with_no_intersection(self):
polygon = Polygon([(105, -5), (107, -5), (107, 12), (105, 12),
(105, -5)])
df = pd.DataFrame([{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(6, 0),
't': datetime(2018, 1, 1, 12, 10, 0)
}, {
'geometry': Point(10, 0),
't': datetime(2018, 1, 1, 12, 15, 0)
}, {
'geometry': Point(10, 10),
't': datetime(2018, 1, 1, 12, 30, 0)
}, {
'geometry': Point(0, 10),
't': datetime(2018, 1, 1, 13, 0, 0)
}]).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '31256'})
traj = Trajectory(1, geo_df)
result = traj.clip(polygon)
expected_result = []
self.assertEqual(expected_result, result)
def cabs_filter(self):
"""
Default CABS-dock filtering method.
:return: trajectory.Trajectory instance with filtered out models, list of indeces of the models (in the input list).
"""
n_replicas = self.trajectory.coordinates.shape[0]
n_models = self.trajectory.coordinates.shape[1]
fromeach = int(self.N / n_replicas)
filtered_models = []
filtered_headers = []
filtered_total_ndx = []
for i, replica in enumerate(self.trajectory.coordinates):
energies = [
header.get_energy(
number_of_peptides=self.trajectory.number_of_peptides)
for header in self.trajectory.headers
if header.replica == i + 1
]
headers = [
header for header in self.trajectory.headers
if header.replica == i + 1
]
filtered_ndx = self.mdl_fltr(replica, energies, N=fromeach)
if len(filtered_models) == 0:
filtered_models = replica[filtered_ndx, :, :]
filtered_headers = [headers[k] for k in filtered_ndx]
else:
filtered_models = numpy.concatenate(
[filtered_models, replica[filtered_ndx, :, :]])
filtered_headers += [headers[k] for k in filtered_ndx]
filtered_total_ndx.extend(numpy.array(filtered_ndx) + i * n_models)
traj = Trajectory(self.trajectory.template,
numpy.array([filtered_models]), filtered_headers)
traj.number_of_peptides = self.trajectory.number_of_peptides
return traj, filtered_total_ndx
def assign_snapshot(self, snapshot):
'''
Assign a single snapshot to the cluster centers
Returns
-------
assignment : int
cluster IDs
distance: float
distance to cluster _generator in measure of the metric (RMSD)
'''
assignments, distances = self.assign(Trajectory([snapshot]))
return assignments[0], distances[0]
def main():
venueData = pd.read_csv("./VenueID_data.csv")
l = Localization(venueData)
trajectorydata = pd.read_csv("./trainTrajectory_final.csv")
#print(trajectorydata["VenueID"].describe())
t = Trajectory(trajectorydata)
usersgroup = l.grouping(groupNum)
testtrajectorydata = pd.read_csv("./testTrajectory_final.csv")
testTrajectory = Trajectory(testtrajectorydata)
models, dics = train(usersgroup=usersgroup, trajectory=t)
for i in range(0, len(models)):
output = open(
'./LocalizationModel/' + str(groupNum) + 'model_state' +
str(states) + "_" + str(i) + '.pkl', 'wb')
s = pickle.dump(models[i], output)
output.close()
for i in range(0, len(models)):
eval_loc_model(testTrajectory=testTrajectory,
model=models[i],
users=usersgroup[i],
dic=dics[i])
def __init__(self, env, K, T):
self.model = Model(3, 2, 100, 64, 64, 10, load_data=False)
self.model.load_weights(WEIGHTS_PATH)
self.env = env
self.T = T # time steps per sequence
self.predictor_args = [self.model, 1, 3, 1, self.T]
self.trajectory = Trajectory(self.env)
self.K = K # K sample action sequences
# self.T = T # time steps per sequence
self.lambd = 1
# self.dim_u = self.env.action_space.sample().shape[0]
self.dim_u = 2
self.U = np.zeros([self.T, self.dim_u])
self.base_act = np.zeros([self.T, self.dim_u])
self.time_limit = self.env._max_episode_steps
self.u_init = np.zeros([self.dim_u])
self.cost = np.zeros([self.K])
self.noise = np.random.normal(loc=0,
scale=1,
size=(self.K, self.T, self.dim_u))
def test_linear_prediction_south(self):
df = pd.DataFrame([{
'geometry': Point(0, 0),
't': datetime(2018, 1, 1, 12, 0, 0)
}, {
'geometry': Point(0, -10),
't': datetime(2018, 1, 1, 12, 1, 0)
}]).set_index('t')
geo_df = GeoDataFrame(df, crs={'init': '4326'})
traj = Trajectory(1, geo_df)
predictor = TrajectoryPredictor(traj)
result = predictor.predict_linearly(timedelta(minutes=1))
expected_result = Point(0, -20)
#print("Got {}, expected {}".format(result, expected_result))
self.assertTrue(result.almost_equals(expected_result))
def split_into_connections(self, traj):
c, i, d = self.assign_trajectory(traj)
l_traj = []
traj_mode = 0
t = Trajectory()
first = None
for index, snapshot in enumerate(traj):
if (c[index] != -1 and i[index]==0 and traj_mode == 0):
# core set
first = snapshot
elif (i[index] != 0 and first is not None):
# in void
t.forward(snapshot)
traj_mode = 1
elif (c[index] != -1 and i[index]==0 and traj_mode == 1):
t.insert(0, first)
t.forward(snapshot)
l_traj.forward(t)
t = Trajectory()
first = snapshot
traj_mode = 0
return l_traj
def convert_to_trajectory(self, route_name, stop_id):
# print("%(bus_name)s converting to trajectory" % {'bus_name': self.number})
segment_intervals = self.segment_intervals()
if None in segment_intervals: # not ready to be converted to trajectory; because a stop doesn't have time data.
print("%(bus_name)s trajectory conversion failed 1: %(segs)s " %{'bus_name': self.number, 'segs': segment_intervals})
return None
if not self.has_full_data:
print("%(bus_name)s trajectory conversion failed 2" % {'bus_name': self.number})
return None
# print("%(bus_name)s converted to trajectory with segment_intervals: " % {'bus_name': self.number})
# print(segment_intervals)
traj = Trajectory(route_name, stop_id, self.start_time)
traj.set_segment_intervals(segment_intervals)
traj.green_light_time = self.green_light_time
traj.red_light_time = self.red_light_time
traj.error = self.error
return traj
def trajectory(self, idx):
return Trajectory.load(idx)
def sampleTrajectory(self, trajectory):
"""
Conduct one step of transition path sampling MCMC to generate a (correlated) trajectory sample.
ARGUMENTS
trajectory (Trajectory) - a previous trajectory sample from the TPS ensemble
RETURN VALUES
trajectory (Trajectory) - new sampled trajectory (correlated with previous trajectory sample)
NOTE
Snapshot objects should be immutable!!!! At least where we can make sure of this.
Does it make sense to roll the probability to reject a trajectory before hand and set the maximal length accordingly.
If we will reject a too long trajectory anyway we can stop once it became too long, right?
"""
# Determine length of trajectory
nframes = len(trajectory)
xframes = self.simulator.n_frames_max
t = Trajectory()
t.pathHamiltonian()
# Compute ptah Hamiltonian
# H_old = trajectory.pathHamiltonian()
l_old = 1.0 * len(trajectory)
max_length_prop = math.ceil(l_old / np.random.rand())
if max_length_prop < xframes:
xframes = max_length_prop
# Stopper
stopper = self.interfaces.in_core
print "Using trajectory of length : ", nframes, " (max allowed : ", xframes, " )",
# Choose a shooting or shift move
SHOOT_PROBABILITY = 1.0 # probability of picking a shooting move or a shift move
if (np.random.rand() < SHOOT_PROBABILITY):
# Shoot part of a new trajectory
# Pick a timeslice to shoot from.
# TODO: This could be changed to more transition regions
frame_index = np.random.random_integers(1, nframes-2)
# Pick a shooting direction.
if (np.random.rand() < 0.5):
# Shoot forward.
print "Shooting forward from frame %d" % frame_index
l_max = xframes - frame_index - 1
partial_trajectory = self.simulator.generate(trajectory[frame_index], l_max, running = stopper)
print "Trial was ", len(partial_trajectory) + frame_index, " long"
if len(partial_trajectory) == l_max + 1:
print "Rejected. Too long"
trial_trajectory = trajectory
else:
print "Accepted."
trial_trajectory = trajectory[0:frame_index] + partial_trajectory
else:
# Shoot backwards
print "Shooting backward from frame %d" % frame_index
l_max = xframes - nframes + frame_index
partial_trajectory = self.simulator.generate(trajectory[frame_index], l_max, running = stopper)
print "Trial was ", len(partial_trajectory) + nframes - frame_index, " long"
if len(partial_trajectory) == l_max + 1:
print "Rejected. Too long"
trial_trajectory = trajectory
else:
print "Accepted."
partial_trajectory.reverse()
trial_trajectory = partial_trajectory[:-1] + trajectory[frame_index:]
else:
# Shift trajectory.
# Pick a timeslice to form start of new trajectory.
# Don't allow shifting by zero -- this screws with python indexing.
nshift = np.random.random_integers(1, nframes-2)
# Pick a shooting direction.
if (np.random.rand() < 0.5):
print "Shifting by +%d" % nshift
# Shoot forward from end.
partial_trajectory = self.simulator.generate(trajectory[-1], xframes - nframes + nshift, running = stopper)
trial_trajectory = trajectory[nshift:-1] + partial_trajectory
else:
# Shoot backwards from beginning.
print "Shifting by -%d" % nshift
partial_trajectory = self.simulator.generate(trajectory[0], xframes - nframes + nshift, running = stopper)
partial_trajectory.reverse()
trial_trajectory = partial_trajectory[:-1] + trajectory[0:-nshift]
# Compute new path Hamiltonian
# H_trial = trial_trajectory.pathHamiltonian()
# log_P_accept = - self.beta * (H_trial - H_old)
#print "log_P_accept = %f" % log_P_accept
return trial_trajectory
def trajectory_coordinates_as_array(self, idx, atom_indices=None):
frame_indices = Trajectory.load_indices(idx)
return self.snapshot_coordinates_as_array(frame_indices, atom_indices)
def IsProjectableReachableSet2(self, DeformInfo):
env = DeformInfo['env']
traj = DeformInfo['traj']
Wori = DeformInfo['Wori']
dWori = DeformInfo['dWori']
ddWori = DeformInfo['ddWori']
Ndim = DeformInfo['Ndim']
Nwaypoints = DeformInfo['Nwaypoints']
critical_pt = DeformInfo['critical_pt']
topp = traj.getTOPPTrajectoryWithoutForceField(env, Wori)
### strategy: execute blindly but reject forces
xt = topp.traj1
tc = self.GetTimeAtCriticalPoint( xt, Wori, critical_pt)
dt = Trajectory.DISCRETIZATION_TIME_STEP
dt = 0.001
Q0 = []
QD0 = []
Qori = []
Qdori = []
D = 0
t = 0
ictr=0
while t < tc:
Qori.append(xt.Eval(t))
Qdori.append(xt.Evald(t))
Q0.append(xt.Eval(t))
QD0.append(xt.Evald(t))
t+=dt
q = xt.Eval(tc)
dq = xt.Evald(tc)
while t < xt.duration:
Qori.append(xt.Eval(t))
Qdori.append(xt.Evald(t))
Q0.append(q)
QD0.append(dq)
#dt = topp.durationVector[ictr]
ictr+=1
qprev = copy.copy(q)
dqprev = copy.copy(dq)
F = traj.get_forces_at_waypoints(q, env)
### get component of force orthogonal to path
#dqn = xt.Evald(t)/np.linalg.norm(xt.Evald(t))
#FN = F - np.dot(F,dqn)*dqn
### the control we would apply if we execute the
### geometrical feasible path without forces
ddq = xt.Evaldd(t) + F
### Get a control which follows ddq as close as possible
### while rejecting the forces as much as possible.
#[qnew, ddq] = params.GetBestControlPathInvariant( q, dq, ddq, xt.Eval(t+dt), xt.Evald(t+dt), F, dt)
### simulate forward with new control
dt2 = 0.5*dt*dt
q = q + dt*dq + dt2*ddq
dq = dq + dt*ddq
t += dt
D+=np.linalg.norm(q-qprev)
print "overall dist",D
### do not move waypoint derivatives
Q0 = np.array(Q0).T
Qori = np.array(Qori).T
self.projectedWaypoints = Q0
#if np.linalg.norm(q-xt.Eval(xt.duration))<0.1:
# print "success"
# return True
#else:
# return False
### check if topp finds solution
np.savetxt("topp/segfault_W1",Q0)
print Q0.shape
t2 = Trajectory(Q0)
Nc = t2.getCriticalPoint(env)
np.savetxt("topp/segfault_W2",Qori)
print Qori.shape
t3 = Trajectory(Qori)
Nc3 = t3.getCriticalPoint(env)
traj2 = t2.topp.traj0
traj3 = t3.topp.traj0
Npts = 1e5
tvect2 = np.linspace(0,traj2.duration, Npts)
tvect3 = np.linspace(0,traj3.duration, Npts)
qvect2 = np.array([traj2.Eval(t) for t in tvect2]).T
qvect3 = np.array([traj3.Eval(t) for t in tvect3]).T
plt.plot(qvect2[0,:],qvect2[1,:],'-r',linewidth=3)
plt.plot(qvect3[0,:],qvect3[1,:],'-g',linewidth=3)
plt.plot(Q0[0,Nc],Q0[1,Nc],'or',markersize=10)
plt.plot(Qori[0,Nc3],Qori[1,Nc3],'og',markersize=10)
print "BEFORE SIMPLE PROJECTABILITY:",Nc3,"/",Qori.shape[1]
print "AFTER SIMPLE PROJECTABILITY:",Nc,"/",Q0.shape[1]
plt.show()
if Nc >= Q0.shape[1]-1:
return True
else:
return False
def _edges_to_kml(self, output = "default.kml"):
os.chdir("/home/moyano/Projects/Tracks/kml/")
sys.stdout.write("\nWriting output to kml ... ")
sys.stdout.flush()
_file = open(output,"w")
#Header
_file.write("<?xml version='1.0' encoding='UTF-8'?>\n")
_file.write( "<kml xmlns='http://www.opengis.net/kml/2.2'>\n")
_file.write( "<Document>\n")
#Line Style)
_file.write( "<Style id='myStyle'>\n")
_file.write( "<LineStyle>\n")
_file.write( "<color>7f00ff00</color>\n")
_file.write( "<width>1</width>\n")
_file.write( "</LineStyle>\n")
_file.write( "<PolyStyle>\n")
_file.write( "<color>7f00ff00</color>\n")
_file.write( "</PolyStyle>\n")
_file.write( "</Style>\n")
#Line Style)
_file.write( "<Style id='myStyle2'>\n")
_file.write( "<LineStyle>\n")
_file.write( "<color>ffff0000</color>\n")
_file.write( "<width>4</width>\n")
_file.write( "</LineStyle>\n")
_file.write( "<PolyStyle>\n")
_file.write( "<color>ff0000ff</color>\n")
_file.write( "</PolyStyle>\n")
_file.write( "</Style>\n")
_file.write( "<Style id='myStyle3'>\n")
_file.write( "<LineStyle>\n")
_file.write( "<color>df0000ff</color>\n")
_file.write( "<width>4</width>\n")
_file.write( "</LineStyle>\n")
_file.write( "<PolyStyle>\n")
_file.write( "<color>ff0000ff</color>\n")
_file.write( "</PolyStyle>\n")
_file.write( "</Style>\n")
count = 0
for edge in self.pattern_edges:
count +=1
_in = edge[0]
_out = edge[1]
_dir = Trajectory.direction(Trajectory.initial_heading(_in.latitude, _in.longitude, _out.latitude, _out.longitude))
_file.write( "<Placemark>\n")
_file.write( "<name>" + str(count) + "</name>\n")
if _dir == '0' or _dir == '1' or _dir == '2' or _dir == '4':
_file.write( "<styleUrl>#myStyle2</styleUrl>\n")
else:
_file.write( "<styleUrl>#myStyle3</styleUrl>\n")
_file.write( "<LineString>\n")
_file.write( "<altitudeMode>relative</altitudeMode>\n")
_file.write( "<coordinates>\n")
_file.write( str(_in.longitude) + "," + str(_in.latitude) + "\n")
_file.write( str(_out.longitude) + "," + str(_out.latitude) + "\n")
_file.write( "</coordinates>\n")
_file.write( "</LineString>\n")
_file.write( "</Placemark>\n")
#Print the grid
for k in self.all_nodes.iterkeys():
node = self.all_nodes[k]
_file.write( "<Placemark>\n")
_file.write( "<name>" + str(count) + "</name>\n")
_file.write( "<styleUrl>#myStyle</styleUrl>\n")
_file.write( "<LineString>\n")
_file.write( "<altitudeMode>relative</altitudeMode>\n")
_file.write( "<coordinates>\n")
_file.write( str(node.x0) + "," + str(node.y0) + "\n")
_file.write( str(node.x1) + "," + str(node.y0) + "\n")
_file.write( str(node.x1) + "," + str(node.y1) + "\n")
_file.write( str(node.x0) + "," + str(node.y1) + "\n")
_file.write( str(node.x0) + "," + str(node.y0) + "\n")
_file.write( "</coordinates>\n")
_file.write( "</LineString>\n")
_file.write( "</Placemark>\n")
#Close tags
_file.write( "</Document>\n")
_file.write( "</kml>\n")
_file.close()
sys.stdout.write("\nDone ... ")
sys.stdout.flush()
def _resume_from_netcdf(self):
"""
Resume execution by reading current positions and energies from a NetCDF file.
"""
# Open NetCDF file for reading
# ncfile = netcdf.NetCDFFile(self.store_filename, 'r') # for Scientific.IO.NetCDF
ncfile = netcdf.Dataset(self.store_filename, "r") # for netCDF4
# TODO: Perform sanity check on file before resuming
# Get current dimensions.
self.iteration = ncfile.variables["activities"].shape[0] - 1
self.nstates = ncfile.variables["activities"].shape[1]
self.n_frames = ncfile.variables["trajectory_coordinates"].shape[1]
self.natoms = ncfile.variables["trajectory_coordinates"].shape[2]
print "iteration = %d, nstates = %d, natoms = %d" % (self.iteration, self.nstates, self.natoms)
# Restore trajectories.
self.trajectories = list()
for replica_index in range(self.nstates):
trajectory = Trajectory()
for frame_index in range(self.n_frames):
x = (
ncfile.variables["trajectory_coordinates"][replica_index, frame_index, :, :]
.astype(numpy.float32)
.copy()
)
coordinates = Quantity(x, nanometers)
v = (
ncfile.variables["trajectory_velocities"][replica_index, frame_index, :, :]
.astype(numpy.float32)
.copy()
)
velocities = Quantity(v, nanometers / picoseconds)
V = ncfile.variables["trajectory_potential"][replica_index, frame_index]
potential_energy = Quantity(V, kilojoules_per_mole)
T = ncfile.variables["trajectory_kinetic"][replica_index, frame_index]
kinetic_energy = Quantity(T, kilojoules_per_mole)
snapshot = Snapshot(
coordinates=coordinates,
velocities=velocities,
kinetic_energy=kinetic_energy,
potential_energy=potential_energy,
)
trajectory.forward(snapshot)
self.trajectories.forward(trajectory)
# Restore state information.
self.replica_states = ncfile.variables["states"][self.iteration, :].copy()
# Restore log probabilities.
print "Reading log probabilities..." # DEBUG
print self.log_P_kl # DEBUG
self.log_P_kl = ncfile.variables["log_probabilities"][self.iteration, :, :]
print self.log_P_kl # DEBUG
# Restore activities
for replica_index in range(self.nstates):
state_index = self.replica_states[replica_index]
K_reduced_unit = self.ensembles[state_index].K_reduced_unit
K = ncfile.variables["activities"][self.iteration, replica_index]
self.activities[replica_index] = K * K_reduced_unit
# Restore path Hamiltonians
for replica_index in range(self.nstates):
state_index = self.replica_states[replica_index]
H_reduced_unit = self.ensembles[state_index].H_reduced_unit
H = ncfile.variables["path_hamiltonians"][self.iteration, replica_index]
self.path_hamiltonians[replica_index] = H * H_reduced_unit
# Close NetCDF file.
ncfile.close()
# Reopen NetCDF file for appending, and maintain handle.
# self.ncfile = netcdf.NetCDFFile(self.store_filename, 'a') # for Scientific.IO.NetCDF
self.ncfile = netcdf.Dataset(self.store_filename, "a") # for netCDF4
# DEBUG: Set number of iterations to be a bit more than we've done.
# self.number_of_iterations = self.iteration + 50
return
def run(self, samples_list, *args):
env = self.robot.rave.GetEnv()
tuningsvip = Kinodynamic.Tunings()
tau0 = self.tunings.torque_limits[0]
tau1 = self.tunings.torque_limits[1]
tuningsvip.limits = [numpy.array([-tau0, -tau1]),
numpy.array([tau0, tau1])]
tuningsvip.grav = env.GetPhysicsEngine().GetGravity()
tuningsvip.checkcoll = False
tuningsvip.t_step = 0.005
# time step to integrate the limiting curves:
tuningsvip.dt_integ = tuningsvip.t_step / 5
tuningsvip.disc_thr = 1e15
tuningsvip.width = 10
tuningsvip.palier = 10
tuningsvip.tolerance_ends = 1e-2
tuningsvip.dicho_steps = 10
tuningsvip.n_close = 10
tuningsvip.n_rand = 0
tuningsvip.try_zero = True
q_start = self.init_state.q
v_start = 0 # dummy
q_target = self.goal_state.q
v_target = numpy.linalg.norm(self.goal_state.qd)
robot = self.robot.rave
q_range = [] # dummy
K = self.tunings.max_iter
# Search for solution
res = Kinodynamic.Bi_CKRRT(robot, q_start, v_start, q_target, v_target,
q_range, K, tuningsvip, bidir=False,
samples_list=samples_list,
dirty_backref=self)
if res[0] == -1:
return None
# Process the results
ver_list = res[3]
pwp_traj_list = res[1]
traj_list = [pwp_traj.GetSampleTraj(pwp_traj.duration,
tuningsvip.t_step)
for pwp_traj in pwp_traj_list]
# Construct the trajectories
pb_list = []
T_list = [pwp_traj.duration for pwp_traj in pwp_traj_list]
traj2_list = []
for i in range(len(T_list)):
traj = traj_list[i]
pb = MintimeProblemTorque.MintimeProblemTorque(robot, traj)
pb.set_dynamics_limits(tuningsvip.limits)
pb.disc_thr = tuningsvip.disc_thr
pb.preprocess()
pb_list.append(pb)
# Integrate profiles
algo = MintimeProfileIntegrator.MintimeProfileIntegrator(pb)
algo.dt_integ = tuningsvip.dt_integ / 10
algo.width = tuningsvip.width
algo.palier = tuningsvip.palier
algo.tolerance_ends = tuningsvip.tolerance_ends
algo.sdot_init = 1e-4
algo.sdot_final = 1e-4
algo.integrate_all_profiles()
algo.integrate_final()
#algo.plot_profiles(sum(T_list[0:i]))
#algo.plot_profiles()
# Resample the trajectory
s_res = algo.s_res
sdot_res = algo.sdot_res
undersample_coef = int(round(traj.t_step / algo.dt_integ))
s_res_u = s_res[range(1, len(s_res), undersample_coef)]
sdot_res_u = sdot_res[range(1, len(s_res), undersample_coef)]
traj2 = pwp_traj_list[i].ResampleTraj(s_res_u, sdot_res_u,
traj.t_step)
traj2_list.append(traj2)
traj2_final = MintimeTrajectory.Concat(list(traj2_list))
trajres = Trajectory(traj2_final.duration,
q_fun=traj2_final.value,
qd_fun=traj2_final.velocity,
qdd_fun=traj2_final.acceleration)
self.plotting = (ver_list, q_start, q_target, traj2_list)
x = trajres.last_state()
last_state = self.State(q=x[:2], qd=x[2:])
if last_state == self.goal_state:
self.trajres = trajres
self.has_found = True
return trajres
return None
cog_handler = []
DEBUG=True
np.random.seed(0)
cog_in = np.loadtxt("trajectories/surfboard_com.txt",dtype='float')
q_in = np.loadtxt("trajectories/surfboard_q.txt",dtype='float')
h = env.env.drawlinestrip(points=cog_in,
linewidth=5,
colors=array((1,0,1,1)))
cog_handler.append(h)
from trajectory import Trajectory
ctraj = Trajectory(cog_in.T)
qtraj = Trajectory(q_in.T)
with env.env:
handler = []
for i in range(0,20):
qcom = np.loadtxt("trajectories/surfboard_com_"+str(i)+".txt",dtype='float')
qcom2 = np.loadtxt("trajectories/surfboard_com_all_"+str(i)+".txt",dtype='float')
h = VisualizeCOMSet(qcom, env, np.array((0.0,1.0,0.0,0.1)))
handler.append(h)
h = VisualizeCOMSet(qcom2, env, np.array((1.0,0.0,1.0,0.02)))
handler.append(h)
Mwaypoints=200
tvec = linspace(0,1,Mwaypoints)
def human(self, value):
self._human = value
self.traj_h = Trajectory(self.T, self.human.dyn)
def number_of_trajectories(self):
return Trajectory.load_number()
def all_trajectory_indices(self):
"""
Return a list of list of frame indices
"""
return Trajectory.load_all_indices()
