def odometry_callback(self, data):
"""get the odometry of the quadrotor"""
self.Pglobal = np.array([data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z])
q = Qt(data.pose.pose.orientation.w, data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z)
self.Rglobal = q.rotation_matrix
V = np.array([data.twist.twist.linear.x, data.twist.twist.linear.y, data.twist.twist.linear.z])
self.Vglobal = np.dot(self.Rglobal, V)
def planning_callback(self, data):
"""main motion planner, plan different trajectories for different stages"""
# get positions
self.Pglobal = np.array([
data.pose.pose.position.x, data.pose.pose.position.y,
data.pose.pose.position.z
])
q = Qt(data.pose.pose.orientation.w, data.pose.pose.orientation.x,
data.pose.pose.orientation.y, data.pose.pose.orientation.z)
self.Rglobal = q.rotation_matrix
V = np.array([
data.twist.twist.linear.x, data.twist.twist.linear.y,
data.twist.twist.linear.z
])
self.Vglobal = np.dot(self.Rglobal, V)
if self.trajectory_counter == 0:
self.initial_position = self.Pglobal
#start generating the trajectory
if self.trajectory_time <= self.initial_hover_time and self.reached_goal == False:
self.hover_up(data)
elif self.trajectory_time > self.initial_hover_time and self.reached_goal == False:
self.generate_trajectory_from_waypoints(data)
elif self.trajectory_time > self.initial_hover_time and self.reached_goal == True:
self.land(data)
# to only hover and land
#if self.trajectory_time <= self.initial_hover_time:
# self.hover_up(data)
#else:
# self.land(data)
self.trajectory_counter += 1
def odom_callback(self, data):
# get positions
self.odom_time = data.header.stamp.to_sec()
self.Pglobal = np.array([
data.pose.pose.position.x, data.pose.pose.position.y,
data.pose.pose.position.z
])
self.q = Qt(data.pose.pose.orientation.w, data.pose.pose.orientation.x,
data.pose.pose.orientation.y, data.pose.pose.orientation.z)
self.Rglobal = self.q.rotation_matrix
V = np.array([
data.twist.twist.linear.x, data.twist.twist.linear.y,
data.twist.twist.linear.z
])
self.Vglobal = np.dot(self.Rglobal, V)
#rpy = self.quaternion_to_euler(self.q[1], self.q[2], self.q[3], self.q[0])
#self.yaw = rpy[0]
vector = np.array([
self.goal[0] - self.Pglobal[0], self.goal[1] - self.Pglobal[1],
self.goal[2] - self.Pglobal[2]
])
self.directionvector = vector / np.linalg.norm(vector)
if self.planning_counter == 0:
self.initial_position = self.Pglobal
def odom_callback(self, data):
#print 'in the callback'
self.odom_time = data.header.stamp.to_sec()
self.Pglobal = np.array([
data.pose.pose.position.x, data.pose.pose.position.y,
data.pose.pose.position.z
])
self.q = Qt(data.pose.pose.orientation.w, data.pose.pose.orientation.x,
data.pose.pose.orientation.y, data.pose.pose.orientation.z)
self.Rglobal = self.q.rotation_matrix
V = np.array([
data.twist.twist.linear.x, data.twist.twist.linear.y,
data.twist.twist.linear.z
])
self.Vglobal = np.dot(self.Rglobal, V)
def odom_callback(self, data):
# get positions
self.odom_time = data.header.stamp.to_sec()
self.Pglobal = np.array([
data.pose.pose.position.x, data.pose.pose.position.y,
data.pose.pose.position.z
])
self.q = Qt(data.pose.pose.orientation.w, data.pose.pose.orientation.x,
data.pose.pose.orientation.y, data.pose.pose.orientation.z)
self.Rglobal = self.q.rotation_matrix
V = np.array([
data.twist.twist.linear.x, data.twist.twist.linear.y,
data.twist.twist.linear.z
])
self.Vglobal = np.dot(self.Rglobal, V)
if self.planning_counter == 0:
self.initial_position = self.Pglobal
def callback(self, data):
""" function to construct the trajectory
polynmial is of the form x = c0+c1*t+c2*t**2+...cn*t**n
"""
if self.counter == 0:
self.Pglobal = np.array([
data.pose.pose.position.x, data.pose.pose.position.y,
data.pose.pose.position.z
])
q = Qt(data.pose.pose.orientation.w, data.pose.pose.orientation.x,
data.pose.pose.orientation.y, data.pose.pose.orientation.z)
self.Rglobal = q.rotation_matrix
V = np.array([
data.twist.twist.linear.x, data.twist.twist.linear.y,
data.twist.twist.linear.z
])
self.Vglobal = np.dot(self.Rglobal, V)
t = time.time()
r = self.r
N = 1 + self.N # because number of terms in a polynomial = degree+1
QQ = []
AA_inv = []
#self.T = [0, 1]
#self.no_of_segments = 1
for i in range(self.no_of_segments):
q = self.construct_Q(N, r, self.T[i], self.T[i + 1])
a = self.construct_A(N, r, self.T[i], self.T[i + 1])
a_inv = scipy.linalg.pinv(a)
QQ = block_diag(QQ, q)
AA_inv = block_diag(AA_inv, a_inv)
#print 'a', a
order = 2 * r * self.no_of_segments
R = np.dot(AA_inv.T, np.dot(QQ, AA_inv))
bx = np.concatenate((self.x0, self.xT), axis=0)
by = np.concatenate((self.y0, self.yT), axis=0)
bz = np.concatenate((self.z0, self.zT), axis=0)
m = Model("qcp")
order = 2 * r * self.no_of_segments
dx = m.addVars(order,
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
vtype=GRB.CONTINUOUS,
name="dx")
dy = m.addVars(order,
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
vtype=GRB.CONTINUOUS,
name="dy")
dz = m.addVars(order,
lb=-GRB.INFINITY,
ub=GRB.INFINITY,
vtype=GRB.CONTINUOUS,
name="dz")
# using LinExpr for the second expression is significantly faster
obj1 = quicksum(dx[i] * LinExpr([(R[i][j], dx[j])
for j in range(order)])
for i in range(order))
obj2 = quicksum(dy[i] * LinExpr([(R[i][j], dy[j])
for j in range(order)])
for i in range(order))
obj3 = quicksum(dz[i] * LinExpr([(R[i][j], dz[j])
for j in range(order)])
for i in range(order))
obj = obj1 + obj2 + obj3 #+ quicksum(T[i] for i in self.no_of_segments)
j = 0
for i in range(order):
if i < r: # was r in stead of 1
m.addConstr(dx[i] == bx[i])
m.addConstr(dy[i] == by[i])
m.addConstr(dz[i] == bz[i])
elif i >= order - r:
m.addConstr(dx[i] == bx[r + j])
m.addConstr(dy[i] == by[r + j])
m.addConstr(dz[i] == bz[r + j])
j += 1
c = 1 # counter
for i in range(r, order - 2 * r, 2 * r):
m.addConstr(dx[i] == self.x_wp[c])
m.addConstr(dy[i] == self.y_wp[c])
m.addConstr(dz[i] == self.z_wp[c])
c = c + 1
for j in range(r):
m.addConstr(dx[i + j] == dx[i + j + r])
m.addConstr(dy[i + j] == dy[i + j + r])
m.addConstr(dz[i + j] == dz[i + j + r])
m.setObjective(obj, GRB.MINIMIZE)
#m.write('model.lp')
m.setParam('OutputFlag', 0)
m.setParam('PSDtol', 1e-3)
m.setParam('NumericFocus', 3)
m.optimize()
runtime = m.Runtime
optimal_objective = obj.getValue()
#print 'optimal objective is:', optimal_objective
x_coeff = [dx[i].X for i in range(order)]
y_coeff = [dy[i].X for i in range(order)]
z_coeff = [dz[i].X for i in range(order)]
Dx = np.asarray(x_coeff)[np.newaxis].T
Dy = np.asarray(y_coeff)[np.newaxis].T
Dz = np.asarray(z_coeff)[np.newaxis].T
pcx = np.dot(AA_inv, Dx)
pcy = np.dot(AA_inv, Dy)
pcz = np.dot(AA_inv, Dz)
poly_coeff_x = pcx.T.ravel().tolist()
poly_coeff_y = pcy.T.ravel().tolist()
poly_coeff_z = pcz.T.ravel().tolist()
if self.hover_counter == 0:
self.hover_start_time = data.header.stamp.to_sec()
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp #rospy.Time.now()
header.frame_id = 'world'
msg.header = header
msg.polynomial_order = self.N
for i in range(len(poly_coeff_x)):
msg.poly_x.append(poly_coeff_x[i])
msg.poly_y.append(poly_coeff_y[i])
msg.poly_z.append(poly_coeff_z[i])
msg.number_of_segments = self.no_of_segments
msg.planning_start_time = self.hover_start_time
for j in self.T:
msg.segment_end_times.append(j)
msg.desired_direction.x = 1
msg.desired_direction.y = 0
msg.desired_direction.z = 0
self.traj_polycoeff.publish(msg)
#time.sleep(self.T[self.no_of_segments]*0.9)
else:
print "now doing nothing"
#self.counter += 1
self.hover_counter += 1