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