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