def calc_repulsive_gradient_ge(gradient, goal, pose):
"""
calculation of movement vector based on repulsive gradient and
goal gradient
:param gradient: repulsive gradient message
:param goal: attractive gradient message (goal)
:param pose: position SoMessage of the robot
:return: movement vector
distance of influence of obstacle = goal_radius + diffusion
"""
v = Vector3()
# distance obstacle - agent
tmp = calc.delta_vector(pose.p, gradient.p)
# shortest distance considered (goal_radius of agent == size of agent)
d = np.linalg.norm([tmp.x, tmp.y, tmp.z]) - pose.goal_radius \
- gradient.goal_radius
if d <= 0:
v = Vector3(np.inf, np.inf, np.inf)
# calculate norm vector for direction
tmp = calc.unit_vector3(tmp)
# calculate repulsion vector - adjust sign/direction
if tmp.x != 0.0:
v.x *= tmp.x
if tmp.y != 0.0:
v.y *= tmp.y
if tmp.z != 0.0:
v.z *= tmp.z
elif 0 < d <= gradient.diffusion:
# unit vector obstacle - agent
tmp = calc.unit_vector3(tmp)
# distance agent - goal
d_goal = calc.get_gradient_distance(pose.p, goal.p) - \
pose.goal_radius - goal.goal_radius
# unit vector agent - goal
ag = calc.delta_vector(goal.p, pose.p)
ag = calc.unit_vector3(ag)
# closest distance to obstacle - diffusion
d_obs_diff = (1.0 / d) - (1.0 / gradient.diffusion)
# parameters
n = 1.0
eta = 1.0
# weighting rep1 and rep2
f_rep1 = eta * d_obs_diff * ((d_goal**n) / (d**n))
f_rep2 = eta * (n / 2.0) * np.square(d_obs_diff) * (d_goal**(n - 1))
v.x = f_rep1 * tmp.x + f_rep2 * ag.x
v.y = f_rep1 * tmp.y + f_rep2 * ag.y
v.z = f_rep1 * tmp.z + f_rep2 * ag.z
elif d > gradient.diffusion:
v = Vector3()
return v
def calc_attractive_gradient_ge(gradient, pose):
"""
calculate movement vector based on attractive gradient
normalized version same as _calc_attractive_gradient
:param gradient: attractive gradient message
:param pose: position SoMessage of the robot
:return: movement vector
"""
v = Vector3()
# distance goal - agent
tmp = calc.delta_vector(gradient.p, pose.p)
# shortest distance considered (goal_radius of agent == size of agent)
d = np.linalg.norm([tmp.x, tmp.y, tmp.z]) - pose.goal_radius \
- gradient.goal_radius
if d <= 0:
v = Vector3()
elif 0 < d <= gradient.diffusion:
v = calc.adjust_length(tmp, d)
elif d > gradient.diffusion:
if gradient.diffusion == 0:
v = calc.adjust_length(tmp, 1)
else:
v = calc.adjust_length(tmp, gradient.diffusion)
return v
def separation(agent, neighbors, r=1.0):
"""
Calculates separation steering force between agent and neighbors
:param agent: agent position and orientation
:param neighbors: neighbor positions and orientations
:param r: weighting parameter (1/r)
:return: normalized movement vector for separation
"""
sep = Vector3()
if len(neighbors) > 0:
# 1/r weighting
weight = 1.0 / r
for q in neighbors:
diff = calc.delta_vector(agent.p, q.p)
# shortest distance between two robots
dist = calc.vector_length(diff) - agent.goal_radius - q.goal_radius
if dist > 0.0:
diff = calc.unit_vector3(diff)
sep.x += weight * (diff.x / dist)
sep.y += weight * (diff.y / dist)
sep.z += weight * (diff.z / dist)
else: # two agents on top of each other: add random vector
tmp = np.random.rand(1, 3)[0]
dist = np.linalg.norm(tmp)
sep.x += weight * (tmp[0] / dist)
sep.y += weight * (tmp[1] / dist)
sep.z += weight * (tmp[2] / dist)
return sep
def calc_attractive_gradient(gradient, pose):
"""
calculate movement vector based on attractive gradient
:param gradient: attractive gradient message
:param pose: position SoMessage of the robot
:return: movement vector
"""
v = Vector3()
# distance goal - agent
tmp = calc.delta_vector(gradient.p, pose.p)
# shortest distance considered (goal_radius of agent == size of agent)
d = np.linalg.norm([tmp.x, tmp.y, tmp.z]) - pose.goal_radius \
- gradient.goal_radius
if d <= 0:
v = Vector3()
elif 0 < d <= gradient.diffusion:
# calculate magnitude of vector
magnitude = d / gradient.diffusion
v = calc.adjust_length(tmp, magnitude)
elif d > gradient.diffusion:
# calculate attraction vector
#v = calc.adjust_length(tmp, 1.0)#TODO Why this?
v = tmp
return v
def action_function(qj, qi, r, epsilon, a, b, avoidance_distance, h):
"""
calculation of action function
:param qj: Positin neighbor Vector3
:param qi: Position agent Vector3
:param r: view_distance / interaction range
:param epsilon: parameter of sigma norm (0,1)
:param a: parameter
:param b: parameter
0 < a <= b; c = |a-b|/np.sqrt(4ab)
:param h: parameter (0,1) specifying boundaries of bump function
:param avoidance_distance: distance between agents
:return: action function value
"""
dq = calc.delta_vector(qj, qi)
z = sigma_norm(epsilon, dq)
r_alpha = sigma_norm_f(epsilon, r)
d_alpha = sigma_norm_f(epsilon, avoidance_distance)
# calculate parameter c for action function
c = np.abs(a - b) / np.sqrt(4 * a * b)
z_phi = z - d_alpha
sigma = (z_phi + c) / (np.sqrt(1 + np.square(z_phi + c)))
phi = 0.5 * ((a + b) * sigma + (a - b))
phi_alpha = bump_function(z / r_alpha, h) * phi
return phi_alpha
def static_list_angle(self,
frameids,
view_angle_xy=None,
view_angle_yz=np.pi,
time=None):
"""
function determines all static gradients within view distance with a
certain frame ID & within a view angle,
only static gradients as pheromones are deposited in environment,
:param frameids: frame ID of agent data
:param view_angle_xy: angle in which agent can see pheromones on
x-y-plane (+/- from heading)
:param view_angle_yz: angle in which agent can see pheromones on
y-z-plane (+/- from heading)
:param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
:return: list of gradients
"""
if not self.ev_thread:
self._evaporate_buffer(time=time)
gradients = []
# no own position available
pose = self.get_last_position()
if pose is None:
rospy.logerr("No own positions available")
return gradients
# determine frames to consider
if not frameids:
frameids = self._static.keys()
# heading vector
heading = tf.transformations.quaternion_matrix([pose.q.x, pose.q.y,
pose.q.z, pose.q.w]).\
dot([pose.direction.x, pose.direction.y, pose.direction.z, 1])
heading = Vector3(heading[0], heading[1], heading[2])
static = copy.deepcopy(self._static)
for frame in frameids:
if frame in static.keys():
for element in static[frame]:
grad = calc.delta_vector(element.p, pose.p)
if calc.vector_length(
grad
) <= element.diffusion + element.goal_radius + self._view_distance:
if view_angle_xy is None or np.abs(calc.angle_between([grad.x, grad.y],
[heading.x, heading.y])) \
<= view_angle_xy:
if view_angle_xy is None or np.abs(calc.angle_between([grad.y, grad.z],
[heading.y,
heading.z])) \
<= view_angle_yz:
gradients.append(element)
return gradients
def move(self):
"""
calculates movement vector considering all gradients
:return: movement vector
"""
pose = self._buffer.get_own_pose()
result = None
# repulsive gradients
gradients = self._buffer.gradients(self.frames, self.static,
self.moving)
if pose:
if gradients:
result = Vector3()
for grdnt in gradients:
if grdnt.attraction == 1:
result = calc.add_vectors(result, gradient.
calc_attractive_gradient(
grdnt, pose))
elif grdnt.attraction == -1:
grad = gradient.calc_repulsive_gradient(grdnt, pose)
# robot position is within obstacle goal radius
# handle infinite repulsion
if grad.x == np.inf or grad.x == -1 * np.inf:
# create random vector with length (goal_radius +
# gradient.diffusion)
dv = calc.delta_vector(pose.p, grdnt.p)
if calc.vector_length(dv) == 0:
result = calc.add_vectors(result,
calc.random_vector(
grdnt.goal_radius
+ grdnt.diffusion
))
else:
result = calc.add_vectors(result,
calc.adjust_length(
dv,
grdnt.goal_radius
+ grdnt.diffusion
))
else:
result = calc.add_vectors(result, grad)
if not result:
return None
# adjust length
d = calc.vector_length(result)
if d > self.maxvel:
result = calc.adjust_length(result, self.maxvel)
elif 0 < d < self.minvel:
result = calc.adjust_length(result, self.minvel)
return result
def move(self):
"""
calculates movement vector to avoid all repulsive gradients
:return: movement vector
"""
pose = self._buffer.get_own_pose()
vector_repulsion = None
# repulsive gradients
gradients_repulsive = self._buffer.repulsive_gradients(self.frames,
self.static,
self.moving)
if pose:
if gradients_repulsive:
vector_repulsion = Vector3()
for grdnt in gradients_repulsive:
grad = gradient.calc_repulsive_gradient(grdnt, pose)
# robot position is within obstacle goal radius
# handle infinite repulsion
if grad.x == np.inf or grad.x == -1 * np.inf:
# create random vector with length (goal_radius +
# gradient.diffusion)
dv = calc.delta_vector(pose.p, grdnt.p)
if not vector_repulsion:
vector_repulsion = Vector3()
if calc.vector_length(dv) == 0:
vector_repulsion = calc.add_vectors(
vector_repulsion, calc.random_vector(
grdnt.goal_radius + grdnt.diffusion))
else:
vector_repulsion = calc.add_vectors(
vector_repulsion, calc.adjust_length(dv,
grdnt.goal_radius + grdnt.diffusion))
else:
vector_repulsion = calc.add_vectors(vector_repulsion,
grad)
if not vector_repulsion:
return None
# adjust length
d = calc.vector_length(vector_repulsion)
if d > self.maxvel:
vector_repulsion = calc.adjust_length(vector_repulsion, self.maxvel)
elif 0 < d < self.minvel:
vector_repulsion = calc.adjust_length(vector_repulsion, self.minvel)
return vector_repulsion
def adjacency_matrix(qj, qi, epsilon, r, h):
"""
calculate adjacency matrix element
:param qj: Position neighbor
:param qi: Position agent
:param epsilon: parameter of sigma norm (0,1)
:param r: view distance / interaction range
:param h: parameter (0,1) of bump_function
:return: adjacency matrix element aij
"""
dq = calc.delta_vector(qi, qj)
z = sigma_norm(epsilon, dq)
r_alpha = sigma_norm_f(epsilon, r)
return bump_function(z / r_alpha, h)
def move(self):
"""
calculates repulsion vector based on Fernandez-Marquez
:return: repulsion movement vector
"""
pose = self._buffer.get_own_pose()
view = self._buffer.agent_list(self.frames)
mov = None
if pose and view:
repulsion_radius = pose.diffusion + pose.goal_radius
mov = Vector3()
for el in view:
distance = calc.get_gradient_distance(el.p, pose.p) \
- el.goal_radius
if distance <= self._buffer.view_distance:
if distance > 0:
diff = repulsion_radius - distance
mov.x += (pose.p.x - el.p.x) * diff / distance
mov.y += (pose.p.y - el.p.y) * diff / distance
mov.z += (pose.p.z - el.p.z) * diff / distance
# enhancement of formulas to cover case when gradients are
# collided (agent is in goal radius of neighbor)
elif distance <= 0:
# create random vector with length=repulsion radius
if distance == - el.goal_radius:
mov = calc.add_vectors(mov, calc.random_vector(
repulsion_radius))
# use direction leading away if available
else:
mov = calc.add_vectors(mov, calc.adjust_length(
calc.delta_vector(pose.p, el.p),
repulsion_radius))
if not mov:
return None
if calc.vector_length(mov) > repulsion_radius:
mov = calc.adjust_length(mov, repulsion_radius)
return mov
def move(self):
"""
calculate movement vector
:return: movement vector
"""
pose = self._buffer.get_own_pose()
g = None
# strongest gradient
grad = self._buffer.strongest_gradient(self.frames, self.static,
self.moving)
if pose:
if grad:
if grad.attraction == 1:
g = gradient.calc_attractive_gradient(grad, pose)
elif grad.attraction == -1:
g = gradient.calc_repulsive_gradient(grad, pose)
# robot position is within obstacle goal radius
# handle infinite repulsion
if g.x == np.inf or g.x == -1 * np.inf:
# create random vector with length (goal_radius +
# gradient.diffusion)
dv = calc.delta_vector(pose.p, grad.p)
if calc.vector_length(dv) == 0:
g = calc.random_vector(grad.goal_radius +
grad.diffusion)
else:
g = calc.adjust_length(dv, grad.goal_radius
+ grad.diffusion)
if not g:
return None
# adjust length
d = calc.vector_length(g)
if d > self.maxvel:
g = calc.adjust_length(g, self.maxvel)
elif 0 < d < self.minvel:
g = calc.adjust_length(g, self.minvel)
return g
def vector_btw_agents(qi, qj, epsilon):
"""
vector between agents
:param qi: agent under consideration
:param qj: neighbor
:param epsilon: fixed parameter (0,1) of sigma_norm
:return: vector along the line btw qj and qi
"""
n = Vector3()
# dq = qj - qi
dq = calc.delta_vector(qj, qi)
# denominator of calculation
denom = np.sqrt(1 + epsilon * np.square(calc.vector_length(dq)))
n.x = dq.x / denom
n.y = dq.y / denom
n.z = dq.z / denom
return n
def cohesion(agent, neighbors):
"""
calculates cohesion steering force
average of neighbor positions
:param agent: agent position and orientation
:param neighbors: list of neighbor positions and orientations
:return: norm vector steering towards average position of neighbors
"""
coh = Vector3()
for q in neighbors:
coh = calc.add_vectors(coh, q.p)
if len(neighbors) > 0:
coh.x /= len(neighbors)
coh.y /= len(neighbors)
coh.z /= len(neighbors)
coh = calc.delta_vector(coh, agent.p)
return coh
def move(self):
"""
calculates repulsion vector based on agent gradients
:return: repulsion movement vector
"""
pose = self._buffer.get_own_pose()
view = self._buffer.agent_list(self.frames)
repulsion = None
if pose and view:
repulsion_radius = pose.diffusion + pose.goal_radius
repulsion = Vector3()
for el in view:
grad = gradient.calc_repulsive_gradient(el, pose)
# case: agents are collided
if abs(grad.x) == np.inf or abs(grad.y) == np.inf or \
abs(grad.z) == np.inf:
dv = calc.delta_vector(pose.p, el.p)
if calc.vector_length(dv) == 0:
repulsion = calc.add_vectors(repulsion,
calc.random_vector(
repulsion_radius))
else:
repulsion = calc.add_vectors(repulsion,
calc.adjust_length(
dv, repulsion_radius))
else:
repulsion = calc.add_vectors(repulsion, grad)
if not repulsion:
return None
if calc.vector_length(repulsion) > repulsion_radius:
repulsion = calc.adjust_length(repulsion, repulsion_radius)
return repulsion
def calc_repulsive_gradient(gradient, pose):
"""
calc movement vector based on repulsive gradient
:param gradient: repulsive gradient message
:param pose: position SoMessage of the robot
:return: movement vector
"""
v = Vector3()
# distance goal - agent
tmp = calc.delta_vector(pose.p, gradient.p)
# shortest distance considered (goal_radius of agent == size of agent)
d = np.linalg.norm([tmp.x, tmp.y, tmp.z]) - pose.goal_radius \
- gradient.goal_radius
if d <= 0 or gradient.diffusion == np.inf: # infinitely large repulsion
v = Vector3(np.inf, np.inf, np.inf)
# calculate norm vector for direction
tmp = calc.unit_vector3(tmp)
# calculate repulsion vector / adjust sign/direction
if tmp.x != 0.0:
v.x *= tmp.x
if tmp.y != 0.0:
v.y *= tmp.y
if tmp.z != 0.0:
v.z *= tmp.z
elif 0 < d <= gradient.diffusion:
# calculate magnitude of vector
magnitude = (gradient.diffusion - d) / gradient.diffusion
# calculate vector
v = calc.adjust_length(tmp, magnitude)
elif d > gradient.diffusion:
v = Vector3()
return v
def agent_velocity(p1, p2):
"""
calculate velocity of agent
:param p1: gradient 1 (soMessage)
:param p2: gradient 2 (soMessage)
:return: current agent velocity
"""
v = calc.delta_vector(p1.p, p2.p)
# delta t in secs
dt = p1.header.stamp.secs - p2.header.stamp.secs
dt += (p1.header.stamp.nsecs - p2.header.stamp.nsecs) / 1000000000.0
if dt == 0.0:
return Vector3()
# velocity = delta distance / delta time
v.x /= dt
v.y /= dt
v.z /= dt
return v
def velocity_consensus(neighbors, agent, epsilon, r, h):
"""
calculates velocity consensus term
:param neighbors: array of neighbors of agent i (tuple position - velocity)
:param agent: agent under consideration (position and velocity)
:param epsilon: sigma norm parameter (0,1)
:param r: view distance / interaction range
:param h: parameter specifying boundaries of bump function
:return: velocity consensus term for flocking
"""
v = Vector3()
for q in neighbors:
# needs velocity - delta velocity
dp = calc.delta_vector(q.v, agent.v)
# needs position
aij = adjacency_matrix(q.p, agent.p, epsilon, r, h)
v.x += aij * dp.x
v.y += aij * dp.y
v.z += aij * dp.z
return v
def move(self):
"""
calculates movement vector, handling all gradients as repulsive
:return: movement vector
"""
# spread pheromone
self.spread()
pose = self._buffer.get_own_pose()
result = None
# repulsive gradients
gradients = self._buffer.gradients(self.frames, self.static,
self.moving)
if pose:
if gradients:
result = Vector3()
if len(gradients) > 1:
for grdnt in gradients:
grad = gradient.calc_repulsive_gradient(grdnt, pose)
# robot position is within obstacle goal radius
# handle infinite repulsion
if grad.x == np.inf or grad.x == -1 * np.inf:
# create random vector with length (goal_radius +
# gradient.diffusion)
dv = calc.delta_vector(pose.p, grdnt.p)
dv = calc.adjust_length(
dv, grdnt.goal_radius + grdnt.diffusion)
result = calc.add_vectors(result, dv)
else:
result = calc.add_vectors(result, grad)
else:
grdnt = gradients[0]
result = calc.random_vector(grdnt.goal_radius +
grdnt.diffusion)
else:
rospy.logwarn("No gradients of frames '%s' available",
self.frames)
else:
rospy.logwarn("No own pose available!")
if not result:
rospy.logwarn("No gradient vector available!")
return None
# adjust length
d = calc.vector_length(result)
if d > self.maxvel:
result = calc.adjust_length(result, self.maxvel)
elif 0 < d < self.minvel:
result = calc.adjust_length(result, self.minvel)
else:
result = calc.random_vector(grdnt.goal_radius + grdnt.diffusion)
return result
def move(self):
"""
:return: movement vector
"""
vector_repulsion = None
pose = self._buffer.get_own_pose()
# repulsive gradients
gradients_repulsive = self._buffer.repulsive_gradients(self.frames,
self.static,
self.moving)
# attractive gradient / set goal
vector_attraction = self.goal_gradient()
if pose:
if gradients_repulsive:
vector_repulsion = Vector3()
for grdnt in gradients_repulsive:
if self.goal:
grad = gradient.calc_repulsive_gradient_ge(grdnt,
self.goal,
pose)
else: # no attractive gradient, apply repulsion only
grad = gradient.calc_repulsive_gradient(grdnt, pose)
# robot position is within obstacle goal radius
# handle infinite repulsion
if abs(grad.x) == np.inf or abs(grad.y) == np.inf or \
abs(grad.z) == np.inf:
# create random vector with length (goal_radius +
# gradient.diffusion)
dv = calc.delta_vector(pose.p, grdnt.p)
if calc.vector_length(dv) == 0:
vector_repulsion = calc.add_vectors(
vector_repulsion, calc.random_vector(
grdnt.goal_radius + grdnt.diffusion))
else:
vector_repulsion = calc.add_vectors(
vector_repulsion, calc.adjust_length(dv,
grdnt.goal_radius + grdnt.diffusion))
else:
vector_repulsion = calc.add_vectors(vector_repulsion,
grad)
if vector_attraction and vector_repulsion:
result = calc.add_vectors(vector_attraction, vector_repulsion)
elif vector_repulsion:
result = vector_repulsion
elif vector_attraction:
result = vector_attraction
else:
return None
d = calc.vector_length(result)
if d > self.maxvel:
result = calc.adjust_length(result, self.maxvel)
elif 0 < d < self.minvel:
result = calc.adjust_length(result, self.minvel)
return result