def _get_force(self):
forces = np.zeros((self.peds.size(), 2))
if self.peds.has_group():
for group in self.peds.groups:
threshold = (len(group) - 1) / 2
member_pos = self.peds.pos()[group, :]
com = stateutils.center_of_mass(member_pos)
force_vec = com - member_pos
norms = stateutils.speeds(force_vec)
softened_factor = (np.tanh(norms - threshold) + 1) / 2
forces[group, :] += (force_vec.T * softened_factor).T
return forces * self.factor
def grad_r_ab(self, state, delta=1e-3):
"""Compute gradient wrt r_ab using finite difference differentiation."""
r_ab = self.r_ab(state)
speeds = stateutils.speeds(state)
desired_directions = stateutils.desired_directions(state)
dx = np.array([[[delta, 0.0]]])
dy = np.array([[[0.0, delta]]])
v = self.value_r_ab(r_ab, speeds, desired_directions)
dvdx = (self.value_r_ab(r_ab + dx, speeds, desired_directions) -
v) / delta
dvdy = (self.value_r_ab(r_ab + dy, speeds, desired_directions) -
v) / delta
# remove gradients from self-intereactions
np.fill_diagonal(dvdx, 0.0)
np.fill_diagonal(dvdy, 0.0)
return np.stack((dvdx, dvdy), axis=-1)
def speeds(self):
"""Return the speeds corresponding to a given state."""
return stateutils.speeds(self.state)
def __call__(self, state):
speeds = stateutils.speeds(state)
return self.value_r_ab(self.r_ab(state), speeds,
stateutils.desired_directions(state))