def snookerStep(func, priorFunc, \ X, XP, Z, \ fitVector=None, args=None): nChains, ndim = shape(X) M, nparms = shape(Z) XNEW = copy(X) XPNEW = copy(XP) if all(fitVector == None): parIndex = arange(ndim) else: parIndex = copy(fitVector) d = len(parIndex) #idz = arange(M) success = [] for i in range(nChains): x = copy(X[i]) xp = XP[i] # now pick random Z's for snooker idx = sampleIndexWithoutReplacement(M, 3) z = Z[idx[0]] xMz = x - z if abs(sum(xMz)) > 0: # project onto x-z zr1 = vecProj(Z[idx[1]], xMz) zr2 = vecProj(Z[idx[2]], xMz) gamma = uniform() * (2.2 - 1.2) + 1.2 diff = gamma * (zr1 - zr2) x[parIndex] = x[parIndex] + diff[parIndex] pf = priorFunc(x) xp = -inf if isfinite(pf): if args == None: xp = func(x) + pf else: xp = func(x, *args) + pf bias = pow(vnorm((x-z), 2) / \ vnorm((X[i] - z), 2), d-1) if log(uniform()) < xp - XP[i] + log(bias): # success! success += [1] XNEW[i] = copy(x) XPNEW[i] = xp else: success += [0] else: success += [0] else: success += [0] return XNEW, XPNEW, success
def snookerStep(func, priorFunc, \ X, XP, Z, \ fitVector=None, args=None): nChains, ndim = shape(X) M, nparms = shape(Z) XNEW = copy(X) XPNEW = copy(XP) bias = copy(XPNEW) if fitVector == None: parIndex = arange(ndim) else: parIndex = copy(fitVector) d = len(parIndex) #idz = arange(M) success = [] sfunc = partial(getLogProb, func, priorFunc, \ args) for i in range(nChains): x = copy(X[i]) xp = XP[i] # now pick random Z's for snooker idx = sampleIndexWithoutReplacement(M, 3) z = Z[idx[0]] xMz = x - z if abs(sum(xMz)) > 0: # project onto x-z zr1 = vecProj(Z[idx[1]], xMz) zr2 = vecProj(Z[idx[2]], xMz) gamma = uniform()*(2.2 - 1.2) + 1.2 diff = gamma * (zr1 - zr2) #x[parIndex] = x[parIndex] + diff[parIndex] XNEW[i, parIndex] = x[parIndex] + diff[parIndex] bias[i] = pow(vnorm((x-z), 2) / \ vnorm((X[i] - z), 2), d-1) pool = Pool(processes=2) XPNEW = array(pool.map(sfunc, XNEW)) pool.close() pool.join() alpha = XPNEW - XP + log(bias) dice = log(uniform(size=nChains)) success = dice < alpha XP[success] = XPNEW[success] X[success,:] = XNEW[success,:] success = list(success.astype(int)) return X, XP, success
def robot_trajectory(positions, lin_vel, angular_vel): ''' Returns (position_t, direction_t, linear_velocity_{t+1}, angular_velocity_{t+1}) ''' prev_dir = None from_pos = positions[:-1] to_pos = positions[1:] for fp, tp in zip(from_pos, to_pos): dir = (tp - fp) / vnorm(tp - fp) if prev_dir is not None: to_dir = dir dir = prev_dir # Try rotation, if it increases the projection then we are good. after_rot = R2D_angle(angular_vel).dot(prev_dir) after_rot_proj = to_dir.dot(after_rot) before_rot_proj = to_dir.dot(prev_dir) angular_vel = np.sign(after_rot_proj - before_rot_proj) * angular_vel # Checks if dir is still on the same side of to_dir as prev_dir # Uses the fact that cross product is a measure of sine of # differences in orientation. As long as sine of the two # differences is same, the product is +ve and the robot must # keep rotating otherwise we have rotated too far and we must # stop. while np.cross(dir, to_dir) * np.cross(prev_dir, to_dir) > 0: yield (pos, dir, 0, angular_vel) dir = R2D_angle(angular_vel).dot(dir) dir = to_dir #for i in range(nf+1): pos = fp vel = (tp - fp) * lin_vel / vnorm(tp - fp) # continue till pos is on the same side of tp as fp while np.dot((pos - tp), (fp - tp)) > 0: yield (pos, dir, lin_vel, 0) pos = pos + vel prev_dir = dir
def vecProj(z, x): # project vector z onto x. Returns projected vector nrm = vnorm(x, 2) unitVector = x / nrm return dot(z, x) * unitVector / nrm
def robot_trajectory(positions, lin_vel, angular_vel, circle_flag=False, r=20, center=np.array([0, 0]), nframes=30): ''' Returns (position_t, direction_t, linear_velocity_{t+1}, angular_velocity_{t+1}) ''' if not circle_flag: prev_dir = None from_pos = positions[:-1] to_pos = positions[1:] for fp, tp in zip(from_pos, to_pos): fp = np.array(fp) tp = np.array(tp) dir = (tp - fp) / vnorm(tp - fp) if prev_dir is not None: to_dir = dir dir = prev_dir # Try rotation, if it increases the projection then we are good. after_rot = rotmat_z(angular_vel).dot(prev_dir) after_rot_proj = to_dir.dot(after_rot) before_rot_proj = to_dir.dot(prev_dir) angular_vel = np.sign(after_rot_proj - before_rot_proj) * angular_vel # Checks if dir is still on the same side of to_dir as prev_dir # Uses the fact that cross product is a measure of sine of # differences in orientation. As long as sine of the two # differences is same, the product is +ve and the robot must # keep rotating otherwise we have rotated too far and we must # stop. while np.dot(np.cross(dir, to_dir), np.cross(prev_dir, to_dir)) > 0: yield (pos, dir, 0, angular_vel) dir = rotmat_z(angular_vel).dot(dir) dir = to_dir #for i in range(nf+1): pos = fp vel = (tp - fp) * lin_vel / vnorm(tp - fp) # continue till pos is on the same side of tp as fp while np.dot((pos - tp), (fp - tp)) > 0: yield (pos, np.arctan2(dir[1], dir[0]), lin_vel, 0) pos = pos + vel prev_dir = dir else: w_v = angular_vel theta = 0 cur = None for it in range(nframes): to_dir = np.array([ center[0] + r * np.cos(theta + it * w_v), center[1] + r * np.sin(theta + it * w_v), 0 ]) if cur is not None: # Assumption of moving in counter clockwise direction dir = (to_dir - cur) / vnorm(to_dir - cur) yield (to_dir, np.pi / 2 + theta + it * w_v, r * w_v, w_v) else: yield (to_dir, np.pi / 2, 0, 0) cur = to_dir
def __init__(self, pos, dir, maxangle, maxdist): self._pos = pos.reshape((2, 1)) self._dir = dir.reshape((2, 1)) / vnorm(dir) self._maxangle = maxangle self._maxdist = maxdist