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 strongest_gradient(self,
frameids=None,
static=True,
moving=True,
time=None):
"""
return gradient with strongest potential on robot
:param frameids: frames to consider
:param static: bool, consider static gradients
:param moving: bool, consider moving gradients
:param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
:return: soMessage gradient
"""
tmp_grad = None
if self.get_last_position() is None:
return tmp_grad
# all gradients within view distance
gradients = self.gradients(frameids, static, moving, time=time)
tmp_att = -1
if gradients:
for grad in gradients:
if grad.attraction == 1:
g = gradient.calc_attractive_gradient(
grad, self.get_last_position())
else:
g = gradient.calc_repulsive_gradient(
grad, self.get_last_position())
if abs(g.x) == np.inf or abs(g.y) == np.inf or \
abs(g.z) == np.inf:
att = np.inf
else:
att = calc.vector_length(g)
# attractive potential is defined inverse to repulsive
# potential --> adjust attractive value for comparison
if grad.attraction == 1:
att = 1 - att
if att > tmp_att:
tmp_grad = grad
tmp_att = att
return tmp_grad
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 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 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