def translated_scaling_map(self, c_, beta0, beta1, beta2, beta3, beta4):
"""calculate star to sphere transformation
formulae as per koditcheck didnot seem to work, caili's formulation is used instead to try
"""
TT0 = []
TT1 = []
TT2 = []
TT3 = []
TT4 = []
for i in range(len(c_)):
v0 = np.sqrt(1 + beta0[i]) * self.rho0 / nrm(c_[i] - self.q0)
v1 = np.sqrt(1 + beta1[i]) * self.rho1 / nrm(c_[i] - self.q1)
v2 = np.sqrt(1 + beta2[i]) * self.rho2 / nrm(c_[i] - self.q2)
v3 = np.sqrt(1 + beta3[i]) * self.rho3 / nrm(c_[i] - self.q3)
v4 = np.sqrt(1 + beta4[i]) * self.rho4 / nrm(c_[i] - self.q4)
#v0 = (1 + beta0[i]) * self.rho0 / nrm(current_position - self.q0)
#v1 = (1 + beta1[i]) * self.rho1 / nrm(current_position - self.q1)
#v2 = (1 + beta2[i]) * self.rho2 / nrm(current_position - self.q2)
#v3 = (1 + beta3[i]) * self.rho3 / nrm(current_position - self.q3)
#v4 = np.sqrt(1 + beta4[i]) * self.rho4 / nrm(current_position - self.q4)
TT0.append(v0 * (c_[i] - self.q0) + self.p0)
TT1.append(v1 * (c_[i] - self.q1) + self.p1)
TT2.append(v2 * (c_[i] - self.q2) + self.p2)
TT3.append(v3 * (c_[i] - self.q3) + self.p3)
TT4.append(v4 * (c_[i] - self.q4) + self.p4)
"""ASK: what about translated center of the workspace and obstacles"""
return TT0, TT1, TT2, TT3, TT4
def get_beta(self, q): # function to return beta values
beta0 = self.R0**2 - nrm(q - self.q0)**2
beta1 = -self.R1**2 + nrm(q - self.q1)**2
beta2 = -self.R2**2 + nrm(q - self.q2)**2
beta3 = -self.R3**2 + nrm(q - self.q3)**2
beta4 = -self.R4**2 + nrm(q - self.q4)**2
beta = beta0 * beta1 * beta2 * beta3 * beta4
return beta, beta0, beta1, beta2, beta3, beta4
def beta_for_sphereworld(self, _q): # function to return beta values
_b0 = self.rho0**2 - nrm(_q - self.p0)**2
_b1 = -self.rho1**2 + nrm(_q - self.p1)**2
_b2 = -self.rho2**2 + nrm(_q - self.p2)**2
_b3 = -self.rho3**2 + nrm(_q - self.p3)**2
_b4 = -self.rho4**2 + nrm(_q - self.p4)**2
beta = _b0 * _b1 * _b2 * _b3 * _b4
return beta, _b0, _b1, _b2, _b3, _b4
def beta_for_sphereworld(self, _q): # function to return beta values
b = [
self.rho[i]**2 -
nrm(_q - self.p[i])**2 if i == 0 else -self.rho[i]**2 +
nrm(_q - self.p[i])**2 for i in range(self.no_of_obstacles)
]
b = [0 if i < 0 else i for i in b]
beta = reduce(mul, b)
return beta
def nav_func(self, q):
Beta, b0, b1, b2, b3, b4 = self.get_beta(q)
dBeta_dq = -2*(q-self.q0)*b1*b2*b3*b4 + \
2*b0*((q-self.q1)*b2*b3*b4 + (q-self.q2)*b3*b4*b1 + \
(q-self.q3)*b4*b1*b2 + (q-self.q4)*b1*b2*b3)
a = nrm(q - self.qd)**2
b = nrm(q - self.qd)**(2 * self.k) + Beta
c = pwr(b, 1.0 / self.k)
phi0 = a / c
dphi_dq = 2 * (q - self.qd) / c - (a / (self.k * b * c)) * (
2 * self.k * pwr(sq(a), 2 * self.k - 2) * (q - self.qd) + dBeta_dq)
return phi0, dphi_dq
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 = 1 #direction_vector[0]
msg.desired_direction.y = 0 #direction_vector[1]
msg.desired_direction.z = 0 #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 distance_to_goal(self, q):
"""takes current position and returns the distance to goal function"""
"""TODO: Decide which function should be goal function"""
dist_to_goal = nrm(q - self.goal)**2
#dist_to_goal = nrm(q - self.goal)**4
#dist_to_goal = nrm(q - self.goal)**(2 * self.k)
return dist_to_goal
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 generate_velocities(self, odom):
dt = 0.1
#msg = velocities_from_navigation_function()
X = np.array([
odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z
])
phi, grad_phi = self.nav_func(X)
Vi = -1.0 * grad_phi # velocity vector
v = Vi / nrm(Vi)
#print v
V_bar = self.VelocityMagnitude * Vi / nrm(
Vi) # unit vector of velocity
#print phi, grad_phi
if self.counter == 0:
Xdesired = X
self.previous_time = time.time()
else:
delt = time.time() - self.previous_time
#print delt, phi, grad_phi, V_bar*delt
Xdesired = X + V_bar * dt
if np.linalg.norm(Xdesired - self.qd) <= 0.01:
print 'hi'
Xdesired = self.qd
self.previous_time = time.time()
#print Xdesired
#msg.desired_velocity.x = V_bar[0]; msg.desired_velocity.y = V_bar[1]
#msg.desired_velocity.z = V_bar[2]; self.pub.publish(msg)
x = Xdesired - X
f1 = open('vel_nav.txt', 'a')
f1.write("%s,%s,%s,%s,%s,%s\n" %
(X[0], X[1], X[2], Xdesired[0], Xdesired[1], Xdesired[2]))
msg_pelican = PoseStamped()
msg_pelican.pose.position.x = Xdesired[0]
msg_pelican.pose.position.y = Xdesired[1]
msg_pelican.pose.position.z = Xdesired[2]
msg_pelican.pose.orientation.x = 0
msg_pelican.pose.orientation.y = 0
msg_pelican.pose.orientation.z = 0
msg_pelican.pose.orientation.w = 1
self.pub_pelican.publish(msg_pelican)
#time.sleep(0.1)
self.counter = self.counter + 1
"""
def translated_scaling_map(self, _c, b):
"""calculate star to sphere transformation
"""
max_betas = [max(b[i]) for i in range(len(b))]
#v = [[np.sqrt(1 + b[i][j]/max_betas[i]) * self.rho[j] / nrm(_c[i] - self.q[j]) for j in range(len(b[i]))] for i in range(len(b))]
v = [[
np.sqrt(1 + b[i][j]) * self.rho[j] / nrm(_c[i] - self.q[j])
for j in range(len(b[i]))
] for i in range(len(b))]
T = [[
v[i][j] * (_c[i] - self.q[j]) + self.p[j] for j in range(len(b[i]))
] for i in range(len(b))]
return T
def translated_scaling_map(self, _c, b):
"""calculate star to sphere transformation
formulae as per koditcheck didnot seem to work, caili's formulation is used instead
"""
max_betas = [max(b[i]) for i in range(len(b))]
#v = [[np.sqrt(1 + b[i][j]/max_betas[i]) * self.rho[j] / nrm(_c[i] - self.q[j]) for j in range(len(b[i]))] for i in range(len(b))]
v = [[
np.sqrt(1 + b[i][j] / max_betas[i]) * self.rho[j] /
nrm(_c[i] - self.q[j]) for j in range(len(b[i]))
] for i in range(len(b))]
T = [[
v[i][j] * (_c[i] - self.q[j]) + self.p[j] for j in range(len(b[i]))
] for i in range(len(b))]
return T
def __init__(self, pot_type_IN = None, int_loc_IN = [0,0,0], \
int_norm_IN = [0,1,0], **kwargs):
self.pot_type = pot_type_IN
# A point on the plane that defines the interface
self.int_loc = np.array(int_loc_IN)
# The norm to the plane that defines the interface
self.int_norm = np.array(int_norm_IN) / nrm(np.array(int_norm_IN))
self.pot_params = kwargs
# Check that all required keyword arguments are provided
if self._kwargs_check():
# Then generate potential function based on pot_type
if self.pot_type is None:
def pot(xyz, sublat=None):
return self.equal_onsite(xyz, onsite=0)
elif self.pot_type == 'step':
def pot(xyz, sublat):
return self.pot_func_step(xyz, sublat, **self.pot_params)
elif self.pot_type == 'tanh':
def pot(xyz, sublat):
return self.pot_func_tanh(xyz, sublat, **self.pot_params)
elif self.pot_type == 'well':
def pot(xyz, sublat=None):
return self.pot_func_BLG_well(xyz, **self.pot_params)
else:
sys.exit('Invalid potential choice in potentials.py')
self.pot_func = pot
def generate_position_trajectory(self, X_, odomm):
msg = velocities_from_navigation_function()
msg1 = Point()
msg.header.stamp = odomm.header.stamp #rospy.Time.now()
direction_vector = (self.goal - X_) / nrm(self.goal - X_)
#direction_vector = np.array([1,0,0])
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.ddes_dot.x = 0#direction_vector[0]
#msg.ddes_dot.y = 0#direction_vector[1]
#msg.ddes_dot.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 = self.hover_at #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.jdes.x = 0; msg.jdes.y = 0; msg.jdes.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))
msg1.x = 0
msg1.y = 0
msg1.z = 0
self.pub1.publish(msg1)
self.pub.publish(msg)
self.position = np.array([position[0], position[1], position[2]])
self.time = time.time()
def plot_interface(self, int_loc):
# Estimate the steps between atoms in the x-direction
approx_x_step = np.min(nrm(self.xyz[0] - self.xyz[1:], axis=1))
slct = np.logical_and.reduce(
(self.xyz[:, 0] > np.min(self.xyz[:, 0]),
self.xyz[:, 0] < np.min(self.xyz[:, 0]) + 5 * approx_x_step,
self.xyz[:, 1] > int_loc[1] - 4 * approx_x_step,
self.xyz[:, 1] < int_loc[1] + 4 * approx_x_step))
ax = make_plot_xyz(self.xyz[slct], self.sublat[slct])
bnd = np.array(
[int_loc - self.lat_vecs_sc[0], int_loc + self.lat_vecs_sc[0]])
ax.plot(bnd[:, 0], bnd[:, 1], 'k-')
plt.show()
def plot_interface(self, int_loc):
cell_rng = [0, 0]
if self.cell_num[0] > 0: cell_rng[0] = self.cell_num[0] - 1
if self.cell_num[1] > 0:
cell_rng[1] = self.cell_num[0]
else:
cell_rng[1] = self.cell_num[0] - 1
xyz = np.concatenate([self.cells[i].xyz for i in list(set(cell_rng))],
axis=0)
sublat = np.concatenate(
[self.cells[i].sublat for i in list(set(cell_rng))], axis=0)
# Estimate the steps between atoms in the x-direction
approx_x_step = np.min(nrm(xyz[0] - xyz[1:], axis=1))
# Make a boolean array which will select atoms furthest to the left in
# the x-direction
slct = np.logical_and(
xyz[:, 0] > np.min(xyz[:, 0]),
xyz[:, 0] < np.min(xyz[:, 0]) + 5 * approx_x_step)
# Select the appropriate atoms
xyz = xyz[slct]
sublat = sublat[slct]
ax = make_plot_xyz(xyz, sublat)
bnd = np.array([
int_loc - self.cells[0].lat_vecs_sc[0],
int_loc + self.cells[0].lat_vecs_sc[0]
])
ax.plot(bnd[:, 0], bnd[:, 1], 'k-')
ax.set_xlim(np.min(xyz[:, 0]), np.max(xyz[:, 0]))
plt.show()
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 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)
def callback(self, odom):
"""generate trajectories"""
#a = time.time()
Xprime = np.array([
odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z
])
X = Xprime / 10.0
#print X
#discretization in x direction
x1 = np.linspace(X[0] - self.dX, X[0] + self.dX, self.n)
y1 = np.array([X[1]])
z1 = np.array([X[2]])
c1 = np.vstack(np.meshgrid(x1, y1, z1)).reshape(3, -1).T
#discretization in y direction
x2 = np.array([X[0]])
y2 = np.linspace(X[1] - self.dX, X[1] + self.dX, self.n)
z2 = np.array([X[2]])
c2 = np.vstack(np.meshgrid(x2, y2, z2)).reshape(3, -1).T
#discretization in z direction
x3 = np.array([X[0]])
y3 = np.array([X[1]])
z3 = np.linspace(X[2] - self.dX, X[2] + self.dX, self.n)
c3 = np.vstack(np.meshgrid(x3, y3, z3)).reshape(3, -1).T
c = np.concatenate((c1, c2, c3), axis=0)
#print '1',len(c)
remove_indices1 = []
#print c
for i in range(len(c)):
xx = c[i, 0]**2
yy = c[i, 1]**2
zz = (c[i, 2] - self.WS[2])**2
s = self.WS[4]
r = self.WS[3]
ratio = (s / r)**2
squir = xx + yy + zz - ratio * xx * yy - ratio * yy * zz - ratio * xx * zz + ratio**2 * xx * yy * zz
if squir >= r**2:
remove_indices1.append(i)
c = np.delete(c, remove_indices1, 0)
#print '2',len(c)
g_ws, r_ws, s_ws = self.similarity_transformation(self.WS, c)
beta_ws = self.calculate_beta_squircle(g_ws, s_ws, r_ws, 1)
g_o1, r_o1, s_o1 = self.similarity_transformation(self.O1, c)
beta_o1 = self.calculate_beta_squircle(g_o1, s_o1, r_o1, 0)
g_o2, r_o2, s_o2 = self.similarity_transformation(self.O2, c)
beta_o2 = self.calculate_beta_squircle(g_o2, s_o2, r_o2, 0)
g_o3, r_o3, s_o3 = self.similarity_transformation(self.O3, c)
beta_o3 = self.calculate_beta_squircle(g_o3, s_o3, r_o3, 0)
g_o4, r_o4, s_o4 = self.similarity_transformation(self.O4, c)
beta_o4 = self.calculate_beta_squircle(g_o4, s_o4, r_o4, 0)
remove_indices2 = []
for i in range(len(c)):
if beta_ws[i] < 0.0 or beta_o1[i] < 0.0 or beta_o2[
i] < 0.0 or beta_o3[i] < 0.0 or beta_o4[i] < 0.0:
remove_indices2.append(i)
c = np.delete(c, remove_indices2, 0)
#print len(c)
beta_ws = np.delete(beta_ws, remove_indices2, 0)
beta_o1 = np.delete(beta_o1, remove_indices2, 0)
beta_o2 = np.delete(beta_o2, remove_indices2, 0)
beta_o3 = np.delete(beta_o3, remove_indices2, 0)
beta_o4 = np.delete(beta_o4, remove_indices2, 0)
s0, s1, s2, s3, s4, sd = self.calculate_analytic_switch(
c, beta_ws, beta_o1, beta_o2, beta_o3, beta_o4)
T0, T1, T2, T3, T4 = self.tranlated_scaling_map(
c, beta_ws, beta_o1, beta_o2, beta_o3, beta_o4)
h = self.star_to_sphere_transform(c, s0, s1, s2, s3, s4, sd, T0, T1,
T2, T3, T4)
phi = self.navigation_function(h)
#print '10', time.time()-a
#print len(phi)
if nrm(X - self.goal) > 0.05:
self.calculate_velocities(phi, X)
#print '11', time.time()-a
else:
print 'quad is withn 0.1 of the goal point'
pass
def distance_to_goal(self, q):
"""returns goal function"""
return nrm(q - self.goal)**2.2 #2.2
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 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 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 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)
