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]
