class AccGen(Trajectory): done = False a_max = 0.6**2.0 / 0.8 t_f = 0 def __init__(self, trajectory_node, mid, start, velo, psi): Trajectory.__init__(self, trajectory_node) self.tg = TrajectoryGenerator() self.midpoint = mid self.start = start n = [ self.start[0] - self.midpoint[0], self.start[1] - self.midpoint[1], self.start[0] - self.midpoint[2] ] self.radius = self.tg.get_distance(self.start, self.midpoint) self.initial_velo = velo self.velo = self.tg.get_norm(self.initial_velo) #define new coordinates self.e_n = self.tg.get_direction(n) self.yp = self.tg.get_direction(self.initial_velo) self.zp = numpy.cross(self.e_n, self.yp) self.psi = psi #angle of rotation about initial e_n direction self.w = self.radius * self.velo self.theta_z = self.tg.get_projection([0, 0, 1], self.e_n) def begin(self): v_max = (self.radius * self.a_max)**(0.5) if self.velo > v_max: self.velo = v_max self.__set_done(False) def loop(self, start_time): self.__set_t_f( self.velo / math.sqrt(self.a_max**2.0 - self.velo**4.0 / self.radius**2.0)) time = start_time r = 10.0 rate = rospy.Rate(r) while not rospy.is_shutdown() and not is_done(): theta = self.velo * time**2.0 / (2 * self.radius * t_f) w = self.velo * time / (self.radius * t_f) alpha = self.velo / (self.radius * t_f) outpos = self.tg.get_circle_point(self.radius, theta) outpos = self.tg.offset(outpos, self.midpoint) outpos.append(self.tg.adjust_yaw([1, 0, 0])) outvelo = self.tg.get_circle_velocity(self.radius, theta, omega) outvelo.append(0) outacc = self.tg.get_circle_acc(self.radius, theta, omega, alpha) outacc.append(0) outmsg = self.tg.get_message(outpos, outvelo, outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1 / r if time >= t_f: self.__set_done(True) def is_done(self): return self.done def __set_done(self, boolean): self.done = boolean def __set_t_f(self, t): self.t_f = t def get_t_f(self): return self.t_f
class StraightLineGen(Trajectory): done = False a_max = 9.81/3. def __init__(self,trajectory_node,start,end): Trajectory.__init__(self,trajectory_node) self.start_point = start self.end_point = end self.tg = TrajectoryGenerator() self.dist = self.tg.get_distance(self.start_point, self.end_point) n = [0.,0.,0.] for i in range(0,3): n[i] = self.end_point[i] - self.start_point[i] self.e_t = self.tg.get_direction(n) self.t_f = math.sqrt(6*self.dist/0.9*self.a_max) self.constant = -2.0/self.t_f**3.0 * self.dist def begin(self): self.__set_done(False) def loop(self,start_time): r = 10.0 rate = rospy.Rate(10) time = start_time while not rospy.is_shutdown() and not self.is_done(): outpos = self.get_position(time) outpos.append(0) outvelo = self.get_velocity(time) outvelo.append(0) outacc = self.get_acceleration(time) outacc.append(0) outmsg = self.tg.get_message(outpos, outvelo, outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1/r if time >= self.t_f: self.__set_done(True) end = self.end_point end.append(0) outmsg = self.tg.get_message(self.end_point, [0.0,0.0,0.0,0.0], [0.0,0.0,0.0,0.0]) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) def __set_done(self,boolean): self.done = boolean def get_position(self,t): outpos = [0.0,0.0,0.0] s = self.get_s(t) for i in range(0,3): outpos[i] = self.start_point[i] + s * self.e_t[i] return outpos def get_s(self,t): return t**2.0 * self.constant * (t-1.5*self.t_f) def get_velocity(self,t): outvelo = [0.0,0.0,0.0] s_d = self.get_s_d(t) for i in range(0,3): outvelo[i] = s_d * self.e_t[i] return outvelo def get_s_d (self,t): return self.constant * (3 * t**2.0 - 3 * self.t_f * t) def get_acceleration(self,t): outacc = [0.0,0.0,0.0] s_dd = self.get_s_dd(t) for i in range(0,3): outacc[i] = s_dd * self.e_t[i] return outacc def get_s_dd (self,t): return self.constant * (6 * t - 3 * self.t_f ) def is_done(self): return self.done
class ArcGen(Trajectory): done = False a_max = 0.6**2.0/0.8 def __init__(self,trajectory_node,mid,start,velo,psi): Trajectory.__init__(self,trajectory_node) self.tg = TrajectoryGenerator() self.midpoint = mid self.start = start n = [self.start[0]-self.midpoint[0],self.start[1]-self.midpoint[1],self.start[0]-self.midpoint[2]] self.radius = self.tg.get_distance(self.start,self.midpoint) self.initial_velo = velo self.velo = self.tg.get_norm(self.initial_velo) #define new coordinates self.e_n = self.tg.get_direction(n) self.yp = self.tg.get_direction(self.initial_velo) self.zp = numpy.cross(self.e_n,self.yp) self.psi = psi #angle of rotation about initial e_n direction self.w = self.radius*self.velo self.theta_z = self.tg.get_projection([0,0,1],self.e_n) def begin(self): v_max = (self.radius*self.a_max)**(0.5) if self.velo > v_max: self.velo = v_max self.__set_done(False) def loop(self, start_time): time = start_time r = 10.0 rate = rospy.Rate(r) while not rospy.is_shutdown() and not self.is_done(): outpos = self.tg.get_circle_point(self.radius,self.w*time) #outpos = self.tg.rotate_vector(outpos,[0,0,self.theta_z]) #outpos = self.tg.vector_to_list(outpos) outpos = self.tg.offset(outpos,self.midpoint) outpos.append(self.tg.adjust_yaw([1,0,0])) outvelo = self.tg.get_circle_velocity(self.radius,self.w*time,self.w) #outvelo = self.tg.rotate_vector(outvelo,[0,0,self.theta_z]) #outvelo = self.tg.vector_to_list(outvelo) outvelo.append(0) outacc = self.tg.get_circle_acc(self.radius,self.w*time,self.w,0) #outacc = self.tg.rotate_vector(outacc,[0,0,self.theta_z]) #outacc = self.tg.vector_to_list(outacc) outacc.append(0) outmsg = self.tg.get_message(outpos,outvelo,outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1/r if self.w*time >= self.psi: outpos = self.tg.get_circle_point(self.radius,self.psi) #outpos = self.tg.rotate_vector(outpos,[0,0,self.theta_z]) #outpos = self.tg.vector_to_list(outpos) outpos = self.tg.offset(outpos,self.midpoint) outpos.append(self.tg.adjust_yaw([1,0,0])) outvelo = self.tg.get_circle_velocity(self.radius,self.psi,self.w) #outvelo = self.tg.rotate_vector(outvelo,[0,0,self.theta_z]) #outvelo = self.tg.vector_to_list(outvelo) outvelo.append(0) outacc = self.tg.get_circle_acc(self.radius,self.psi,self.w,0) #outacc = self.tg.rotate_vector(outacc,[0,0,self.theta_z]) #outacc = self.tg.vector_to_list(outacc) outacc.append(0) outmsg = self.tg.get_message(outpos,outvelo,outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1/r self.__set_done(True) def is_done(self): return self.done def __set_done(self,boolean): self.done = boolean
class AccGen(Trajectory): done = False a_max = 0.6**2.0/0.8 t_f = 0 def __init__(self,trajectory_node,mid,start,velo,psi): Trajectory.__init__(self,trajectory_node) self.tg = TrajectoryGenerator() self.midpoint = mid self.start = start n = [self.start[0]-self.midpoint[0],self.start[1]-self.midpoint[1],self.start[0]-self.midpoint[2]] self.radius = self.tg.get_distance(self.start,self.midpoint) self.initial_velo = velo self.velo = self.tg.get_norm(self.initial_velo) #define new coordinates self.e_n = self.tg.get_direction(n) self.yp = self.tg.get_direction(self.initial_velo) self.zp = numpy.cross(self.e_n,self.yp) self.psi = psi #angle of rotation about initial e_n direction self.w = self.radius*self.velo self.theta_z = self.tg.get_projection([0,0,1],self.e_n) def begin(self): v_max = (self.radius*self.a_max)**(0.5) if self.velo > v_max: self.velo = v_max self.__set_done(False) def loop(self, start_time): self.__set_t_f(self.velo/math.sqrt(self.a_max**2.0 - self.velo**4.0/self.radius**2.0)) time = start_time r = 10.0 rate = rospy.Rate(r) while not rospy.is_shutdown() and not is_done(): theta = self.velo*time**2.0/(2*self.radius*t_f) w = self.velo*time/(self.radius*t_f) alpha = self.velo/(self.radius*t_f) outpos = self.tg.get_circle_point(self.radius, theta) outpos = self.tg.offset(outpos,self.midpoint) outpos.append(self.tg.adjust_yaw([1,0,0])) outvelo = self.tg.get_circle_velocity(self.radius,theta,omega) outvelo.append(0) outacc = self.tg.get_circle_acc(self.radius,theta,omega,alpha) outacc.append(0) outmsg = self.tg.get_message(outpos,outvelo,outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1/r if time >= t_f: self.__set_done(True) def is_done(self): return self.done def __set_done(self,boolean): self.done = boolean def __set_t_f(self,t): self.t_f = t def get_t_f(self): return self.t_f
class StraightLineGen(Trajectory): done = False a_max = 9.81 / 3. def __init__(self, trajectory_node, start, end): Trajectory.__init__(self, trajectory_node) self.start_point = start self.end_point = end self.tg = TrajectoryGenerator() self.dist = self.tg.get_distance(self.start_point, self.end_point) n = [0., 0., 0.] for i in range(0, 3): n[i] = self.end_point[i] - self.start_point[i] self.e_t = self.tg.get_direction(n) self.t_f = math.sqrt(6 * self.dist / 0.9 * self.a_max) self.constant = -2.0 / self.t_f**3.0 * self.dist def begin(self): self.__set_done(False) def loop(self, start_time): r = 10.0 rate = rospy.Rate(10) time = start_time while not rospy.is_shutdown() and not self.is_done(): outpos = self.get_position(time) outpos.append(0) outvelo = self.get_velocity(time) outvelo.append(0) outacc = self.get_acceleration(time) outacc.append(0) outmsg = self.tg.get_message(outpos, outvelo, outacc) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) rate.sleep() time += 1 / r if time >= self.t_f: self.__set_done(True) end = self.end_point end.append(0) outmsg = self.tg.get_message(self.end_point, [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]) self.trajectory_node.send_msg(outmsg) self.trajectory_node.send_permission(False) def __set_done(self, boolean): self.done = boolean def get_position(self, t): outpos = [0.0, 0.0, 0.0] s = self.get_s(t) for i in range(0, 3): outpos[i] = self.start_point[i] + s * self.e_t[i] return outpos def get_s(self, t): return t**2.0 * self.constant * (t - 1.5 * self.t_f) def get_velocity(self, t): outvelo = [0.0, 0.0, 0.0] s_d = self.get_s_d(t) for i in range(0, 3): outvelo[i] = s_d * self.e_t[i] return outvelo def get_s_d(self, t): return self.constant * (3 * t**2.0 - 3 * self.t_f * t) def get_acceleration(self, t): outacc = [0.0, 0.0, 0.0] s_dd = self.get_s_dd(t) for i in range(0, 3): outacc[i] = s_dd * self.e_t[i] return outacc def get_s_dd(self, t): return self.constant * (6 * t - 3 * self.t_f) def is_done(self): return self.done
class AvoidanceController(): def __init__(self, my_id, bodies): self.gain = 2. #x,y direction self.gain_z = 0. #z direction self.tg = TrajectoryGenerator() self.my_id = my_id self.bodies = bodies self.states = [] for i in range(0, len(self.bodies)): self.states.append(QuadPositionDerived()) rospy.Subscriber("/body_data/id_" + str(self.bodies[i]), QuadPositionDerived, self.__set_states) def get_potential_output(self): distances = self.__get_distances() directions = self.__get_directions() u = [0., 0., 0.] for i in range(0, len(distances)): if (distances[i] < 0.1): for j in range(0, 2): u[j] += self.gain / 0.1 * directions[i][j] u[2] += self.gain_z / 0.1 * directions[i][2] else: for j in range(0, 2): u[j] += self.gain / distances[i] * directions[i][j] u[2] += self.gain_z / distances[i] * directions[i][2] return u def get_blending_constant(self): alpha_max = 0.8 # <= 1. alpha_min = 0. # >=0. r_min = 1.2 r_max = 2. k = (alpha_min - alpha_max) / (r_max - r_min) m = alpha_max - k * r_min distances = self.__get_distances() if self.gain == 0: return 0 elif min(distances) <= r_min: return alpha_max elif min(distances) > r_max: return alpha_min else: return (k * min(distances) + m) def __set_states(self, data): topic_name = data._connection_header["topic"] for i in range(0, len(self.bodies)): name = "/body_data/id_" + str(self.bodies[i]) if topic_name == name: self.states[i] = data def __get_my_state(self): for i in range(0, len(self.bodies)): if self.bodies[i] == self.my_id: return self.states[i] def __get_distances(self): distances = [] for i in range(0, len(self.states)): if self.bodies[i] != self.my_id: distance = self.tg.get_distance( self.__get_pos_from_state(self.__get_my_state()), self.__get_pos_from_state(self.states[i])) distances.append(distance) return distances def __get_directions(self): directions = [] for i in range(0, len(self.states)): if self.bodies[i] != self.my_id: direction = self.tg.get_direction2( self.__get_pos_from_state(self.__get_my_state()), self.__get_pos_from_state(self.states[i])) directions.append(direction) return directions def __get_pos_from_state(self, state): return [state.x, state.y, state.z]