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