def __init__(self):
self.PI = 3.14159
self.desiredState = np.zeros((12, 1))
self.desiredState[0] = 3
self.desiredState[1] = 5
self.desiredState[2] = 10
self.desiredState[8] = self.PI / 2
WaypointGeneration = WaypointGen()
self.waypoints, self.desVel, self.desAcc, self.timeVec = WaypointGeneration.waypoint_calculation(
)
self.desiredPos = WaypointGeneration.desiredPos
self.desiredTimes = WaypointGeneration.desiredTimes
def __init__(self):
self.mpcPublisher = rospy.Publisher("/hummingbird/command/motor_speed",
Actuators,
queue_size=1)
self.receivedImuQuat = Quaternion()
self.thrustConstant = 8.54858e-06
self.momentConstant = 1.6e-2
self.g = 9.81 # [m/s^2]
self.m = 0.716 # [kg]
Ixx = 0.007 # [kg*m^2]
Iyy = 0.007 # [kg*m^2]
Izz = 0.012 # [kg*m^2]
gamma = self.thrustConstant / self.momentConstant
dt = 0.1 # [sec]
L = 0.17 # [m]
# state update matrix
self.A = np.array([[1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, dt * self.g, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, -dt * self.g, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, dt, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, dt, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, dt],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]])
# input matrix
self.B = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0], [0, 0, 0, 0], [dt / self.m, 0, 0, 0],
[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0],
[0, dt / Ixx, 0, 0], [0, 0, dt / Iyy, 0],
[0, 0, 0, dt / Izz]])
# gravity component of input
self.Bg = np.array([0, 0, 0, 0, 0, -self.g * dt, 0, 0, 0, 0, 0, 0])
self.equilibriumInput = np.zeros((4))
self.equilibriumInput[0] = self.m * self.g
self.PI = 3.14159
self.speedAllocationMatrix = np.array([[
self.thrustConstant, self.thrustConstant, self.thrustConstant,
self.thrustConstant
], [0, L * self.thrustConstant, 0, (-1) * L * self.thrustConstant],
[(-1) * L * self.thrustConstant,
0, L * self.thrustConstant, 0],
[
self.momentConstant,
(-1) * self.momentConstant,
self.momentConstant,
(-1) * self.momentConstant
]])
QMult = 1
self.Q = QMult * np.eye(12)
self.Q[2][2] = 500 / QMult
self.R = 1000 * np.array([[1, 0, 0, 0], [0, 5, 0, 0], [0, 0, 5, 0],
[0, 0, 0, 1]])
self.Uinf = linalg.solve_discrete_are(self.A, self.B, self.Q, self.R,
None, None)
self.dlqrGain = np.dot(
np.linalg.inv(self.R +
np.dot(self.B.T, np.dot(self.Uinf, self.B))),
np.dot(self.B.T, np.dot(self.Uinf, self.A)))
# time now subtracted by start time
self.startTime = rospy.get_time()
# generate the waypoints
WaypointGeneration = WaypointGen()
self.waypoints, self.desVel, self.desAcc, self.timeVec = WaypointGeneration.waypoint_calculation(
)
self.desiredPos = WaypointGeneration.desiredPos
self.desiredTimes = WaypointGeneration.desiredTimes
self.mpcHorizon = 2
# number of inputs
self.nu = 4
# number of states
self.nx = 12
self.u = cv.Variable((self.nu, self.mpcHorizon))
self.x = cv.Variable((self.nx, self.mpcHorizon + 1))
# set up the MPC constraints
D2R = self.PI / 180
minVal, maxVal = self.find_min_max_waypoints()
posTolerance = 0.5 # [m]
angTolerance = 5 * D2R # [rad]
self.xmin = np.array(([
minVal[0] - posTolerance, minVal[1] - posTolerance,
minVal[2] - posTolerance, -np.Inf, -np.Inf, -np.Inf, -np.Inf,
-np.Inf, minVal[3] - angTolerance, -np.Inf, -np.Inf, -np.Inf
]))
self.xmax = np.array(([
maxVal[0] + posTolerance, maxVal[1] + posTolerance,
maxVal[2] + posTolerance, np.Inf, np.Inf, np.Inf, np.Inf, np.Inf,
maxVal[3] + angTolerance, np.Inf, np.Inf, np.Inf
]))
self.umin = np.array(([-self.m * self.g, -0.5, -0.5, -0.5]))
self.umax = np.array(([1.5 * self.m * self.g, 0.5, 0.5, 0.5]))
def __init__(self):
self.dlqrPublisher = rospy.Publisher(
"/hummingbird/command/motor_speed", Actuators, queue_size=1)
self.receivedImuQuat = Quaternion()
self.thrustConstant = 8.54858e-06
self.momentConstant = 1.6e-2
g = 9.81 # [m/s^2]
m = 0.716 # [kg]
Ixx = 0.007 # [kg*m^2]
Iyy = 0.007 # [kg*m^2]
Izz = 0.012 # [kg*m^2]
gamma = self.thrustConstant / self.momentConstant
dt = 0.01 # [sec]
L = 0.17 # [m]
# state update matrix
A = np.array([[1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, dt * g, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, -dt * g, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, dt, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, dt, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, dt],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]])
# input matrix
B = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0], [dt / m, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0], [0, 0, 0, 0], [0, dt / Ixx, 0, 0],
[0, 0, dt / Iyy, 0], [0, 0, 0, dt / Izz]])
self.equilibriumInput = np.zeros((4, 1))
self.equilibriumInput[0] = m * g
self.PI = 3.14159
self.speedAllocationMatrix = np.array([[
self.thrustConstant, self.thrustConstant, self.thrustConstant,
self.thrustConstant
], [0, L * self.thrustConstant, 0, (-1) * L * self.thrustConstant],
[(-1) * L * self.thrustConstant,
0, L * self.thrustConstant, 0],
[
self.momentConstant,
(-1) * self.momentConstant,
self.momentConstant,
(-1) * self.momentConstant
]])
QMult = 1
Q = QMult * np.eye(12)
Q[2][2] = 500 / QMult
Q[8][8] = 10000 / QMult
R = 1000 * np.array([[1, 0, 0, 0], [0, 5, 0, 0], [0, 0, 5, 0],
[0, 0, 0, 0.00001]])
# R = 1000*np.array([[1, 0, 0, 0],
# [0, 5, 0, 0],
# [0, 0, 5, 0],
# [0, 0, 0, 0.001]])
Uinf = linalg.solve_discrete_are(A, B, Q, R, None, None)
self.dlqrGain = np.dot(np.linalg.inv(R + np.dot(B.T, np.dot(Uinf, B))),
np.dot(B.T, np.dot(Uinf, A)))
# time now subtracted by start time
self.startTime = rospy.get_time()
# generate the waypoints
WaypointGeneration = WaypointGen()
self.waypoints, self.desVel, self.desAcc, self.timeVec = WaypointGeneration.waypoint_calculation(
)
self.desiredPos = WaypointGeneration.desiredPos
self.desiredTimes = WaypointGeneration.desiredTimes
self.attitudeControlOnly = 0
# deadbands [x-pos, y-pos, z-pos, yaw]
self.waypointDeadband = np.array(([0.3, 0.3, 0.5, 1 * self.PI / 180]))
def __init__(self):
self.dlqrPublisher = rospy.Publisher(
"/hummingbird/command/motor_speed", Actuators, queue_size=1)
self.receivedImuQuat = Quaternion()
self.thrustConstant = 8.54858e-06
self.momentConstant = 1.6e-2
g = 9.81 # [m/s^2]
m = 0.716 # [kg]
Ixx = 0.007 # [kg*m^2]
Iyy = 0.007 # [kg*m^2]
Izz = 0.012 # [kg*m^2]
gamma = self.thrustConstant / self.momentConstant
dt = 0.01 # [sec]
L = 0.17 # [m]
# state update matrix
A = np.array([[1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, dt, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, dt * g, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, -dt * g, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, dt, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, dt, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, dt],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]])
# input matrix
B = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0], [dt / m, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0], [0, 0, 0, 0], [0, dt / Ixx, 0, 0],
[0, 0, dt / Iyy, 0], [0, 0, 0, dt / Izz]])
# output matrix
C = np.array(([0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0]))
# enlarged state matrix with the error accumulation as a state
Ag = np.vstack((np.hstack((A, np.zeros(
(12, 2)))), np.hstack((C, np.eye(2)))))
# enlarged input matrix with the error accumulation as a state
Bg = np.vstack((B, np.zeros((2, 4))))
self.equilibriumInput = np.zeros((4, 1))
self.equilibriumInput[0] = m * g
self.PI = 3.14159
self.speedAllocationMatrix = np.array([[
self.thrustConstant, self.thrustConstant, self.thrustConstant,
self.thrustConstant
], [0, L * self.thrustConstant, 0, (-1) * L * self.thrustConstant],
[(-1) * L * self.thrustConstant,
0, L * self.thrustConstant, 0],
[
self.momentConstant,
(-1) * self.momentConstant,
self.momentConstant,
(-1) * self.momentConstant
]])
QMult = 1
Q = QMult * np.eye(12)
Q[2][2] = 500 / QMult
Q[8][8] = 10000 / QMult
# state cost with enlarged state vector
Qint = np.eye(2)
Qint[1][1] = 1
Qg = block_diag(Q, Qint)
R = 1000 * np.array([[1, 0, 0, 0], [0, 5, 0, 0], [0, 0, 5, 0],
[0, 0, 0, 0.000001]])
# R = np.array([[1, 0, 0, 0],
# [0, 1, 0, 0],
# [0, 0, 1, 0],
# [0, 0, 0, 0.1]])
UinfInt = linalg.solve_discrete_are(Ag, Bg, Qg, R, None, None)
# gain matrix containing both the lqr gain and the integral gain
self.combinedGainMatrix = np.dot(
np.linalg.inv(R + np.dot(Bg.T, np.dot(UinfInt, Bg))),
np.dot(Bg.T, np.dot(UinfInt, Ag)))
self.dlqrGainInt = self.combinedGainMatrix[:, 0:12]
print('LQR Gain With Integrator: ')
print(self.dlqrGainInt)
print('\n')
self.integralGain = self.combinedGainMatrix[:, 12:20]
print('Integral Gain: ')
print(self.integralGain)
print('\n')
self.previousError = np.zeros((2, 1))
Uinf = linalg.solve_discrete_are(A, B, Q, R, None, None)
self.dlqrGain = np.dot(np.linalg.inv(R + np.dot(B.T, np.dot(Uinf, B))),
np.dot(B.T, np.dot(Uinf, A)))
print('LQR Gain: ')
print(self.dlqrGain)
print('\n')
# time now subtracted by start time
self.startTime = rospy.get_time()
# generate the waypoints
WaypointGeneration = WaypointGen()
self.waypoints, self.desVel, self.desAcc, self.timeVec = WaypointGeneration.waypoint_calculation(
)
self.desiredPos = WaypointGeneration.desiredPos
self.desiredTimes = WaypointGeneration.desiredTimes
# deadbands [x-pos, y-pos, z-pos, yaw]
self.waypointDeadband = np.array(([0.5, 0.5, 0.5, 5 * self.PI / 180]))
self.waypointEndAchieved = 0
# error accumulation limits
self.errorAccumLimit = np.array(([0.5, 1 * self.PI / 180]))
#!/usr/bin/env python3
from __future__ import print_function
from __future__ import division
import rospy
import rosbag
import numpy as np
import matplotlib.pyplot as plt
from waypoint_generation_library import WaypointGen
from scipy import linalg
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from mpl_toolkits import mplot3d
WaypointGeneration = WaypointGen()
waypoints, desVel, desAcc, timeVec = WaypointGeneration.waypoint_calculation()
desiredPos = WaypointGeneration.desiredPos
# plot the waypoints
figPos = plt.figure()
axPos = plt.axes(projection = '3d')
axPos.plot3D(desiredPos[:,0], desiredPos[:,1], desiredPos[:,2], 'ro')
pnt3d = axPos.scatter(waypoints[:,0], waypoints[:,1], waypoints[:,2], c = timeVec)
cbar = plt.colorbar(pnt3d)
cbar.set_label("Time [sec]")
# label the axes and give title
axPos.set_xlabel('X-Axis [m]')
axPos.set_ylabel('Y-Axis [m]')
axPos.set_zlabel('Z-Axis [m]')