def calculate_position_trajectory(self, X_):
proximity = 0.01 # how close you want the uav to fly to the target
msg = velocities_from_navigation_function()
msg.header.stamp = rospy.Time.now()
direction_vector = (self.goal-X_)/nrm(self.goal-X_)
position = self.scale_factor*(self.goal - proximity*direction_vector)
msg.desired_position.x = position[0]
msg.desired_position.y = position[1]
msg.desired_position.z = position[2]
msg.desired_velocity.x = 0
msg.desired_velocity.y = 0
msg.desired_velocity.z = 0
msg.desired_acceleration.x = 0
msg.desired_acceleration.y = 0
msg.desired_acceleration.z = 0
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.desired_direction.x = direction_vector[0]
msg.desired_direction.y = direction_vector[1]
msg.desired_direction.z = direction_vector[2]
msg.controller = 0 # 1: velocity controller, 0: postion controller
self.pub.publish(msg)
def calculate_position_trajectory(self, X_, odomm):
msg = velocities_from_navigation_function()
msg.header.stamp = odomm.header.stamp #rospy.Time.now()
#direction_vector = (self.goal-X_)/nrm(self.goal-X_)
#direction_vector = (self.goal-X_)/nrm(self.goal-X_)
position = self.scale_factor * X_
#self.goal = self.goal*self.scale_factor
#direction_vector = (self.goal-X_)/nrm(self.goal-X_)
# no matter what desired direction wont change
msg.desired_direction.x = 1 #direction_vector[0]
msg.desired_direction.y = 0 #direction_vector[1]
msg.desired_direction.z = 0 #direction_vector[2]
msg.desired_position.x = self.position[0] = -5 #position[0]
msg.desired_position.y = self.position[1] = 0 #position[1]
msg.desired_position.z = self.position[2] = 0.1
print self.position
msg.desired_velocity.x = 0
msg.desired_velocity.y = 0
msg.desired_velocity.z = 0
msg.desired_acceleration.x = 0
msg.desired_acceleration.y = 0
msg.desired_acceleration.z = 0
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.controller = 0 # 1: velocity controller, 0: postion controller
self.pub.publish(msg)
self.time = time.time()
def calculate_position_trajectory(self, X_):
msg = velocities_from_navigation_function()
msg.header.stamp = rospy.Time.now()
#direction_vector = (self.goal-X_)/nrm(self.goal-X_)
position = self.scale_factor*X_
direction_vector = (self.goal-X_)/nrm(self.goal-X_)
# no matter what desired direction wont change
msg.desired_direction.x = direction_vector[0]
msg.desired_direction.y = direction_vector[1]
msg.desired_direction.z = direction_vector[2]
msg.desired_position.x = position[0]
msg.desired_position.y = position[1]
msg.desired_position.z = 2
msg.desired_velocity.x = 0
msg.desired_velocity.y = 0
msg.desired_velocity.z = 0
msg.desired_acceleration.x = 0
msg.desired_acceleration.y = 0
msg.desired_acceleration.z = 0
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
#msg.desired_direction.x = direction_vector[0]
#msg.desired_direction.y = direction_vector[1]
#msg.desired_direction.z = direction_vector[2]
msg.controller = 0 # 1: velocity controller, 0: postion controller
self.pub.publish(msg)
self.time = time.time()
def calculate_position_trajectory(self, X_, odomm):
msg = velocities_from_navigation_function()
msg.header.stamp = odomm.header.stamp#rospy.Time.now()
#direction_vector = (self.goal-X_)/nrm(self.goal-X_)
#direction_vector = (self.goal-X_)/nrm(self.goal-X_)
position = self.scale_factor*X_
#self.goal = self.goal*self.scale_factor
#direction_vector = (self.goal-X_)/nrm(self.goal-X_)
# no matter what desired direction wont change
msg.ddes.x = 1#direction_vector[0]
msg.ddes.y = 0#direction_vector[1]
msg.ddes.z = 0#direction_vector[2]
#msg.ddes = np.array([1,0,0]
msg.pdes.x = self.position[0] = -0.75#position[0]
msg.pdes.y = self.position[1] = 0.0#position[1]
msg.pdes.z = self.position[2] = 0.75
#msg.pdes = [-0.75,0.0,0.75]
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.jdes.x = 0
msg.jdes.y = 0
msg.jdes.z = 0
msg.sdes.x = 0
msg.sdes.y = 0
msg.sdes.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.time = time.time()
def calculate_velocity_trajectory(self, phi_, X_, odomm):
"""
This function calculates the velocities for the quadrotor by numerically differentiating
the navigation function in the vicinity of the current position.
The vicinity is given by variable self.dX and the number of discretization points by self.n
in the callback function.
For example (all this is in callback), 11 points are taking within 0.1m of the current x, y
and z position and a grid is made at which value of navigation function is calculated.
This function then takes those values as a list and calculate all required derivative
using numpy's gradient function. Subsequently it publishes these values at the center point of the grid.
"""
#print 'hiiii', self.goal, X_
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
#self.goal = self.goal - Vtarget*delt#np.array([self.goal[0], self.goal[1]-Vtarget*delt, self.goal[2]])
#print self.goal
#X_ = X_*self.scale_factor; goal = self.goal*self.scale_factor
dist_to_goal = nrm(X_ - self.goal)
msg = velocities_from_navigation_function()
msg.header.stamp = odomm.header.stamp #rospy.Time.now()
direction_vector = (self.goal - X_) / nrm(self.goal - X_)
# no matter what desired direction wont change
msg.desired_direction.x = direction_vector[0]
msg.desired_direction.y = direction_vector[1]
msg.desired_direction.z = direction_vector[2]
#t = time.time()-self.time
#print 'its here', nrm(X_-self.goal), self.proximity
if nrm(X_ - self.goal) > self.proximity and self.phase_one == False:
#print 'hi'
cx = (self.n + 1) / 2
cy = (3 * self.n + 1) / 2
cz = (5 * self.n + 1) / 2
dNdX = np.gradient(phi_)
dNdX = [-i for i in dNdX]
v1 = np.array([dNdX[cx], dNdX[cy], dNdX[cz]])
#print 'v1', v1
#smoothing_factor = scipy.special.erf(0.5*t)*scipy.special.erf(20*(dist_to_goal-self.proximity))
smoothing_factor = 1 #1/(1+3*np.exp(-2*(t-1)))*scipy.special.erf(20*(dist_to_goal-self.proximity))
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])
f0 = open('desired_velocity.txt', 'a')
f0.write("%s, %s, %s, %s\n" % (v[0], v[1], v[2], smoothing_factor))
if self.counter == 1:
msg.desired_velocity.x = v[0]
msg.desired_velocity.y = v[1]
msg.desired_velocity.z = v[2]
msg.desired_acceleration.x = 0
msg.desired_acceleration.y = 0
msg.desired_acceleration.z = 0
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.controller = 1 # 1: velocity controller, 0: position controller
#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.vx_previous = v[0]
self.vy_previous = v[1]
self.vz_previous = v[2]
elif self.counter < 5:
#delt = (max(self.time_points)-min(self.time_points))/self.counter
msg.desired_velocity.x = v[0]
msg.desired_velocity.y = v[1]
msg.desired_velocity.z = v[2]
msg.desired_acceleration.x = 0.1 * (v[0] -
self.vx_previous) / delt
msg.desired_acceleration.y = 0.1 * (v[1] -
self.vy_previous) / delt
msg.desired_acceleration.z = 0.1 * (v[2] -
self.vz_previous) / delt
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.controller = 1 # 1: velocity controller, 0: postion controller
self.a = self.time_points[-1]
#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]])
else:
t = list(
np.linspace(min(self.time_points), max(self.time_points),
self.counter))
#delt = (max(self.time_points)-min(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)
t = list(
np.linspace(min(self.time_points),
max(self.time_points), 100))
a = splrep(t, self.smooth_vx, k=4, s=3)
b = splrep(t, self.smooth_vy, k=4, s=3)
c = splrep(t, self.smooth_vz, k=4, s=3)
aa = splev(t, a, der=1)
bb = splev(t, b, der=1)
cc = splev(t, c, der=1)
aaa = splev(t, a, der=2)
bbb = splev(t, b, der=2)
ccc = splev(t, c, der=2)
msg.desired_velocity.x = v[0]
msg.desired_velocity.y = v[1]
msg.desired_velocity.z = v[2]
msg.desired_acceleration.x = aa[-1]
msg.desired_acceleration.y = bb[-1]
msg.desired_acceleration.z = cc[-1]
msg.desired_jerk.x = aaa[-1]
msg.desired_jerk.y = bbb[-1]
msg.desired_jerk.z = ccc[-1]
msg.controller = 1 # 1: velocity controller, 0: postion controller
self.switching_position = X_ * self.scale_factor * 0.3048
#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]])
else:
self.phase_one = True
msg.desired_velocity.x = Vtarget[0]
msg.desired_velocity.y = Vtarget[1]
msg.desired_velocity.z = Vtarget[2]
position = self.switching_position
#position = self.scale_factor*(self.goal)# - 0.025*direction_vector)
msg.desired_position.x = position[0]
msg.desired_position.y = position[1]
msg.desired_position.z = position[2]
msg.desired_acceleration.x = 0
msg.desired_acceleration.y = 0
msg.desired_acceleration.z = 0
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.controller = 0 # 1: velocity controller, 0: postion controller
print 'it switched'
self.pub.publish(msg)
def calculate_velocity_trajectory(self, phi_, X_):
"""
This function calculates the velocities for the quadrotor by numerically differentiating
the navigation function in the vicinity of the current position.
The vicinity is given by variable self.dX and the number of discretization points by self.n
in the callback function.
For example (all this is in callback), 11 points are taking within 0.1m of the current x, y
and z position and a grid is made at which value of navigation function is calculated.
This function then takes those values as a list and calculate all required derivative
using numpy's gradient function. Subsequently it publishes these values at the center point of the grid.
"""
dist_to_goal = nrm(X_-self.goal)
self.counter = self.counter+1
msg = velocities_from_navigation_function()
msg.header.stamp = rospy.Time.now()
direction_vector = (self.goal-X_)/nrm(self.goal-X_)
# no matter what desired direction wont change
msg.desired_direction.x = direction_vector[0]
msg.desired_direction.y = direction_vector[1]
msg.desired_direction.z = direction_vector[2]
#t = time.time()-self.time
if nrm(X_-self.goal) > self.proximity and self.phase_one == False:
cx = (self.n+1)/2
cy = (3*self.n+1)/2
cz = (5*self.n+1)/2
dNdX = np.gradient(phi_)
dNdX = [-i for i in dNdX]
v1 = np.array([dNdX[cx], dNdX[cy], dNdX[cz]])
current_time = time.time()
t = current_time - self.time
self.time_points.append(t)
smoothing_factor = scipy.special.erf(0.5*t)*scipy.special.erf(50*(dist_to_goal-self.proximity))
v = smoothing_factor*self.VelocityMagnitude*v1/nrm(v1)
#f0 = open('smoothing_factor.txt', 'a')
#f0.write("%s, %s, %s, %s\n" % (self.goal, X_, dist_to_goal, smoothing_factor))
self.smooth_vx.append(v[0]); self.smooth_vy.append(v[1]); self.smooth_vz.append(v[2])
if self.counter == 1:
#self.position = self.scale_factor* np.array([X_[0], X_[1], X_[2]])
#msg.desired_position.x = self.position[0]
#msg.desired_position.y = self.position[1]
#msg.desired_position.z = self.position[2]
msg.desired_velocity.x = 0#v[0]
msg.desired_velocity.y = 0#v[1]
msg.desired_velocity.z = 0#v[2]
msg.desired_acceleration.x = 0
msg.desired_acceleration.y = 0
msg.desired_acceleration.z = 0
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.controller = 1 # 1: velocity controller, 0: position controller
#msg.desired_direction.x = direction_vector[0]
#msg.desired_direction.y = direction_vector[1]
#msg.desired_direction.z = direction_vector[2]
self.vx_previous = v[0]; self.vy_previous = v[1]; self.vz_previous = v[2]
elif self.counter < 5:
delt = (max(self.time_points)-min(self.time_points))/self.counter
#self.position = self.position + V_*delt
#msg.desired_position.x = 0#self.position[0]
#msg.desired_position.y = 0#self.position[1]
#msg.desired_position.z = 0#self.position[2]
msg.desired_velocity.x = v[0]
msg.desired_velocity.y = v[1]
msg.desired_velocity.z = v[2]
msg.desired_acceleration.x = 0.1*(v[0]-self.vx_previous)/delt
msg.desired_acceleration.y = 0.1*(v[1]-self.vy_previous)/delt
msg.desired_acceleration.z = 0.1*(v[2]-self.vz_previous)/delt
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.controller = 1 # 1: velocity controller, 0: postion controller
self.a = self.time_points[-1]
#msg.desired_direction.x = direction_vector[0]
#msg.desired_direction.y = direction_vector[1]
#msg.desired_direction.z = direction_vector[2]
#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))
#delt = (max(self.time_points)-min(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)
t = list(np.linspace(min(self.time_points), max(self.time_points), 100))
a = splrep(t,self.smooth_vx,k=4,s=3); b = splrep(t,self.smooth_vy,k=4,s=3); c = splrep(t,self.smooth_vz,k=4,s=3)
aa = splev(t,a,der=1); bb = splev(t,b,der=1); cc = splev(t,c,der=1)
aaa = splev(t,a,der=2); bbb = splev(t,b,der=2); ccc = splev(t,c,der=2)
msg.desired_velocity.x = v[0]
msg.desired_velocity.y = v[1]
msg.desired_velocity.z = v[2]
#self.position = self.position + V_*delt
#msg.desired_position.x = self.position[0]
#msg.desired_position.y = self.position[1]
#msg.desired_position.z = self.position[2]
msg.desired_acceleration.x = aa[-1]
msg.desired_acceleration.y = bb[-1]
msg.desired_acceleration.z = cc[-1]
msg.desired_jerk.x = aaa[-1]
msg.desired_jerk.y = bbb[-1]
msg.desired_jerk.z = ccc[-1]
msg.controller = 1 # 1: velocity controller, 0: postion controller
self.switching_position = self.scale_factor*X_
else:
self.phase_one = True
msg.desired_velocity.x = 0
msg.desired_velocity.y = 0
msg.desired_velocity.z = 0
position = self.switching_position
#position = self.scale_factor*(self.goal)# - 0.025*direction_vector)
msg.desired_position.x = position[0]
msg.desired_position.y = position[1]
msg.desired_position.z = position[2]
msg.desired_acceleration.x = 0
msg.desired_acceleration.y = 0
msg.desired_acceleration.z = 0
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.controller = 0 # 1: velocity controller, 0: postion controller
#self.c = self.counter
#self.pub.publish(msg)
print 'it switched'
self.pub.publish(msg)
def calculate_velocities(self, phi_, X_):
"""This function calculates the velocities for the quadrotor by numerically differentiating
the navigation function in the vicinity of the current position.
The vicinity is given by variable self.dX and the number of discretization points by self.n
in the callback function.
For example (all this is in callback), 11 points are taking within 0.1m of the current x, y
and z position and a grid is made at which value of navigation function is calculated.
This function then takes those values as a list and calculate all required derivative
using numpy's gradient function. Subsequently it publishes these values at the center point of the grid.
"""
#msg = Desired_Trajectory()
# print len(phi_)
self.counter = self.counter + 1
msg = velocities_from_navigation_function()
msg.header.stamp = rospy.Time.now()
cx = (self.n + 1) / 2
cy = (3 * self.n + 1) / 2
cz = (5 * self.n + 1) / 2
dNdX = np.gradient(phi_)
dNdX = [-i for i in dNdX]
v = np.array([dNdX[cx], dNdX[cy], dNdX[cz]])
v = self.VelocityMagnitude * v / nrm(v)
self.smooth_vx.append(v[0])
self.smooth_vy.append(v[1])
self.smooth_vz.append(v[2])
current_time = time.time()
t = current_time - self.time
self.time_points.append(t)
direction_vector = (self.goal - X_) / nrm(self.goal - X_)
if self.counter == 1:
msg.desired_velocity.x = 0 #v[0]
msg.desired_velocity.y = 0 #v[1]
msg.desired_velocity.z = 1 #v[2]
msg.desired_acceleration.x = 0
msg.desired_acceleration.y = 0
msg.desired_acceleration.z = 0
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.desired_direction.x = direction_vector[0]
msg.desired_direction.y = direction_vector[1]
msg.desired_direction.z = direction_vector[2]
self.vx_previous = v[0]
self.vy_previous = v[1]
self.vz_previous = v[2]
elif self.counter < 25:
delt = (max(self.time_points) -
min(self.time_points)) / self.counter
msg.desired_velocity.x = 0 #v[0]
msg.desired_velocity.y = 0 #v[1]
msg.desired_velocity.z = 1 #v[2]
msg.desired_acceleration.x = 0 #0.1*(v[0]-self.vx_previous)/delt
msg.desired_acceleration.y = 0 #0.1*(v[1]-self.vy_previous)/delt
msg.desired_acceleration.z = 0 #0.1*(v[2]-self.vz_previous)/delt
msg.desired_jerk.x = 0
msg.desired_jerk.y = 0
msg.desired_jerk.z = 0
msg.desired_direction.x = direction_vector[0]
msg.desired_direction.y = direction_vector[1]
msg.desired_direction.z = direction_vector[2]
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 > 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)
t = list(
np.linspace(min(self.time_points), max(self.time_points),
100))
a = splrep(t, self.smooth_vx, k=4, s=3)
b = splrep(t, self.smooth_vy, k=4, s=3)
c = splrep(t, self.smooth_vz, k=4, s=3)
aa = splev(t, a, der=1)
bb = splev(t, b, der=1)
cc = splev(t, c, der=1)
aaa = splev(t, a, der=2)
bbb = splev(t, b, der=2)
ccc = splev(t, c, der=2)
msg.desired_velocity.x = v[0]
msg.desired_velocity.y = v[1]
msg.desired_velocity.z = v[2]
msg.desired_acceleration.x = aa[-1]
msg.desired_acceleration.y = bb[-1]
msg.desired_acceleration.z = cc[-1]
msg.desired_jerk.x = aaa[-1]
msg.desired_jerk.y = bbb[-1]
msg.desired_jerk.z = ccc[-1]
msg.desired_direction.x = direction_vector[0]
msg.desired_direction.y = direction_vector[1]
msg.desired_direction.z = direction_vector[2]
self.pub.publish(msg)
