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 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 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):
"""
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 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):
"""
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 flocking_vector(neighbors, agent, epsilon, a, b, repulsion_radius,
view_distance, h):
"""
calculate overall flocking vector
:param neighbors: neighbor data
:param agent: agent data
:param epsilon: fixed parameter (0,1) of sigma_norm
:param a: parameter action function
:param b: parameter action function
:param repulsion_radius: scale (desired distance between agents)
:param view_distance: interaction range of robot
:param h: parameter (0,1) of bump_function
:return: vector of steering force
"""
grad = gradient_based(neighbors, agent, epsilon, a, b, repulsion_radius,
view_distance, h)
vel = velocity_consensus(neighbors, agent, epsilon, view_distance, h)
return calc.add_vectors(grad, vel)
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 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
