def move(self):
"""
calculates movement vector
:return: movement vector
"""
pose = self._buffer.get_own_pose()
g = None
# strongest gradient
grad = self._buffer.max_reach_attractive_gradient(self.frames,
self.static,
self.moving)
if pose and grad:
g = gradient.calc_attractive_gradient(grad, pose)
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 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 move(self):
"""
:return: movement vector following trail
"""
pose = self._buffer.get_own_pose()
# get all gradients within view distance
view = self._buffer.static_list_angle(self.frames, self.angle_xy,
self.angle_yz)
result = None
if pose:
if view:
result = Vector3()
for grdnt in view:
grad = gradient.calc_attractive_gradient_ge(grdnt, pose)
result = calc.add_vectors(result, grad)
if not result:
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
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 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 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):
"""
flocking calculations based on Olfati-Saber
:return: movement vector
"""
own_poses = self._buffer.own_pos
# own position - we need minimum two values to calculate velocities
if len(own_poses) >= 2:
pose = own_poses[-1].p
else:
return
# neighbors of agent
view = self._buffer.agent_set(self.frame)
# create array of tuples with neighbor position - neighbor velocity &
# own pos & velocity (p, v)
Boid = collections.namedtuple('Boid', ['p', 'v'])
agent = Boid(pose, agent_velocity(own_poses[-1], own_poses[-2]))
neighbors = []
for neighbor in view:
if len(neighbor) >= 2: # at least 2 datapoints are available
neighbors.append(
Boid(neighbor[-1].p,
agent_velocity(neighbor[-1], neighbor[0])))
repulsion_radius = own_poses[-1].diffusion + own_poses[-1].goal_radius
# calculate new velocity based on steering force
acceleration = flocking_vector(neighbors, agent, self.epsilon, self.a,
self.b, repulsion_radius,
self._buffer.view_distance, self.h)
if calc.vector_length(acceleration) > self.max_acceleration:
acceleration = calc.adjust_length(acceleration,
self.max_acceleration)
velocity = calc.add_vectors(agent.v, acceleration)
if calc.vector_length(velocity) > self.maxvel:
velocity = calc.adjust_length(velocity, self.maxvel)
return velocity
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 random movement vector
:return: movement vector
"""
tmp = np.random.rand(1, 3)
g = Vector3()
g.x = 2 * tmp[0][0] - 1
g.y = 2 * tmp[0][1] - 1
g.z = 2 * tmp[0][2] - 1
# 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 flocking vector based on Reynolds
:return: movement Vector (Vector3)
"""
pose = self._buffer.get_own_pose()
view = self._buffer.agent_list([self.frame])
mov = None
if pose:
mov = separation(pose, view)
mov = calc.add_vectors(mov, cohesion(pose, view))
mov = calc.add_vectors(mov, alignment(pose, view))
if mov and calc.vector_length(mov) > self.max_velocity:
mov = calc.adjust_length(mov, self.max_velocity)
return mov
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 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