|
| 1 | +import jax |
| 2 | +import jax.numpy as jnp |
| 3 | +from tqdm import tqdm |
| 4 | + |
| 5 | +MAX_STEPS = 2_000 |
| 6 | +MAX_ABS_VALUE = 5 |
| 7 | + |
| 8 | + |
| 9 | +class SecondOrderSystem: |
| 10 | + def __init__(self, start_state, target_state, step_forward, step_backward, sample_velocity): |
| 11 | + self.start_state = start_state |
| 12 | + self.target_state = target_state |
| 13 | + self.step_forward = step_forward |
| 14 | + self.step_backward = step_backward |
| 15 | + self.sample_velocity = sample_velocity |
| 16 | + |
| 17 | + |
| 18 | +def one_way_shooting(system, trajectory, fixed_length, key): |
| 19 | + key = jax.random.split(key) |
| 20 | + |
| 21 | + # pick a random point along the trajectory |
| 22 | + point_idx = jax.random.randint(key[0], (1,), 1, len(trajectory) - 1)[0] |
| 23 | + # pick a random direction, either forward or backward |
| 24 | + direction = jax.random.randint(key[1], (1,), 0, 2)[0] |
| 25 | + |
| 26 | + # TODO: Fix correct dt in ps |
| 27 | + velocity = (trajectory[point_idx] - trajectory[point_idx - 1]) / 0.001 |
| 28 | + |
| 29 | + if direction == 0: |
| 30 | + trajectory = trajectory[:point_idx + 1] |
| 31 | + step_function = system.step_forward |
| 32 | + else: # direction == 1: |
| 33 | + trajectory = trajectory[point_idx:][::-1] |
| 34 | + step_function = system.step_backward |
| 35 | + |
| 36 | + steps = MAX_STEPS if fixed_length == 0 else fixed_length |
| 37 | + |
| 38 | + key, iter_key = jax.random.split(key[3]) |
| 39 | + while len(trajectory) < steps: |
| 40 | + key, iter_key = jax.random.split(key) |
| 41 | + point, velocity = step_function(trajectory[-1], velocity, iter_key) |
| 42 | + trajectory.append(point) |
| 43 | + |
| 44 | + if jnp.isnan(point).any() or jnp.isnan(velocity).any(): |
| 45 | + return False, trajectory |
| 46 | + |
| 47 | + # ensure that our trajectory does not explode |
| 48 | + if (jnp.abs(point) > MAX_ABS_VALUE).any(): |
| 49 | + return False, trajectory |
| 50 | + |
| 51 | + if system.start_state(trajectory[0]) and system.target_state(trajectory[-1]): |
| 52 | + if fixed_length == 0 or len(trajectory) == fixed_length: |
| 53 | + return True, trajectory |
| 54 | + return False, trajectory |
| 55 | + |
| 56 | + if system.target_state(trajectory[0]) and system.start_state(trajectory[-1]): |
| 57 | + if fixed_length == 0 or len(trajectory) == fixed_length: |
| 58 | + return True, trajectory[::-1] |
| 59 | + return False, trajectory |
| 60 | + |
| 61 | + return False, trajectory |
| 62 | + |
| 63 | + |
| 64 | +def two_way_shooting(system, trajectory, fixed_length, key): |
| 65 | + key = jax.random.split(key) |
| 66 | + |
| 67 | + # pick a random point along the trajectory |
| 68 | + point_idx = jax.random.randint(key[0], (1,), 1, len(trajectory) - 1)[0] |
| 69 | + point = trajectory[point_idx] |
| 70 | + # simulate forward from the point until max_steps |
| 71 | + |
| 72 | + steps = MAX_STEPS if fixed_length == 0 else fixed_length |
| 73 | + |
| 74 | + initial_velocity = system.sample_velocity(key[1]) |
| 75 | + |
| 76 | + key, iter_key = jax.random.split(key[2]) |
| 77 | + new_trajectory = [point] |
| 78 | + |
| 79 | + velocity = initial_velocity |
| 80 | + while len(new_trajectory) < steps: |
| 81 | + key, iter_key = jax.random.split(key) |
| 82 | + point, velocity = system.step_forward(new_trajectory[-1], velocity, iter_key) |
| 83 | + new_trajectory.append(point) |
| 84 | + |
| 85 | + if jnp.isnan(point).any() or jnp.isnan(velocity).any(): |
| 86 | + return False, trajectory |
| 87 | + |
| 88 | + # ensure that our trajectory does not explode |
| 89 | + if (jnp.abs(point) > MAX_ABS_VALUE).any(): |
| 90 | + return False, trajectory |
| 91 | + |
| 92 | + if system.start_state(point) or system.target_state(point): |
| 93 | + break |
| 94 | + |
| 95 | + velocity = initial_velocity |
| 96 | + while len(new_trajectory) < steps: |
| 97 | + key, iter_key = jax.random.split(key) |
| 98 | + point, velocity = system.step_backward(new_trajectory[0], velocity, iter_key) |
| 99 | + new_trajectory.insert(0, point) |
| 100 | + |
| 101 | + if jnp.isnan(point).any() or jnp.isnan(velocity).any(): |
| 102 | + return False, trajectory |
| 103 | + |
| 104 | + # ensure that our trajectory does not explode |
| 105 | + if (jnp.abs(point) > MAX_ABS_VALUE).any(): |
| 106 | + return False, trajectory |
| 107 | + |
| 108 | + if system.start_state(point) or system.target_state(point): |
| 109 | + break |
| 110 | + |
| 111 | + # throw away the trajectory if it's not the right length |
| 112 | + if fixed_length != 0 and len(new_trajectory) != fixed_length: |
| 113 | + return False, trajectory |
| 114 | + |
| 115 | + if system.start_state(new_trajectory[0]) and system.target_state(new_trajectory[-1]): |
| 116 | + return True, new_trajectory |
| 117 | + |
| 118 | + if system.target_state(new_trajectory[0]) and system.start_state(new_trajectory[-1]): |
| 119 | + return True, new_trajectory[::-1] |
| 120 | + |
| 121 | + return False, trajectory |
| 122 | + |
| 123 | + |
| 124 | +def mcmc_shooting(system, proposal, initial_trajectory, num_paths, key, fixed_length=0, warmup=50): |
| 125 | + # pick an initial trajectory |
| 126 | + trajectories = [initial_trajectory] |
| 127 | + |
| 128 | + with tqdm(total=num_paths + warmup, desc='warming up' if warmup > 0 else '') as pbar: |
| 129 | + while len(trajectories) <= num_paths + warmup: |
| 130 | + if len(trajectories) > warmup: |
| 131 | + pbar.set_description('') |
| 132 | + |
| 133 | + key, traj_idx_key, iter_key, accept_key = jax.random.split(key, 4) |
| 134 | + traj_idx = jax.random.randint(traj_idx_key, (1,), warmup + 1, len(trajectories))[0] |
| 135 | + # during warmup, we want an iterative scheme |
| 136 | + traj_idx = traj_idx if traj_idx < len(trajectories) else -1 |
| 137 | + |
| 138 | + found, new_trajectory = proposal(system, trajectories[traj_idx], fixed_length, iter_key) |
| 139 | + |
| 140 | + if not found: |
| 141 | + continue |
| 142 | + |
| 143 | + ratio = len(trajectories[-1]) / len(new_trajectory) |
| 144 | + # The first trajectory might have a very unreasonable length, so we skip it |
| 145 | + if len(trajectories) == 1 or jax.random.uniform(accept_key, shape=(1,)) < ratio: |
| 146 | + trajectories.append(new_trajectory) |
| 147 | + pbar.update(1) |
| 148 | + |
| 149 | + return trajectories[warmup + 1:] |
| 150 | + |
| 151 | + |
| 152 | +def unguided_md(system, initial_point, num_paths, key, fixed_length=0): |
| 153 | + trajectories = [] |
| 154 | + current_frame = initial_point.clone() |
| 155 | + current_trajectory = [] |
| 156 | + |
| 157 | + key, velocity_key = jax.random.split(key) |
| 158 | + velocity = system.sample_velocity(velocity_key) |
| 159 | + |
| 160 | + with tqdm(total=num_paths) as pbar: |
| 161 | + while len(trajectories) < num_paths: |
| 162 | + key, iter_key = jax.random.split(key) |
| 163 | + next_frame, velocity = system.step_forward(current_frame, velocity, iter_key) |
| 164 | + |
| 165 | + assert not jnp.isnan(next_frame).any() |
| 166 | + |
| 167 | + is_transition = not (system.start_state(next_frame) or system.target_state(next_frame)) |
| 168 | + if is_transition: |
| 169 | + if len(current_trajectory) == 0: |
| 170 | + current_trajectory.append(current_frame) |
| 171 | + |
| 172 | + if fixed_length != 0 and len(current_trajectory) > fixed_length: |
| 173 | + current_trajectory = [] |
| 174 | + is_transition = False |
| 175 | + else: |
| 176 | + current_trajectory.append(next_frame) |
| 177 | + elif len(current_trajectory) > 0: |
| 178 | + current_trajectory.append(next_frame) |
| 179 | + |
| 180 | + if fixed_length == 0 or len(current_trajectory) == fixed_length: |
| 181 | + if system.start_state(current_trajectory[0]) and system.target_state(current_trajectory[-1]): |
| 182 | + trajectories.append(current_trajectory) |
| 183 | + pbar.update(1) |
| 184 | + elif system.target_state(current_trajectory[0]) and system.start_state(current_trajectory[-1]): |
| 185 | + trajectories.append(current_trajectory[::-1]) |
| 186 | + pbar.update(1) |
| 187 | + current_trajectory = [] |
| 188 | + |
| 189 | + current_frame = next_frame |
| 190 | + |
| 191 | + return trajectories |
0 commit comments