def generate_position_trajectory(self, X_, odomm):
msg = velocities_from_navigation_function()
msg.header.stamp = odomm.header.stamp #rospy.Time.now()
position = self.scale_factor * X_
msg.ddes.x = 1 #direction_vector[0]
msg.ddes.y = 0 #direction_vector[1]
msg.ddes.z = 0 #direction_vector[2]
msg.pdes.x = self.initial_position[0]
msg.pdes.y = self.initial_position[1]
if self.initial_position[2] < 3:
msg.pdes.z = 3 #initial_position[3]
else:
msg.pdes.z = self.initial_position[2]
msg.vdes.x = 0
msg.vdes.y = 0
msg.vdes.z = 0
msg.ades.x = 0
msg.ades.y = 0
msg.ades.z = 0
msg.controller = 0 # 1: velocity controller, 0: postion controller
#f0 = open('desired_trajectory.txt', 'a')
#f0.write("%s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" % (msg.pdes.x, msg.pdes.y, msg.pdes.z, msg.vdes.x, msg.vdes.y, msg.vdes.z, 0.0, msg.ades.x, msg.ades.y, msg.ades.z, msg.jdes.x, msg.jdes.y, msg.jdes.z, msg.sdes.x, msg.sdes.y, msg.sdes.z))
self.pub.publish(msg)
self.position = np.array([position[0], position[1], position[2]])
self.time = time.time()
def generate_velocity_trajectory(self, phi_, X_, odom):
self.counter = self.counter + 1
current_time = time.time()
t = current_time - self.time
self.time_points.append(t)
#delt = 0.01#(max(self.time_points)-min(self.time_points))/self.counter
Vtarget = np.array([0, 0, 0]) # velocity of the target in m/sec
dist_to_goal = nrm(X_ - self.goal)
msg = velocities_from_navigation_function()
msg.header.stamp = odom.header.stamp #rospy.Time.now()
self.heading_direction = np.array([1, 0, 0])
msg.ddes.x = self.heading_direction[0]
msg.ddes.y = self.heading_direction[1]
msg.ddes.z = self.heading_direction[2]
cx = (self.n + 1) / 2
cy = (3 * self.n + 1) / 2
cz = (5 * self.n + 1) / 2
time_factor = 1 / (1 + 2 * np.exp(-2 * (t - 1)))
distance_to_goal_factor = scipy.special.erf(
6.0 * (dist_to_goal - self.proximity))
#d1 = np.linalg.norm(self.x1-X_); d2 = np.linalg.norm(self.x2-X_)
#rospy.loginfo('d1 and d2 are:%f, %f',d1, d2)
#collision_factor1 = 1.0/(1.0+np.exp(-100*(d1-self.d0))); collision_factor2 = 1.0/(1.0+np.exp(-100*(d2-self.d0)))
smoothing_factor = time_factor * distance_to_goal_factor #* collision_factor1 * collision_factor2
if nrm(
X_ - self.goal
) > self.proximity and self.phase_one == False: #0#direction_vector[2]#1#direction_vector[0
dNdX = np.gradient(phi_)
dNdX = [-i for i in dNdX]
v1 = np.array([dNdX[cx], dNdX[cy], dNdX[cz]])
if nrm(v1) == 0:
v = 0.8 * self.v_previous
rospy.loginfo("the invalid goal for %s is: %f, %f, %f",
self.namespace, self.goal[0], self.goal[1],
self.goal[2])
else:
v = smoothing_factor * (self.VelocityMagnitude * v1 / nrm(v1))
self.smooth_vx.append(v[0])
self.smooth_vy.append(v[1])
self.smooth_vz.append(v[2])
self.position = self.position + v * self.dt
if self.counter == 1:
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]
msg.vdes.y = v[1]
msg.vdes.z = v[2]
msg.ades.x = 0
msg.ades.y = 0
msg.ades.z = 0
msg.controller = 0 # 1: velocity controller, 0: position controller
self.v_previous = np.array([v[0], v[1], v[2]])
elif self.counter < 10:
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]
msg.vdes.y = v[1]
msg.vdes.z = v[2]
msg.ades.x = (v[0] - self.v_previous[0]) / self.dt
msg.ades.y = (v[1] - self.v_previous[1]) / self.dt
msg.ades.z = (v[2] - self.v_previous[2]) / self.dt
#msg.jdes.x = (msg.ades.x-self.a_previous[0])/self.dt
#msg.jdes.y = (msg.ades.y-self.a_previous[1])/self.dt
#msg.jdes.z = (msg.ades.z-self.a_previous[2])/self.dt
msg.controller = 0 # 1: velocity controller, 0: postion controller
#self.a = self.time_points[-1]
self.v_previous = np.array([v[0], v[1], v[2]])
#self.a_previous = np.array([msg.ades.x, msg.ades.y, msg.ades.z])
else:
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]
msg.vdes.y = v[1]
msg.vdes.z = v[2]
msg.ades.x = (v[0] - self.v_previous[0]) / self.dt
msg.ades.y = (v[1] - self.v_previous[1]) / self.dt
msg.ades.z = (v[2] - self.v_previous[2]) / self.dt
#msg.jdes.x = (msg.ades.x-self.a_previous[0])/self.dt
#msg.jdes.y = (msg.ades.y-self.a_previous[1])/self.dt
#msg.jdes.z = (msg.ades.z-self.a_previous[2])/self.dt
msg.controller = 0 # 1: velocity controller, 0: postion controller
#self.a = self.time_points[-1]
self.v_previous = np.array([v[0], v[1], v[2]])
#self.a_previous = np.array([msg.ades.x, msg.ades.y, msg.ades.z])
"""
t = list(np.linspace(min(self.time_points), max(self.time_points), self.counter))
if self.counter > 20: # number of points used in making spline
t.pop(0); self.smooth_vx.pop(0); self.smooth_vy.pop(0); self.smooth_vz.pop(0); self.time_points.pop(0)
t = list(np.linspace(min(self.time_points), max(self.time_points), 20))
a = splrep(t,self.smooth_vx,k=5,s=4); b = splrep(t,self.smooth_vy,k=5,s=4); c = splrep(t,self.smooth_vz,k=5,s=4)
a2 = splev(t,a,der=1); b2 = splev(t,b,der=1); c2 = splev(t,c,der=1)
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]; msg.vdes.y = v[1]; msg.vdes.z = v[2]
msg.ades.x = a2[-1]; msg.ades.y = b2[-1]; msg.ades.z = c2[-1]
msg.controller = 0 # 1: velocity controller, 0: postion controller
self.switching_position = X_*self.scale_factor
self.v_previous =np.array([v[0], v[1], v[2]])
"""
else:
#self.phase_one = True
position = self.switching_position
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = Vtarget[0]
msg.vdes.y = Vtarget[1]
msg.vdes.z = Vtarget[2]
msg.ades.x = 0
msg.ades.y = 0
msg.ades.z = 0
msg.controller = 0 # 1: velocity controller, 0: postion controller
print 'the quad has switched'
self.pub.publish(msg)
def generate_velocity_trajectory(self, phi_, X_, odom):
self.counter = self.counter + 1
#current_time = time.time()
t = time.time() - self.time
self.time_points.append(t)
#delt = 0.01#(max(self.time_points)-min(self.time_points))/self.counter
#Vtarget = np.array([0, 0, 0]) # velocity of the target in m/sec
dist_to_goal = nrm(X_ - self.goal)
msg = velocities_from_navigation_function()
msg1 = Point()
msg.header.stamp = odom.header.stamp #rospy.Time.now()
self.heading_direction = np.array([1, 0, 0])
msg.ddes.x = self.heading_direction[0]
msg.ddes.y = self.heading_direction[1]
msg.ddes.z = self.heading_direction[2]
cx = (self.n + 1) / 2
cy = (3 * self.n + 1) / 2
cz = (5 * self.n + 1) / 2
timefactor = 1 / (1 + np.exp(-10 * (t)))
distancefactor = scipy.special.erf(8.0 *
(dist_to_goal - self.proximity))
smoothing_factor = timefactor * distancefactor
f2 = open('smoothing_factor_for_firefly{}.txt'.format(self.no), 'a')
f2.write("%s, %s, %s, %s\n" %
(time.time(), timefactor, distancefactor, smoothing_factor))
#if nrm(X_-self.goal) > self.proximity and self.phase_one == False:#0#direction_vector[2]#1#direction_vector[0
dNdX = np.gradient(phi_)
dNdX = [-i for i in dNdX]
v1 = np.array([dNdX[cx], dNdX[cy], dNdX[cz]])
if nrm(v1) == 0:
v = 0.8 * self.v_previous
#rospy.loginfo("the invalid goal for %s is: %f, %f, %f", self.namespace, self.goal[0], self.goal[1], self.goal[2])
else:
v = smoothing_factor * (self.VelocityMagnitude * v1 / nrm(v1))
self.smooth_vx.append(v[0])
self.smooth_vy.append(v[1])
self.smooth_vz.append(v[2])
self.position = self.position + v * self.dt
if self.counter == 1:
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]
msg.vdes.y = v[1]
msg.vdes.z = v[2]
msg.ades.x = 0
msg.ades.y = 0
msg.ades.z = 0
msg.controller = 0 # 1: velocity controller, 0: position controller
self.v_previous = np.array([v[0], v[1], v[2]])
msg1.x = 0
msg1.y = 0
msg1.z = 0
elif self.counter < 10:
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]
msg.vdes.y = v[1]
msg.vdes.z = v[2]
msg.ades.x = (v[0] - self.v_previous[0]) / self.dt
msg.ades.y = (v[1] - self.v_previous[1]) / self.dt
msg.ades.z = (v[2] - self.v_previous[2]) / self.dt
#msg.jdes.x = (msg.ades.x-self.a_previous[0])/self.dt
#msg.jdes.y = (msg.ades.y-self.a_previous[1])/self.dt
#msg.jdes.z = (msg.ades.z-self.a_previous[2])/self.dt
msg.controller = 0 # 1: velocity controller, 0: postion controller
#self.a = self.time_points[-1]
self.v_previous = np.array([v[0], v[1], v[2]])
#self.a_previous = np.array([msg.ades.x, msg.ades.y, msg.ades.z])
else:
"""
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]; msg.vdes.y = v[1]; msg.vdes.z = v[2]
msg.ades.x = (v[0]-self.v_previous[0])/self.dt
msg.ades.y = (v[1]-self.v_previous[1])/self.dt
msg.ades.z = (v[2]-self.v_previous[2])/self.dt
#msg.jdes.x = (msg.ades.x-self.a_previous[0])/self.dt
#msg.jdes.y = (msg.ades.y-self.a_previous[1])/self.dt
#msg.jdes.z = (msg.ades.z-self.a_previous[2])/self.dt
msg.controller = 0 # 1: velocity controller, 0: postion controller
#self.a = self.time_points[-1]
self.v_previous = np.array([v[0], v[1], v[2]])
msg1.x = 0; msg1.y = 0; msg1.z = 0
if nrm(self.position-self.goal*self.scale_factor) < 0.5: # normal scale so when the goal point is 10cm away from the mav
msg1.x = 1 if self.no == '1' else 0
msg1.y = 1 if self.no == '2' else 0
msg1.y = 1 if self.no == '3' else 0
"""
#self.a_previous = np.array([msg.ades.x, msg.ades.y, msg.ades.z])
t = list(
np.linspace(min(self.time_points), max(self.time_points),
self.counter))
if self.counter > 100: # number of points used in making spline
t.pop(0)
self.smooth_vx.pop(0)
self.smooth_vy.pop(0)
self.smooth_vz.pop(0)
self.time_points.pop(0)
t = list(
np.linspace(min(self.time_points), max(self.time_points),
100))
a = splrep(t, self.smooth_vx, k=5, s=4)
b = splrep(t, self.smooth_vy, k=5, s=4)
c = splrep(t, self.smooth_vz, k=5, s=4)
a2 = splev(t, a, der=1)
b2 = splev(t, b, der=1)
c2 = splev(t, c, der=1)
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]
msg.vdes.y = v[1]
msg.vdes.z = v[2]
msg.ades.x = a2[-1]
msg.ades.y = b2[-1]
msg.ades.z = c2[-1]
msg.controller = 0 # 1: velocity controller, 0: postion controller
self.switching_position = X_ * self.scale_factor
self.v_previous = np.array([v[0], v[1], v[2]])
"""
else:
#self.phase_one = True
position = self.switching_position
msg.pdes.x = self.position[0]; msg.pdes.y = self.position[1]; msg.pdes.z = self.position[2]
msg.vdes.x = Vtarget[0]; msg.vdes.y = Vtarget[1]; msg.vdes.z = Vtarget[2]
msg.ades.x = 0; msg.ades.y = 0; msg.ades.z = 0
msg.controller = 0 # 1: velocity controller, 0: postion controller
print 'the quad has switched'
"""
f3 = open('desired_traj{}.txt'.format(self.no), 'a')
f3.write("%s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" %
(time.time(), msg.pdes.x, msg.pdes.y, msg.pdes.z, msg.vdes.x,
msg.vdes.y, msg.vdes.z, msg.ades.x, msg.ades.y, msg.ades.z))
self.pub1.publish(msg1)
self.pub.publish(msg)
def generate_velocity_trajectory(self, phi_, X_, odom):
self.counter = self.counter + 1
current_time = time.time()
t = current_time - self.time
self.time_points.append(t)
#delt = 0.01#(max(self.time_points)-min(self.time_points))/self.counter
Vtarget = np.array([0, 0, 0]) # velocity of the target in m/sec
dist_to_goal = nrm(X_ - self.goal)
msg = velocities_from_navigation_function()
msg.header.stamp = odom.header.stamp #rospy.Time.now()
#rx = 0.8; ry = 0.65; secs = 6
#x = rx*sin((t/secs/200)*180/np.pi); y = 1.0*ry*cos((t/secs/200)*180/np.pi)
#self.goal = np.array([x, y, 0.1])
#self.goal = np.array([0, 0, 0.1])
#rospy.loginfo("goal is %f, %f, %f", self.goal[0], self.goal[1], self.goal[2])
#self.heading_direction = (self.goal-X_)/nrm(self.goal-X_)
self.heading_direction = np.array([1, 0, 0])
msg.ddes.x = self.heading_direction[0]
msg.ddes.y = self.heading_direction[1]
msg.ddes.z = self.heading_direction[2]
cx = (self.n + 1) / 2
cy = (3 * self.n + 1) / 2
cz = (5 * self.n + 1) / 2
#smoothing_factor = scipy.special.erf(0.5*t)*scipy.special.erf(20*(dist_to_goal-self.proximity))
smoothing_factor = 1 / (1 +
15 * np.exp(-2 * (t - 1))) * scipy.special.erf(
1.5 * (dist_to_goal - self.proximity))
if nrm(
X_ - self.goal
) > self.proximity and self.phase_one == False: #0#direction_vector[2]#1#direction_vector[0
dNdX = np.gradient(phi_)
dNdX = [-i for i in dNdX]
v1 = np.array([dNdX[cx], dNdX[cy], dNdX[cz]])
if nrm(v1) == 0:
v = self.previous_velocity
rospy.loginfo(
"the target that gave invalid velocity is: %f, %f, %f",
self.goal[0], self.goal[1], self.goal[2])
else:
v = smoothing_factor * (self.VelocityMagnitude * v1 / nrm(v1))
self.smooth_vx.append(v[0])
self.smooth_vy.append(v[1])
self.smooth_vz.append(v[2])
self.position = self.position + v * self.dt
if self.counter == 1:
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]
msg.vdes.y = v[1]
msg.vdes.z = v[2]
msg.ades.x = 0
msg.ades.y = 0
msg.ades.z = 0
msg.controller = 0 # 1: velocity controller, 0: position controller
self.vx_previous = v[0]
self.vy_previous = v[1]
self.vz_previous = v[2]
elif self.counter < 10:
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]
msg.vdes.y = v[1]
msg.vdes.z = v[2]
msg.ades.x = (v[0] - self.vx_previous) / self.dt
msg.ades.y = (v[1] - self.vy_previous) / self.dt
msg.ades.z = (v[2] - self.vz_previous) / self.dt
msg.controller = 0 # 1: velocity controller, 0: postion controller
#self.a = self.time_points[-1]
self.vx_previous = v[0]
self.vy_previous = v[1]
self.vz_previous = v[2]
else:
t = list(
np.linspace(min(self.time_points), max(self.time_points),
self.counter))
if self.counter > 200: # number of points used in making spline
t.pop(0)
self.smooth_vx.pop(0)
self.smooth_vy.pop(0)
self.smooth_vz.pop(0)
self.time_points.pop(0)
t = list(
np.linspace(min(self.time_points),
max(self.time_points), 200))
a = splrep(t, self.smooth_vx, k=5, s=4)
b = splrep(t, self.smooth_vy, k=5, s=4)
c = splrep(t, self.smooth_vz, k=5, s=4)
a2 = splev(t, a, der=1)
b2 = splev(t, b, der=1)
c2 = splev(t, c, der=1)
msg.pdes.x = self.position[0]
msg.pdes.y = self.position[1]
msg.pdes.z = self.position[2]
msg.vdes.x = v[0]
msg.vdes.y = v[1]
msg.vdes.z = v[2]
msg.ades.x = a2[-1]
msg.ades.y = b2[-1]
msg.ades.z = c2[-1]
msg.controller = 0 # 1: velocity controller, 0: postion controller
self.switching_position = X_ * self.scale_factor
self.vx_previous = v[0]
self.vy_previous = v[1]
self.vz_previous = v[2]
#self.goal = self.goal+np.array([0,-0.002,0])
#if self.goal[1]<=-0.4:
# self.goal = np.array([self.goal[0],-0.4,self.goal[2]])
self.previous_velocity = v
else:
#self.phase_one = True
position = self.switching_position
msg.pdes.x = position[0]
msg.pdes.y = position[1]
msg.pdes.z = position[2]
msg.vdes.x = Vtarget[0]
msg.vdes.y = Vtarget[1]
msg.vdes.z = Vtarget[2]
msg.ades.x = 0
msg.ades.y = 0
msg.ades.z = 0
msg.controller = 0 # 1: velocity controller, 0: postion controller
print 'the quad has switched'
#f0 = open('desired_trajectory.txt', 'a')
#f0.write("%s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" % (msg.pdes.x, msg.pdes.y, msg.pdes.z, msg.vdes.x, msg.vdes.y, msg.vdes.z, smoothing_factor, msg.ades.x, msg.ades.y, msg.ades.z, msg.jdes.x, msg.jdes.y, msg.jdes.z, msg.sdes.x, msg.sdes.y, msg.sdes.z))
self.pub.publish(msg)