def publish_thrust(self, thrust):
# create single message
thrust_msg = RateThrust()
thrust_msg.thrust.z = thrust
self.input_publisher.publish(thrust_msg)
rospy.loginfo(thrust_msg)
rospy.loginfo("Published body vertical thrust: {}".format(thrust))
def step(self, action):
[thrustd, dphi, dtheta, dpsi] = action
new_ctrl = RateThrust()
new_ctrl.angular_rates.x = dphi
new_ctrl.angular_rates.y = dtheta
new_ctrl.angular_rates.z = dpsi
new_ctrl.thrust.z = thrustd
self.ctrl_pub.publish(new_ctrl)
def callback(self, msg):
rt_msg = RateThrust()
t = np.array([msg.ua.x, msg.ua.y, msg.ua.z + 9.81])
norm = np.linalg.norm(t)
rt_msg.thrust.z = norm
rt_msg.angular_rates.x = msg.twist.angular.x
rt_msg.angular_rates.y = msg.twist.angular.y
rt_msg.angular_rates.z = msg.twist.angular.z
self.thrust_publisher.publish(rt_msg)
def handle_output_path(req):
print("returning path instructions")
msg = RateThrust()
msg.header.frame_id = "uav/imu"
msg.header.stamp = Time.now()
msg.thrust.z = self.idleThrust + 1
msg.angular_rates.x = 0.05
msg.angular_rates.y = 0.05
msg.angular_rates.z = 0.05
return OutputPathsResponse(msg)
def publish_thrust(self, thrust):
"""
@description publish thrust values to 'wake up' flightgoggles
simulator. It assumes flightgoggles simulator is running
"""
# create single message
thrust_msg = RateThrust()
thrust_msg.thrust.z = thrust
self.fg_publisher.publish(thrust_msg)
rospy.loginfo(thrust_msg)
rospy.loginfo("Published body vertical thrust: {}".format(thrust))
def controlCommandCallback(self, msg):
print(msg.expected_execution_time.to_sec() - rospy.Time.now().to_sec())
rateThrustCommand = RateThrust()
rateThrustCommand.header.stamp = rospy.Time.now()
rateThrustCommand.angular_rates.x = msg.bodyrates.x
rateThrustCommand.angular_rates.y = msg.bodyrates.y
rateThrustCommand.angular_rates.z = msg.bodyrates.z
rateThrustCommand.thrust.x = 0
rateThrustCommand.thrust.y = 0
rateThrustCommand.thrust.z = msg.collective_thrust
self.bodyRatesCommandPub.publish(rateThrustCommand)
def command(self, c, omega):
omega_x = float(omega[0]) / self.max_omega
omega_y = float(omega[1]) / self.max_omega
omega_z = float(omega[2]) / self.max_omega
msg = RateThrust()
msg.thrust.x = 0
msg.thrust.y = 0
msg.thrust.z = c
msg.angular_rates.x = omega_x
msg.angular_rates.y = omega_y
msg.angular_rates.z = omega_z
self.rate_thrust_publisher.publish(msg)
def Publish_rateThrust(Thrust,roll_rate,pitch_rate,yaw_rate):
rate_data=RateThrust()
rate_data.header.stamp = rospy.Time.now()
rate_data.header.frame_id = "uav/imu"
rate_data.angular_rates.x=np.float64(roll_rate)
rate_data.angular_rates.y=np.float64(pitch_rate)
rate_data.angular_rates.z=np.float64(yaw_rate)
rate_data.thrust.x=np.float64(0.0)
rate_data.thrust.x=np.float64(0.0)
rate_data.thrust.z=np.float64(Thrust)
pub.publish(rate_data)
def callback(self, range, rate_thrust, pid_z, pid_roll, pid_pitch,
pid_yaw):
print(range.range)
if range.range >= -1.0:
msg = RateThrust()
msg.header.frame_id = "uav/imu"
msg.header.stamp = Time.now()
msg.thrust.z = self.idle_thrust + 1
msg.angular_rates.x = 0
msg.angular_rates.y = 0
msg.angular_rates.z = 0
self.pub_vel.publish(msg)
else:
print("Its here")
msg = RateThrust()
msg.header.frame_id = "uav/imu"
msg.header.stamp = Time.now()
msg.thrust.z = rate_thrust.thrust.z - pid_z.data
msg.angular_rates.x = rate_thrust.angular_rates.x - pid_roll.data
msg.angular_rates.y = rate_thrust.angular_rates.y - pid_pitch.data
msg.angular_rates.z = rate_thrust.angular_rates.z - pid_yaw.data
self.pub_vel.publish(msg)
def __init__(self):
self.beginning_time = rospy.get_time()
self.idle_thrust = float(9.81)
self.path_sub = message_filters.Subscriber(
'/pathplanning/input/rateThrust', RateThrust)
self.ratethrust_z_sub = message_filters.Subscriber(
"/rateThrustZ/control_effort", Float64)
self.ratethrust_roll_sub = message_filters.Subscriber(
"/rateThrustPitch/control_effort", Float64)
self.ratethrust_pitch_sub = message_filters.Subscriber(
"/rateThrustRoll/control_effort", Float64)
self.ratethrust_yaw_sub = message_filters.Subscriber(
"/rateThrustYaw/control_effort", Float64)
self.ratethrust_height_sub = message_filters.Subscriber(
"/rateThrustYaw/control_effort", Float64)
self.pub_vel = rospy.Publisher('output/rateThrust',
RateThrust,
queue_size=2)
ts = message_filters.ApproximateTimeSynchronizer([
self.ratethrust_height_sub, self.path_sub, self.ratethrust_z_sub,
self.ratethrust_roll_sub, self.ratethrust_pitch_sub,
self.ratethrust_yaw_sub
],
10,
0.1,
allow_headerless=True)
ts.registerCallback(self.callback)
time_to_start = 0.5
d = rospy.Duration(time_to_start, 0)
now = rospy.get_rostime()
end_time = d + now
msg = RateThrust()
msg.header.frame_id = "uav/imu"
msg.header.stamp = Time.now()
msg.thrust.z = self.idle_thrust + 0.1
msg.angular_rates.x = 0
msg.angular_rates.y = 0
msg.angular_rates.z = 0
while rospy.get_rostime() < end_time:
print("ITS STARTING")
self.pub_vel.publish(msg)
def ControlLoop(self):
# Set the rate
rate = rospy.Rate(50)
# Keep track how many loops have happend
loop_counter = 0
# While running
while not rospy.is_shutdown():
# Create the message we are going to send
msg = RateThrust()
msg.header.stamp = rospy.get_rostime()
# If the drone has not been armed
if self.armed == False:
# Arm the drone
msg.thrust = Vector3(0, 0, 10)
msg.angular_rates = Vector3(0, 0, 0)
self.armed = True
# Behave normally
else:
# Use a PID to calculate the rates you want to publish to maintain an angle
roll_output = self.rollPID.get_output(self.roll_setpoint,
self.roll_reading)
pitch_output = self.pitchPID.get_output(
self.pitch_setpoint, self.pitch_reading)
if self.no_yaw:
yaw_output = self.yaw_output
#rospy.loginfo("my output: " + str(yaw_output) + " old yaw:" + str(self.yawPID.get_output(self.yaw_setpoint, self.yaw_reading)))
else:
yaw_output = self.yawPID.get_output(
self.yaw_setpoint, self.yaw_reading)
# Create the message
msg.thrust = Vector3(0, 0, self.thrust_setpoint)
msg.angular_rates = Vector3(roll_output, pitch_output,
yaw_output)
# Publish the message
self.rate_pub.publish(msg)
# Sleep any excess time
rate.sleep()
def __init__(self):
rospy.init_node('robot_controller', anonymous=True)
self.local_deg_pub = rospy.Publisher('/uav/input/rateThrust',
RateThrust,
queue_size=10)
self.reset_pub = rospy.Publisher('/uav/collision', Empty, queue_size=1)
self.tf_pos_sub = rospy.Subscriber('/tf', TFMessage, self.tf_callback)
self.joy_sub = rospy.Subscriber('/control_nodes/joy', Joy,
self.joy_callback)
self.rate = rospy.Rate(30)
self.pose = TFMessage()
self.deg = RateThrust()
self.joy = Joy()
self.reset = Empty()
self.mode = 2 #default is mode 2
def callback(self,data):
rt_msg = RateThrust()
rt_msg.header.stamp = rospy.Time.now()
rt_msg.header.frame_id = 'uav/imu'
rt_msg.angular_rates.x = 0.0
rt_msg.angular_rates.y = 0.0
rt_msg.angular_rates.z = 0.0
rt_msg.thrust.x = 0.0
rt_msg.thrust.y = 0.0
rt_msg.thrust.z = 9.9
self.rt_publisher.publish(rt_msg)
#rospy.loginfo("Published rateThrust message :) !")
rospy.loginfo(rt_msg)
def crash_vehicle(self):
self.crashFlag = False
while 1:
if self.loc_flag:
print self.get_dist_to_gate()
target_rate_thrust = RateThrust()
target_rate_thrust.header.frame_id = "home"
target_rate_thrust.header.stamp = rospy.Time.now()
target_rate_thrust.angular_rates.x = 1
target_rate_thrust.angular_rates.y = 0
target_rate_thrust.angular_rates.z = 0
target_rate_thrust.thrust.z = 10.5
self.rate_thrust_pub.publish(target_rate_thrust)
if self.crashFlag:
break
self.rate.sleep()
self.crashFlag = False
print 'crashed'
def gate_cb(self, state):
markers = state.markers
for marker in markers:
if marker.landmarkID.data == "Gate10":
if marker.markerID.data == "1":
marker1 = [marker.x, marker.y]
if marker.markerID.data == "2":
marker2 = [marker.x, marker.y]
if marker.markerID.data == "3":
marker3 = [marker.x, marker.y]
if marker.markerID.data == "4":
marker4 = [marker.x, marker.y]
# try:
marker1, marker2, marker3, marker4
markersArr = np.array([marker1, marker2, marker3, marker4])
if self.imu_cb_flag:
imu_state = [
self.ang_x, self.ang_y, self.ang_z, self.lin_x, self.lin_y,
self.lin_z
]
nn_input = np.concatenate(
(markersArr.flatten(), np.array(imu_state)))
print nn_input
outputs = self.model.predict(nn_input.reshape(-1, 14))
roll = outputs[0][0]
pitch = outputs[0][1]
yaw = 0
thrust = outputs[0][2]
print outputs
ang_scale = 1
thrust_scale = 1
target_attitude_thrust = RateThrust()
target_attitude_thrust.header.frame_id = "home"
target_attitude_thrust.header.stamp = rospy.Time.now()
target_attitude_thrust.angular_rates.x = roll
target_attitude_thrust.angular_rates.y = pitch
target_attitude_thrust.angular_rates.z = yaw
target_attitude_thrust.thrust.z = thrust
self.attitude_thrust_pub.publish(target_attitude_thrust)
def __init__(self):
# Rate init
# DECIDE ON PUBLISHING RATE
self.rate = rospy.Rate(120.0) # MUST be more then 2Hz
self.joy_cb_flag = False
self.imu_cb_flag = False
self.attitude_thrust_pub = rospy.Publisher("/uav/input/rateThrust",
RateThrust,
queue_size=10)
self.model = load_model('working_model_roll_pitch_thrust.h5')
self.model._make_predict_function()
self.counter = 1
attitude_target_sub = rospy.Subscriber("/joy", Joy, self.joy_cb)
attitude_target_sub = rospy.Subscriber("/uav/sensors/imu", Imu,
self.imu_cb)
attitude_target_sub = rospy.Subscriber("/uav/camera/left/ir_beacons",
IRMarkerArray, self.gate_cb)
self.attitude_thrust_pub = rospy.Publisher("/uav/input/rateThrust",
RateThrust,
queue_size=10)
thrust_scale = 2
ang_scale = 1
counter = 0
while not rospy.is_shutdown():
# if(self.joy_cb_flag == True):
target_attitude_thrust = RateThrust()
target_attitude_thrust.header.frame_id = "home"
target_attitude_thrust.header.stamp = rospy.Time.now()
target_attitude_thrust.angular_rates.x = 0
target_attitude_thrust.angular_rates.y = 0
target_attitude_thrust.angular_rates.z = 0
target_attitude_thrust.thrust.z = 10.5
self.attitude_thrust_pub.publish(target_attitude_thrust)
self.rate.sleep()
counter += 1
if counter > 20:
break
def ControlLoop(self):
# Set the rate
rate = rospy.Rate(50)
# Keep track how many loops have happend
loop_counter = 0
# While running
while not rospy.is_shutdown():
# Create the message we are going to send
msg = RateThrust()
msg.header.stamp = rospy.get_rostime()
# If the drone has not been armed
if self.armed == False:
# Arm the drone
msg.thrust = Vector3(0, 0, 10)
msg.angular_rates = Vector3(0, 0, 0)
# Behave normally (Drone is armed)
else:
# Use a PID to calculate the rates you want to publish to maintain an angle
output = [
pids.get_output(setp, read) for pids, setp, read in zip(
self.rpy_PIDS, self.rpy_setpoints, self.rpy_readings)
]
# Create the message
msg.thrust = Vector3(0, 0, self.thrust_setpoint)
msg.angular_rates = Vector3(output[0], output[1], output[2])
# Publish the message
self.rate_pub.publish(msg)
# Sleep any excess time
rate.sleep()
def act(self, state, counter):
# if np.random.rand() <= self.epsilon:
# return random.randrange(self.action_size)
print state
outputs = self.model.predict(state.reshape(-1, 14))
roll = outputs[0][0]
pitch = outputs[0][1]
yaw = 0
thrust = outputs[0][2]
print outputs
ang_scale = 1
thrust_scale = 1
target_rate_thrust = RateThrust()
target_rate_thrust.header.frame_id = "home"
target_rate_thrust.header.stamp = rospy.Time.now()
target_rate_thrust.angular_rates.x = roll
target_rate_thrust.angular_rates.y = pitch
target_rate_thrust.angular_rates.z = yaw
if counter < 5:
target_rate_thrust.thrust.z = thrust + 1
else:
target_rate_thrust.thrust.z = thrust
self.rate_thrust_pub.publish(target_rate_thrust)
return outputs # returns action
def pubRateThrust():
# pub = rospy.Publisher('output/rateThrust', RateThrust, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
thr_msg = RateThrust()
thr_msg.header.frame_id = "uav/imu"
thr_msg.header.stamp = rospy.Time.now()
marker = IRMarker()
pitch = 0.0
roll = -0.1
yaw = 0.5
vertical = 11
thr_msg.angular_rates.x = roll
thr_msg.angular_rates.y = pitch
thr_msg.angular_rates.z = yaw
thr_msg.thrust.z = vertical
# rospy.loginfo(thr_msg)
# pub.publish(thr_msg)
rate.sleep()
def main():
# init rosnodes
rospy.init_node('lqr_controller')
tf_listener = tf.TransformListener()
imu_sub = rospy.Subscriber('/uav/sensors/imu', Imu, callback=imu_cb, queue_size=1)
start_sim_pub = rospy.Publisher('/uav/input/arm', Empty, queue_size=1)
end_sim_pub = rospy.Publisher('/uav/input/reset', Empty, queue_size=1)
ctrl_pub = rospy.Publisher('/uav/input/rateThrust', RateThrust, queue_size=10)
path_pub = rospy.Publisher('ref_traj', Path, queue_size=1)
tf_br = tf.TransformBroadcaster()
# Global Constants
Ixx = rospy.get_param("/uav/flightgoggles_uav_dynamics/vehicle_inertia_xx")
Iyy = rospy.get_param("/uav/flightgoggles_uav_dynamics/vehicle_inertia_yy")
Izz = rospy.get_param("/uav/flightgoggles_uav_dynamics/vehicle_inertia_zz")
m = rospy.get_param("/uav/flightgoggles_uav_dynamics/vehicle_mass")
g = 9.81
# aggr = rospy.get_param("/splinegen/aggr")
aggr = 10**4
T = 40.0 # T=None to use aggr instead.
# Flat Dynamics
FLAT_STATES = 7
FLAT_CTRLS = 4
A = np.zeros((FLAT_STATES, FLAT_STATES))
A[0:3, 3:6] = np.eye(3)
B = np.zeros((FLAT_STATES, FLAT_CTRLS))
B[3:, :] = np.eye(4)
Gff = np.array([[0, 0, g, 0]]).T # gravity compensation
Q = np.diag([10, 10, 20, 0.01, 0.01, 0.01, 10])
R = np.eye(FLAT_CTRLS) * 1
# Trajectory generation
# get gate poses
num_gates = 14
gate_ids = list(range(0, num_gates))
gate_transforms = get_gate_positions(gate_ids)
# inital drone pose and generated spline of waypoints
print("Solving for optimal trajectory...")
dt = 0.05
rate = rospy.Rate(int(1./dt))
x0 = np.array([[0., 0., 1., 0., 0., 0., 0.]]).T
drone_traj = DroneTrajectory()
drone_traj.set_start(position=x0[:3], velocity=x0[3:6])
drone_traj.set_end(position=x0[:3], velocity=x0[3:6])
for (trans, rot) in gate_transforms.values():
drone_traj.add_gate(trans, rot)
drone_traj.solve(aggr, T=T)
# PID Controller for setting angular rates
pid_phi = PID(Kp=7, Ki=0, Kd=1, setpoint=0)
pid_theta = PID(Kp=7, Ki=0, Kd=1, setpoint=0)
# Optimal control law for flat system
print("Solving linear feedback system...")
K, S, E = control.lqr(A, B, Q, R)
# Generate trajectory (starttime-sensitive)
print("Generating optimal trajectory...")
start_time = rospy.get_time()
xref_traj = drone_traj.as_path(dt=dt, frame='world', start_time=rospy.Time.now())
max_time = xref_traj.poses[-1].header.stamp.to_sec() - start_time
# plotting
N = len(xref_traj.poses) + int(3.0 / dt)
time_axis = []
xref_traj_series = np.zeros((FLAT_STATES, N))
x_traj_series = np.zeros((FLAT_STATES, N))
# run simulation
print("Running simulation and executing controls...")
iter = 0
x = x0
prev_pose = None
phid_traj = []
thetad_traj = []
psid_traj = []
phi_traj = []
theta_traj = []
psi_traj = []
# for pose in xref_traj.poses:
while not rospy.is_shutdown() and iter < N:
# publish arm command and ref traj
start_sim_pub.publish(Empty())
path_pub.publish(xref_traj)
# get next target waypoint
t = (rospy.get_time() - start_time) # % max_time
pos_g, vel_g, ori_g = drone_traj.full_pose(t)
# TODO: Use these desired roll/pitch or the ones generated from fdbk law?
[psid, _, _] = Rotation.from_quat(ori_g).as_euler('ZYX')
psid = 0
xref = np.array([[
pos_g[0], pos_g[1], pos_g[2],
vel_g[0], vel_g[1], vel_g[2],
psid]]).T
xref_traj_series[:, iter] = np.ndarray.flatten(xref)
tf_br.sendTransform((xref[0][0], xref[1][0], xref[2][0]),
ori_g,
rospy.Time.now(),
"xref_pose",
"world")
# feedforward acceleration
ff = np.array([[
drone_traj.val(t=t, order=2, dim=0),
drone_traj.val(t=t, order=2, dim=1),
drone_traj.val(t=t, order=2, dim=2),
0]]).T
try:
(trans, rot) = tf_listener.lookupTransform('world', 'uav/imu', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
# calculate linear velocities, define new state
lin_vel = (np.array(trans) - x[:3,0]) / dt
x[:3,0] = trans # new position
x[3:6,0] = lin_vel # new linear velocity
[psi, theta, phi] = Rotation.from_quat(rot).as_euler("ZYX")
x[6] = psi
phi_traj.append(phi)
theta_traj.append(theta)
psi_traj.append(psi)
x_traj_series[:, iter] = np.ndarray.flatten(x)
u = -K*(x-xref) + Gff + ff
# print("%.3f, %.3f, %.3f" % (ff[0][0], ff[1][0], ff[2][0]))
up = np.array(
np.ndarray.flatten(u).tolist()[0])
[thrustd, phid, thetad, psid] = inverse_dyn(rot, xref, up, m)
# [psid, thetad, phid] = Rotation.from_quat(ori_g).as_euler('ZYX')
phid_traj.append(phid)
thetad_traj.append(thetad)
psid_traj.append(psid)
# generate desired roll, pitch rates, minimize error btwn desired and current
dphi = pid_phi(phi - phid)
dtheta = pid_theta(theta - thetad)
dpsi = u[3]
# convert rotation quaternion to euler angles and rotation matrix
# rpy rates around around body axis
# print(thrustd)
new_ctrl = RateThrust()
new_ctrl.angular_rates.x = dphi
new_ctrl.angular_rates.y = dtheta
new_ctrl.angular_rates.z = dpsi
new_ctrl.thrust.z = thrustd
ctrl_pub.publish(new_ctrl)
# Plot results
time_axis.append(t)
iter += 1
rate.sleep()
end_sim_pub.publish(Empty())
np.savez(file="lqr_and_xref", lqr_xtraj=x_traj_series, xref_traj_series=xref_traj_series)
# fig, axs = plt.subplots(1, 3)
# fig.suptitle('Target(red) v.s actual(green) roll and pitch')
# axs[0].set_title('phi')
# axs[1].set_title('theta')
# axs[2].set_title('psi')
# axs[0].scatter(time_axis, phid_traj, c = 'r')
# axs[0].scatter(time_axis, phi_traj, c = 'g')
# axs[1].scatter(time_axis, thetad_traj, c = 'r')
# axs[1].scatter(time_axis, theta_traj, c = 'g')
# axs[2].scatter(time_axis, psid_traj, c = 'r')
# axs[2].scatter(time_axis, psi_traj, c = 'g')
# plt.show()
# plot x, y, z, vx, vy, vz
fig, axs = plt.subplots(2, 3)
axs[0, 0].set_title('X')
axs[0, 1].set_title('Y')
axs[0, 2].set_title('Z')
axs[1, 0].set_title('VX')
axs[1, 1].set_title('VY')
axs[1, 2].set_title('VZ')
for ax in axs.flat:
ax.set(xlabel='Time(s)')
fig.suptitle('Absolute State error v.s time (LQR)')
x_error = abs(xref_traj_series - x_traj_series)
for i in range(3):
# position
axs[0, i].scatter(time_axis, x_error[i,:iter], c = 'r')
# velocity
axs[1, i].scatter(time_axis, x_error[i+3,:iter], c = 'r')
axs[1, i].scatter(time_axis, x_error[i+3,:iter], c = 'g')
plt.show()
def pid_controller(self, state_msg, traj_msg):
# In general u = -K*x + (N_u + K*N_x)*r
# r = reference state
# x = state
# K = LQR gains
# N_u, N_x = refrence input and reference state matrices
# extract reference values
x_r, y_r, z_r = [
traj_msg.pose.position.x, traj_msg.pose.position.y,
traj_msg.pose.position.z
]
vx_r, vy_r, vz_r = [
traj_msg.twist.linear.x, traj_msg.twist.linear.y,
traj_msg.twist.linear.z
]
ori_quat_r = [
traj_msg.pose.orientation.x, traj_msg.pose.orientation.y,
traj_msg.pose.orientation.z, traj_msg.pose.orientation.w
]
psi_r, theta_r, phi_r = tf.transformations.euler_from_quaternion(
ori_quat_r, axes='rzyx')
p_r, q_r, r_r = [
traj_msg.twist.angular.x, traj_msg.twist.angular.y,
traj_msg.twist.angular.z
]
#print("Traj rot: \n{}".format(traj_msg.rot))
Rbw_ref = np.array(
[[traj_msg.rot[0], traj_msg.rot[1], traj_msg.rot[2]],
[traj_msg.rot[3], traj_msg.rot[4], traj_msg.rot[5]],
[traj_msg.rot[6], traj_msg.rot[7], traj_msg.rot[8]]])
#print("Rbw ref: \n{}".format(Rbw_ref))
# extract drone real state values
x, y, z = [
state_msg.pose.position.x, state_msg.pose.position.y,
state_msg.pose.position.z
]
vx, vy, vz = [
state_msg.twist.linear.x, state_msg.twist.linear.y,
state_msg.twist.linear.z
]
ori_quat = [
state_msg.pose.orientation.x, state_msg.pose.orientation.y,
state_msg.pose.orientation.z, state_msg.pose.orientation.w
]
psi, theta, phi = tf.transformations.euler_from_quaternion(ori_quat,
axes='rzyx')
Rwb = tf.transformations.quaternion_matrix(ori_quat)
Rbw = Rwb[0:3, 0:3].T # get body to world frame rotation
p, q, r = [
state_msg.twist.angular.x, state_msg.twist.angular.y,
state_msg.twist.angular.z
]
# ******************************************#
# POSITION CONTROL LOOP
# ******************************************#
if (self.loops == 0):
# compute desired input ua = ua_ref + ua_e
# ue_ref : from dif flatness
# ua_e = -Kp*(p - p_ref) - Kd * (v - v_ref) using PID controller
pos = np.array([[x], [y], [z]])
pos_ref = np.array([[x_r], [y_r], [z_r]])
v = np.array([[vx], [vy], [vz]])
v_ref = np.array([[vx_r], [vy_r], [vz_r]])
Kp = np.diag([lqrg.Kpx2, lqrg.Kpy2, lqrg.Kpz2])
Kd = np.diag([lqrg.Kdx2, lqrg.Kdy2, lqrg.Kdz2])
Ki = np.diag([lqrg.Kix2, lqrg.Kiy2, lqrg.Kiz2])
self.pos_err = self.pos_err + (pos - pos_ref)
ua_e = -1.0 * np.dot(Kp, pos - pos_ref) - 1.0 * np.dot(
Kd, v - v_ref) - 1.0 * np.dot(Ki,
self.pos_err) # PID control law
#print("pos error mag: {}".format(np.linalg.norm(pos - pos_ref)))
#print("vel error mag: {}".format(np.linalg.norm(v-v_ref)))
ua_ref = np.array([[traj_msg.ua.x], [traj_msg.ua.y],
[traj_msg.ua.z]])
ua = ua_e + ua_ref # desired acceleration
#print("ua mag {}".format(np.linalg.norm(ua)))
ua = self.clip_vector(ua, self.max_thrust) # saturate input
# compute Thrust -T- as
# T = m*wzb.T*(ua + g*zw)
#
e3 = np.array([[0.0], [0.0],
[1.0]]) # z axis of body expressed in body frame
zw = np.array(
[[0.0], [0.0],
[1.0]]) # z axis of world frame expressed in world frame
#Rwb = tf.transformations.euler_matrix(psi, theta, phi, axes='rzyx') # world to body frame rotation
#print("Rbw real: \n{}".format(Rbw))
wzb = np.dot(Rbw, e3) # zb expressed in world frame
self.T = self.m * np.dot(wzb.T, (ua + g * zw))[0][0]
#print("T : {}".format(self.T))
# Following Fassler, Fontana, Theory and Math Behing RPG Quadrotor Control
# Rbw_des = [xb_des yb_des zb_des] with zb_des = ua + g*zw / ||ua + g*zw||
# represents the desired orientation (and not the one coming from dif flatness)
zb_des = (ua + g * zw) / np.linalg.norm(ua + g * zw)
#T = self.m*np.dot(zb_des.T,(-ua + g*zw))[0][0]
#print("PSI value: {}".format(psi_r))
yc_des = np.array(df_flat.get_yc(
psi_r)) #transform to np.array 'cause comes as np.matrix
xb_des = np.cross(yc_des, zb_des, axis=0)
xb_des = xb_des / np.linalg.norm(xb_des)
yb_des = np.cross(zb_des, xb_des, axis=0)
self.Rbw_des = np.concatenate((xb_des, yb_des, zb_des), axis=1)
# Orientation error matrix is
# Rbw.T*Rbw_des because iff Rbw = Rbw_des, then Rbw.T*Rbw_des = I_3
# Define angular velocity error
w_b = np.array([[p], [q], [r]])
w_b_r = np.array([[p_r], [q_r], [r_r]])
#print("w_b ref: \n{}".format(w_b_r))
self.w_b_des = np.dot(Rbw.T, np.dot(Rbw_ref, w_b_r))
#print("w_b des: \n{}".format(w_b_des))
w_b_e = w_b - np.dot(Rbw.T, np.dot(self.Rbw_des, self.w_b_des))
# compute input with PID control law
Kp = np.diag([lqrg.Kp1, lqrg.Kp1, lqrg.Kp1])
Kd = np.diag([lqrg.Kd1, lqrg.Kd1, lqrg.Kd1])
#w_b_in = -np.dot(Kp,or_e) - np.dot(Kd,w_b_e)
self.loops = self.loops + 1
else:
self.loops = self.loops + 1
if (self.loops == self.pos_loop_freq):
self.loops = 0
#print("**Loops: {}".format(self.loops))
#print("Position Error: {}".format(np.linalg.norm(self.pos_err)))
#print("T computed: {}".format(self.T))
# ******************************************#
# ORIENTATION CONTROL LOOP
# ******************************************#
# ********************************** #
# USING EULER ANGLES #
# ********************************** #
or_des = np.array(
df_flat.RotToRPY_ZYX(self.Rbw_des)
) # ... need to convert to np.array to use this function..
#print("Orientation des: {}".format(or_des))
phi_des = or_des[0][0]
theta_des = or_des[1][0]
psi_des = or_des[2][0]
# phi (x axis) angular position dynamics control law
uc_e_x = -1.0 * self.Kr * phi + (self.N_ur +
np.dot(self.Kr, self.N_xr)) * phi_des
# theta (y axis) angular position dynamics control law
uc_e_y = -1.0 * self.Kr * theta + (
self.N_ur + np.dot(self.Kr, self.N_xr)) * theta_des
# psi (z axis) angular position dynamics control law
uc_e_z = -1.0 * self.Kr * psi + (self.N_ur +
np.dot(self.Kr, self.N_xr)) * psi_des
#compose angular position dynamics input vector
#uc_e = np.array([[uc_e_x[0]],[uc_e_y[0]],[uc_e_z[0]]])
# the final input to the drone is u = u_e + u_r
# input = error_input + reference_input
ucx, ucy, ucz = [
traj_msg.uc.x + uc_e_x.item(0), traj_msg.uc.y + uc_e_y.item(0),
traj_msg.uc.z + uc_e_z.item(0)
]
uc = np.array([[ucx], [ucy], [ucz]])
#print(ua,uc)
# compute w_b angular velocity commands as
# w_b = K.inv * uc
# where (euler angle derivate) = K*w_b
K = np.array(
[[1.0,
np.sin(phi) * np.tan(theta),
np.cos(phi) * np.tan(theta)],
[0.0, np.cos(phi), -1.0 * np.sin(phi)],
[0.0,
np.sin(phi) / np.cos(theta),
np.cos(phi) / np.cos(theta)]])
Kinv = np.linalg.inv(K)
w_b_in = np.dot(Kinv, uc)
# create and fill message
rt_msg = RateThrust()
rt_msg.header.stamp = rospy.Time.now()
rt_msg.header.frame_id = 'uav/imu'
rt_msg.angular_rates.x = 1.0 * self.w_b_des[0][0] + 1.0 * w_b_in[0][
0] #- p_r #traj_msg.twist.angular.x
rt_msg.angular_rates.y = 1.0 * self.w_b_des[1][0] + 1.0 * w_b_in[1][
0] #- q_r #traj_msg.twist.angular.y
rt_msg.angular_rates.z = 1.0 * self.w_b_des[2][0] + 1.0 * w_b_in[2][
0] #- r_r #traj_msg.twist.angular.z
rt_msg.thrust.x = 0.0
rt_msg.thrust.y = 0.0
rt_msg.thrust.z = self.T # self.m*np.linalg.norm(t)
# publish
self.input_publisher.publish(rt_msg)
rospy.loginfo(rt_msg)
def callback(self, image, ir_marker):
if self.current_state == 'rotating_to_gate':
self.target_quaternion = reorient_to_centroid(
self.init_vector, self.current_coords,
self.current_orientation, self.gate_list[self.target_gate + 1])
next_gate_rotmat = q2q_to_rot(self.current_orientation,
self.target_quaternion)
roll_pitch_yaw = rotationMatrixToEulerAngles(next_gate_rotmat)
print(roll_pitch_yaw)
#Controls time to rotate
time_to_rotate = 2
d = rospy.Duration(time_to_rotate, 0)
now = rospy.get_rostime()
end_time = d + now
msg = RateThrust()
msg.header.frame_id = "uav/imu"
msg.header.stamp = Time.now()
msg.thrust.z = self.idle_thrust
msg.angular_rates.x = roll_pitch_yaw[0] / time_to_rotate
msg.angular_rates.y = roll_pitch_yaw[1] / time_to_rotate
msg.angular_rates.z = roll_pitch_yaw[2] / time_to_rotate
while rospy.get_rostime() < end_time:
print("ITS AT STATE 1")
self.pub_vel.publish(msg)
self.current_state = states[1]
elif self.current_state == 'rotating_to_IR_centroid':
# minimum_distance = 99999
# closest_gate=''
#
# for key in range(len(gate_dictionary)):
# x,y = IR_marker_centroid_calculator(key)
# dist = abs(math.hypot(x - image_center_x, y - image_center_y))
# if dist < minimum_distance:
# dist = minimum_distance
# closest_gate = key
gate_dictionary = IR_marker_cluster_seperator(ir_marker)
target_gate_list = gate_dictionary[gates[self.target_gate]]
#print(gate_dictionary, "REEEEEEEEEEEEEEEEE", target_gate_list)
x, y = IR_marker_centroid_calculator(target_gate_list)
delta_x = (-1) * (image_center_x - x)
delta_y = (-1) * (image_center_y - y)
msg = RateThrust()
msg.header.frame_id = "uav/imu"
msg.header.stamp = Time.now()
msg.thrust.z = self.idle_thrust + 1
msg.angular_rates.x = 0
msg.angular_rates.y = delta_y / 15
msg.angular_rates.z = delta_x / 15
print("ITS AT STATE 2")
self.pub_vel.publish(msg)
if ((abs(delta_x) < 100) and (abs(delta_y)) < 100):
self.current_state = states[2]
elif self.current_state == 'flying':
gate_dictionary = IR_marker_cluster_seperator(ir_marker)
target_gate_list = gate_dictionary[gates[self.target_gate]]
if len(target_gate_list) is 0:
successful_gate_callback(self)
msg = RateThrust()
msg.header.frame_id = "uav/imu"
msg.header.stamp = Time.now()
msg.thrust.z = self.idle_thrust
msg.angular_rates.x = 0
msg.angular_rates.y = 0
msg.angular_rates.z = 0
#print("ITS AT STATE 3")
self.pub_vel.publish(msg)
else:
x, y = IR_marker_centroid_calculator(target_gate_list)
delta_x = (-1) * (image_center_x - x)
delta_y = (-1) * (image_center_y - y)
msg = RateThrust()
msg.header.frame_id = "uav/imu"
msg.header.stamp = Time.now()
msg.thrust.z = self.idle_thrust + 0.1
msg.angular_rates.x = 100
msg.angular_rates.y = delta_x / 30
msg.angular_rates.z = delta_y / 30
self.pub_vel.publish(msg)
#print("ITS AT STATE 3")
elif self.current_state == 'hover':
msg = RateThrust()
msg.header.frame_id = "uav/imu"
msg.header.stamp = Time.now()
msg.thrust.z = self.idle_thrust + 1
msg.angular_rates.x = 0
msg.angular_rates.y = 0
msg.angular_rates.z = 0
def gate_cb(self, state):
self.counter += 1
print self.counter
markers = state.markers
for marker in markers:
if marker.landmarkID.data == "Gate10":
if marker.markerID.data == "1":
marker1 = [marker.x, marker.y]
if marker.markerID.data == "2":
marker2 = [marker.x, marker.y]
if marker.markerID.data == "3":
marker3 = [marker.x, marker.y]
if marker.markerID.data == "4":
marker4 = [marker.x, marker.y]
if marker.landmarkID.data == "Gate21":
if marker.markerID.data == "2":
marker221 = [marker.x, marker.y]
if marker.markerID.data == "3":
marker321 = [marker.x, marker.y]
if marker.markerID.data == "4":
marker421 = [marker.x, marker.y]
try:
marker1, marker2, marker3, marker4
markersArr = np.array([marker1, marker2, marker3, marker4])
except:
print 'gate 10 not in view'
if self.counter > 250:
try:
marker221, marker321, marker421
y_diff = marker321[1] - marker221[1]
marker121 = [marker421[0], marker421[0] - y_diff]
# print 'markers'
# print marker121,marker221,marker321,marker421
markersArr = np.array(
[marker221, marker121, marker321, marker421])
except:
print 'gate 21 not in view'
if self.imu_cb_flag:
imu_state = [
self.ang_x, self.ang_y, self.ang_z, self.lin_x, self.lin_y,
self.lin_z
]
nn_input = np.concatenate(
(markersArr.flatten(), np.array(imu_state)))
# print nn_input
outputs = self.model.predict(nn_input.reshape(-1, 14))
roll = outputs[0][0]
pitch = outputs[0][1]
yaw = 0
thrust = outputs[0][2]
# print outputs
if abs(roll) == 0.25:
roll = 0
if self.counter > 320:
thrust = 12.4
roll = 0
ang_scale = 1
thrust_scale = 1
target_attitude_thrust = RateThrust()
target_attitude_thrust.header.frame_id = "home"
target_attitude_thrust.header.stamp = rospy.Time.now()
target_attitude_thrust.angular_rates.x = roll
target_attitude_thrust.angular_rates.y = pitch
target_attitude_thrust.angular_rates.z = yaw
target_attitude_thrust.thrust.z = thrust
self.attitude_thrust_pub.publish(target_attitude_thrust)
def main():
# rosparams
aggr = 10**4
T = 40.0 # T=None to use aggr instead.
# init rosnodes
rospy.init_node('lqr_controller')
tf_listener = tf.TransformListener()
imu_sub = rospy.Subscriber('/uav/sensors/imu',
Imu,
callback=imu_data,
queue_size=1)
start_sim_pub = rospy.Publisher('/uav/input/arm', Empty, queue_size=1)
end_sim_pub = rospy.Publisher('/uav/input/reset', Empty, queue_size=1)
ctrl_pub = rospy.Publisher('/uav/input/rateThrust',
RateThrust,
queue_size=10)
path_pub = rospy.Publisher('ref_traj', Path, queue_size=1)
prediction_pub = rospy.Publisher('mpc_pred', Path, queue_size=10)
reference_pub = rospy.Publisher('mpc_ref', Path, queue_size=10)
tf_br = tf.TransformBroadcaster()
# Global Constants
Ixx = rospy.get_param("/uav/flightgoggles_uav_dynamics/vehicle_inertia_xx")
Iyy = rospy.get_param("/uav/flightgoggles_uav_dynamics/vehicle_inertia_yy")
Izz = rospy.get_param("/uav/flightgoggles_uav_dynamics/vehicle_inertia_zz")
m = rospy.get_param("/uav/flightgoggles_uav_dynamics/vehicle_mass")
g = 9.81
# Flat Dynamics
FLAT_STATES = 7
FLAT_CTRLS = 4
A = np.zeros((FLAT_STATES, FLAT_STATES))
A[0:3, 3:6] = np.eye(3)
B = np.zeros((FLAT_STATES, FLAT_CTRLS))
B[3:, :] = np.eye(4)
Gff = np.array([[0, 0, g, 0]]).T # gravity compensation
Q = np.diag([20, 20, 20, 0.1, 0.1, 0.1, 1])
R = np.diag([.1, .1, .1, 10])
S = Q * 10
# Trajectory generation
# get gate poses
num_gates = 14
gate_ids = list(range(0, num_gates))
gate_transforms = get_gate_positions(gate_ids)
# inital drone pose and generated spline of waypoints
print("Solving for optimal trajectory...")
dt = 0.06
rate = rospy.Rate(int(1. / dt))
x0 = np.array([[0., 0., 1., 0., 0., 0., 0.]]).T
drone_traj = DroneTrajectory()
drone_traj.set_start(position=x0[:3], velocity=x0[3:6])
drone_traj.set_end(position=x0[:3], velocity=x0[3:6])
for (trans, rot) in gate_transforms.values():
drone_traj.add_gate(trans, rot)
drone_traj.solve(aggr, T=T)
# PID Controller for setting angular rates
pid_phi = PID(Kp=7, Ki=0, Kd=1, setpoint=0)
pid_theta = PID(Kp=7, Ki=0, Kd=1, setpoint=0)
# Optimal control law for flat system
print("Solving linear feedback system...")
# K, S, E = control.lqr(A, B, Q, R)
horizon = 10
mpc = DroneMPC(A, B, Q, R, S, N=horizon, dt=dt)
# Generate trajectory (starttime-sensitive)
print("Generating trajectory for visualizing...")
start_time = rospy.get_time()
xref_traj = drone_traj.as_path(dt=dt,
frame='world',
start_time=rospy.Time.now())
# plotting
N = len(xref_traj.poses) + int(3.0 / dt)
time_axis = []
xref_traj_series = np.zeros((FLAT_STATES, N))
x_traj_series = np.zeros((FLAT_STATES, N))
# run simulation
print("Running simulation and executing controls...")
iter = 0
x = x0
prev_pose = None
phid_traj = []
thetad_traj = []
psid_traj = []
phi_traj = []
theta_traj = []
psi_traj = []
# for pose in xref_traj.poses:
while not rospy.is_shutdown() and iter < N:
# publish arm command and ref traj
start_sim_pub.publish(Empty())
path_pub.publish(xref_traj)
try:
(trans, rot) = tf_listener.lookupTransform('world', 'uav/imu',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# calculate linear velocities, define new state
lin_vel = (np.array(trans) - x[:3, 0]) / dt
x[:3, 0] = trans # new position
x[3:6, 0] = lin_vel # new linear velocity
[psi, theta, phi] = Rotation.from_quat(rot).as_euler("ZYX")
x[6] = psi
phi_traj.append(phi)
theta_traj.append(theta)
psi_traj.append(psi)
x_traj_series[:, iter] = np.ndarray.flatten(x)
# get next target waypoint
t = rospy.get_time() - start_time
poses = [
drone_traj.val(t + offset, dim=None, order=0)
for offset in np.arange(0, (horizon + 1) * dt, dt)
]
vels = [
drone_traj.val(t + offset, dim=None, order=1)
for offset in np.arange(0, (horizon + 1) * dt, dt)
]
psis = [atan2(v[1], v[0]) for v in vels]
if psis[0] - psi < -np.pi:
psis[0] += 2 * np.pi
if psis[0] - psi > np.pi:
psis[0] -= 2 * np.pi
for i in range(len(psis[0:-2])):
if psis[i + 1] - psis[i] < -np.pi:
psis[i + 1] += 2 * np.pi
if psis[i + 1] - psis[i] > np.pi:
psis[i + 1] -= 2 * np.pi
ref_traj = [[p[0], p[1], p[2], v[0], v[1], v[2], psi]
for p, v, psi in zip(poses, vels, psis)]
pos_g, vel_g, ori_g = drone_traj.full_pose(t)
xref = np.array([[
pos_g[0], pos_g[1], pos_g[2], vel_g[0], vel_g[1], vel_g[2], psis[0]
]]).T
xref_traj_series[:, iter] = np.ndarray.flatten(xref)
tf_br.sendTransform((xref[0][0], xref[1][0], xref[2][0]), ori_g,
rospy.Time.now(), "xref_pose", "world")
# feedforward acceleration
# ff = np.array([[
# drone_traj.val(t=t, order=2, dim=0),
# drone_traj.val(t=t, order=2, dim=1),
# drone_traj.val(t=t, order=2, dim=2),
# 0]]).T
u_mpc, x_mpc = mpc.solve(x, np.array(ref_traj).transpose())
u = u_mpc[:, 0].flatten() + Gff.flatten()
reference_pub.publish(
mpc.to_path(np.array(ref_traj).transpose(),
start_time=rospy.Time.now(),
frame='world'))
prediction_pub.publish(
mpc.to_path(x_mpc, start_time=rospy.Time.now(), frame='world'))
# print(xref)
# print("fb: {}, ff: {}".format(-K * (x - xref), ff))
# print("%.3f, %.3f, %.3f" % (ff[0][0], ff[1][0], ff[2][0]))
[thrustd, phid, thetad, psid] = inverse_dyn(rot, x.flatten(), u, m)
# [psid, thetad, phid] = Rotation.from_quat(ori_g).as_euler('ZYX')
phid_traj.append(phid)
thetad_traj.append(thetad)
psid_traj.append(psid)
# generate desired roll, pitch rates, minimize error btwn desired and current
dphi = pid_phi(phi - phid)
dtheta = pid_theta(theta - thetad)
dpsi = u[3]
# convert rotation quaternion to euler angles and rotation matrix
# rpy rates around around body axis
# print(thrustd)
new_ctrl = RateThrust()
new_ctrl.angular_rates.x = dphi
new_ctrl.angular_rates.y = dtheta
new_ctrl.angular_rates.z = dpsi
new_ctrl.thrust.z = thrustd
ctrl_pub.publish(new_ctrl)
# Plot results
time_axis.append(t)
iter += 1
rate.sleep()
end_sim_pub.publish(Empty())
# fig, axs = plt.subplots(1, 3)
# fig.suptitle('Target(red) v.s actual(green) roll and pitch')
# axs[0].set_title('phi')
# axs[1].set_title('theta')
# axs[2].set_title('psi')
# axs[0].scatter(time_axis, phid_traj, c='r')
# axs[0].scatter(time_axis, phi_traj, c='g')
# axs[1].scatter(time_axis, thetad_traj, c='r')
# axs[1].scatter(time_axis, theta_traj, c='g')
# axs[2].scatter(time_axis, psid_traj, c='r')
# axs[2].scatter(time_axis, psi_traj, c='g')
# plt.show()
# plot x, y, z, vx, vy, vz
fig, axs = plt.subplots(2, 3)
axs[0, 0].set_title('X')
axs[0, 1].set_title('Y')
axs[0, 2].set_title('Z')
axs[1, 0].set_title('VX')
axs[1, 1].set_title('VY')
axs[1, 2].set_title('VZ')
for ax in axs.flat:
ax.set(xlabel='Time(s)')
fig.suptitle('Absolute State error v.s time (MPC)')
x_error = abs(xref_traj_series - x_traj_series)
for i in range(3):
# position
axs[0, i].scatter(time_axis, x_error[i, :iter], c='r')
# axs[0, i].scatter(time_axis, x_traj_series[i,:iter], c = 'g')
# velocity
# axs[1, i].scatter(time_axis, xref_traj_series[i+3,:iter], c = 'r')
axs[1, i].scatter(time_axis, x_error[i + 3, :iter], c='g')
plt.show()
#! /usr/bin/python
import rospy
import math
from mav_msgs.msg import RateThrust
from std_msgs.msg import Bool
if __name__ == '__main__':
rospy.init_node('msp_fc_test_node')
pub = rospy.Publisher('/uav/control/rate_thrust', RateThrust, queue_size=1)
pub_arm = rospy.Publisher('/uav/control/arm', Bool, queue_size=1)
rate = rospy.Rate(10)
i = 0
while not rospy.is_shutdown():
msg = RateThrust()
msg.angular_rates.x = 0
msg.angular_rates.y = 0
msg.angular_rates.z = 0
msg.thrust.z = 0 # math.sin(i) / 2 + 0.5
pub.publish(msg)
arm_msg = Bool()
arm_msg.data = True
pub_arm.publish(arm_msg)
i += 0.1
rate.sleep()
def callback(self, state_msg, traj_msg):
# In general u = -K*x + (N_u + K*N_x)*r
# r = reference state
# x = state
# K = LQR gains
# N_u, N_x = refrence input and reference state matrices
# extract reference values
x_r, y_r, z_r = [
traj_msg.pose.position.x, traj_msg.pose.position.y,
traj_msg.pose.position.z
]
vx_r, vy_r, vz_r = [
traj_msg.twist.linear.x, traj_msg.twist.linear.y,
traj_msg.twist.linear.z
]
ori_quat_r = [
traj_msg.pose.orientation.x, traj_msg.pose.orientation.y,
traj_msg.pose.orientation.z, traj_msg.pose.orientation.w
]
psi_r, theta_r, phi_r = tf.transformations.euler_from_quaternion(
ori_quat_r, axes='rzyx')
p_r, q_r, r_r = [
traj_msg.twist.angular.x, traj_msg.twist.angular.y,
traj_msg.twist.angular.z
]
# extract drone real state values
x, y, z = [
state_msg.pose.position.x, state_msg.pose.position.y,
state_msg.pose.position.z
]
vx, vy, vz = [
state_msg.twist.linear.x, state_msg.twist.linear.y,
state_msg.twist.linear.z
]
ori_quat = [
state_msg.pose.orientation.x, state_msg.pose.orientation.y,
state_msg.pose.orientation.z, state_msg.pose.orientation.w
]
psi, theta, phi = tf.transformations.euler_from_quaternion(ori_quat,
axes='rzyx')
p, q, r = [
state_msg.twist.angular.x, state_msg.twist.angular.y,
state_msg.twist.angular.z
]
#compute state error: error = reference - real
x_e, y_e, z_e = [x - x_r, y - y_r, z - z_r]
vx_e, vy_e, vz_e = [vx - vx_r, vy - vy_r, vz - vz_r]
phi_e, theta_e, psi_e = [phi - phi_r, theta - theta_r, psi - psi_r]
p_e, q_e, r_e = [p - p_r, q - q_r, r - r_r]
# compute input error u = -Kx
# first, for translation dynamics
ua_e_x = -1.0 * np.dot(self.Kt, np.array([[x_e], [vx_e]]))
ua_e_y = -1.0 * np.dot(self.Kt, np.array([[y_e], [vy_e]]))
ua_e_z = -1.0 * np.dot(self.Kt, np.array([[z_e], [vz_e]]))
# second, for orientation dynamics
uc_e_x = -1.0 * np.dot(self.Kr, np.array([phi_e]))
uc_e_y = -1.0 * np.dot(self.Kr, np.array([theta_e]))
uc_e_z = -1.0 * np.dot(self.Kr, np.array([psi_e]))
#print("uc_e_x: {} type: {}".format(uc_e_x,type(uc_e_x)) )
#print("ua_e_x: {} type: {}".format(ua_e_x,type(ua_e_x)) )
# the final input to the drone is u = u_e + u_r
# input = error_input + reference_input
#print("traj_msg.ua.x: {} type: {}".format(traj_msg.ua.x,type(traj_msg.ua.x)) )
uax, uay, uaz = [
traj_msg.ua.x + ua_e_x.item(0), traj_msg.ua.y + ua_e_y.item(0),
traj_msg.ua.z + ua_e_z.item(0)
]
ucx, ucy, ucz = [
traj_msg.uc.x + uc_e_x.item(0), traj_msg.uc.y + uc_e_y.item(0),
traj_msg.uc.z + uc_e_z.item(0)
]
#print("uax: {} type: {}".format(uax,type(uax)) )
ua = np.array([[uax], [uay], [uaz]])
uc = np.array([[ucx], [ucy], [ucz]])
#print(ua,uc)
# compute Thrust -T- as
# T = m*wzb.T*(ua + g*zw)
#
zb = np.array([[0.0], [0.0],
[1.0]]) # z axis of body expressed in body frame
zw = np.array([[0.0], [0.0], [1.0]
]) # z axis of world frame expressed in world frame
Rwb = tf.transformations.euler_matrix(
psi_r, theta_r, phi_r, axes='rzyx') # world to body frame rotation
Rbw = Rwb[0:3, 0:3].T # body to world frame rotation only
wzb = np.dot(Rbw, zb) # zb expressed in world frame
T = self.m * np.dot(wzb.T, (ua + g * zw))[0]
#print(wzb.T,type(wzb))
# compute w_b angular velocity commands as
# w_b = K.inv * uc
# where (euler angle derivate) = K*w_b
K = np.array([[
1.0,
np.sin(phi_r) * np.tan(theta_r),
np.cos(phi_r) * np.tan(theta_r)
], [0.0, np.cos(phi_r), -1.0 * np.sin(phi_r)],
[
0.0,
np.sin(phi_r) / np.cos(theta_r),
np.cos(phi_r) / np.cos(theta_r)
]])
Kinv = np.linalg.inv(K)
w_b = np.dot(Kinv, uc).flatten()
# create and fill message
rt_msg = RateThrust()
rt_msg.header.stamp = rospy.Time.now()
rt_msg.header.frame_id = 'uav/imu'
rt_msg.angular_rates.x = w_b[0] #traj_msg.twist.angular.x
rt_msg.angular_rates.y = w_b[1] #traj_msg.twist.angular.y
rt_msg.angular_rates.z = w_b[2] #traj_msg.twist.angular.z
rt_msg.thrust.x = 0.0
rt_msg.thrust.y = 0.0
rt_msg.thrust.z = T[0] # self.m*np.linalg.norm(t)
# publish
self.input_publisher.publish(rt_msg)
rospy.loginfo(rt_msg)
def clean(control):
cleaned=RateThrust()
cleaned.head=control.head
cleaned.angular_rates=control.bodyrates
cleaned.thrust=control.collective_thrust
return cleaned
def linear_velocity_control(self, state_msg, traj_msg):
# In general u = -K*x + (N_u + K*N_x)*r
# r = reference state
# x = state
# K = LQR gains
# N_u, N_x = refrence input and reference state matrices
# extract reference values
x_r, y_r, z_r = [
traj_msg.pose.position.x, traj_msg.pose.position.y,
traj_msg.pose.position.z
]
vx_r, vy_r, vz_r = [
traj_msg.twist.linear.x, traj_msg.twist.linear.y,
traj_msg.twist.linear.z
]
ori_quat_r = [
traj_msg.pose.orientation.x, traj_msg.pose.orientation.y,
traj_msg.pose.orientation.z, traj_msg.pose.orientation.w
]
psi_r, theta_r, phi_r = tf.transformations.euler_from_quaternion(
ori_quat_r, axes='rzyx')
p_r, q_r, r_r = [
traj_msg.twist.angular.x, traj_msg.twist.angular.y,
traj_msg.twist.angular.z
]
# extract drone real state values
x, y, z = [
state_msg.pose.position.x, state_msg.pose.position.y,
state_msg.pose.position.z
]
vx, vy, vz = [
state_msg.twist.linear.x, state_msg.twist.linear.y,
state_msg.twist.linear.z
]
ori_quat = [
state_msg.pose.orientation.x, state_msg.pose.orientation.y,
state_msg.pose.orientation.z, state_msg.pose.orientation.w
]
psi, theta, phi = tf.transformations.euler_from_quaternion(ori_quat,
axes='rzyx')
p, q, r = [
state_msg.twist.angular.x, state_msg.twist.angular.y,
state_msg.twist.angular.z
]
# vx dynamics control law
ua_e_x = -1.0 * self.Kr * vx + (self.N_ur +
np.dot(self.Kr, self.N_xr)) * vx_r
# vy dynamics control law
ua_e_y = -1.0 * self.Kr * vy + (self.N_ur +
np.dot(self.Kr, self.N_xr)) * vy_r
# vz dynamics control law
ua_e_z = -1.0 * self.Kr * vz + (self.N_ur +
np.dot(self.Kr, self.N_xr)) * vz_r
# compose position dynamics input vector
#ua_e = np.array([[ua_e_x[0]],[ua_e_y[0]],[ua_e_z[0]]])
# phi (x axis) angular position dynamics control law
uc_e_x = -1.0 * self.Kr * phi + (self.N_ur +
np.dot(self.Kr, self.N_xr)) * phi_r
# theta (y axis) angular position dynamics control law
uc_e_y = -1.0 * self.Kr * theta + (
self.N_ur + np.dot(self.Kr, self.N_xr)) * theta_r
# psi (z axis) angular position dynamics control law
uc_e_z = -1.0 * self.Kr * psi + (self.N_ur +
np.dot(self.Kr, self.N_xr)) * psi_r
#compose angular position dynamics input vector
#uc_e = np.array([[uc_e_x[0]],[uc_e_y[0]],[uc_e_z[0]]])
# the final input to the drone is u = u_e + u_r
# input = error_input + reference_input
uax, uay, uaz = [
traj_msg.ua.x + ua_e_x.item(0), traj_msg.ua.y + ua_e_y.item(0),
traj_msg.ua.z + ua_e_z.item(0)
]
ucx, ucy, ucz = [
traj_msg.uc.x + uc_e_x.item(0), traj_msg.uc.y + uc_e_y.item(0),
traj_msg.uc.z + uc_e_z.item(0)
]
ua = np.array([[uax], [uay], [uaz]])
uc = np.array([[ucx], [ucy], [ucz]])
#print(ua,uc)
# compute Thrust -T- as
# T = m*wzb.T*(ua + g*zw)
#
zb = np.array([[0.0], [0.0],
[1.0]]) # z axis of body expressed in body frame
zw = np.array([[0.0], [0.0], [1.0]
]) # z axis of world frame expressed in world frame
Rwb = tf.transformations.euler_matrix(
psi, theta, phi, axes='rzyx') # world to body frame rotation
Rbw = Rwb[0:3, 0:3].T # body to world frame rotation only
wzb = np.dot(Rbw, zb) # zb expressed in world frame
T = self.m * np.dot(wzb.T, (ua + g * zw))[0]
#print(wzb.T,type(wzb))
# compute w_b angular velocity commands as
# w_b = K.inv * uc
# where (euler angle derivate) = K*w_b
K = np.array(
[[1.0,
np.sin(phi) * np.tan(theta),
np.cos(phi) * np.tan(theta)],
[0.0, np.cos(phi), -1.0 * np.sin(phi)],
[0.0,
np.sin(phi) / np.cos(theta),
np.cos(phi) / np.cos(theta)]])
Kinv = np.linalg.inv(K)
w_b = np.dot(Kinv, uc).flatten()
# create and fill message
rt_msg = RateThrust()
rt_msg.header.stamp = rospy.Time.now()
rt_msg.header.frame_id = 'uav/imu'
rt_msg.angular_rates.x = w_b[0] #traj_msg.twist.angular.x
rt_msg.angular_rates.y = w_b[1] #traj_msg.twist.angular.y
rt_msg.angular_rates.z = w_b[2] #traj_msg.twist.angular.z
rt_msg.thrust.x = 0.0
rt_msg.thrust.y = 0.0
rt_msg.thrust.z = T[0] # self.m*np.linalg.norm(t)
# publish
self.input_publisher.publish(rt_msg)
rospy.loginfo(rt_msg)
def geometric_controller(self, state_msg, traj_msg):
# In general u = -K*x + (N_u + K*N_x)*r
# r = reference state
# x = state
# K = LQR gains
# N_u, N_x = refrence input and reference state matrices
# extract reference values
x_r, y_r, z_r = [
traj_msg.pose.position.x, traj_msg.pose.position.y,
traj_msg.pose.position.z
]
vx_r, vy_r, vz_r = [
traj_msg.twist.linear.x, traj_msg.twist.linear.y,
traj_msg.twist.linear.z
]
ori_quat_r = [
traj_msg.pose.orientation.x, traj_msg.pose.orientation.y,
traj_msg.pose.orientation.z, traj_msg.pose.orientation.w
]
psi_r, theta_r, phi_r = tf.transformations.euler_from_quaternion(
ori_quat_r, axes='rzyx')
p_r, q_r, r_r = [
traj_msg.twist.angular.x, traj_msg.twist.angular.y,
traj_msg.twist.angular.z
]
w_b_r = np.array([[p_r], [q_r], [r_r]])
Rbw_ref = np.array(
[[traj_msg.rot[0], traj_msg.rot[1], traj_msg.rot[2]],
[traj_msg.rot[3], traj_msg.rot[4], traj_msg.rot[5]],
[traj_msg.rot[6], traj_msg.rot[7], traj_msg.rot[8]]])
#print("Rbw ref: \n{}".format(Rbw_ref))
# extract drone real state values
x, y, z = [
state_msg.pose.position.x, state_msg.pose.position.y,
state_msg.pose.position.z
]
vx, vy, vz = [
state_msg.twist.linear.x, state_msg.twist.linear.y,
state_msg.twist.linear.z
]
ori_quat = [
state_msg.pose.orientation.x, state_msg.pose.orientation.y,
state_msg.pose.orientation.z, state_msg.pose.orientation.w
]
psi, theta, phi = tf.transformations.euler_from_quaternion(ori_quat,
axes='rzyx')
Rwb = tf.transformations.quaternion_matrix(ori_quat)
Rbw = Rwb[0:3, 0:3].T # get body to world frame rotation
p, q, r = [
state_msg.twist.angular.x, state_msg.twist.angular.y,
state_msg.twist.angular.z
]
w_b = np.array([[p], [q], [r]])
# ******************************************#
# POSITION CONTROL LOOP
# ******************************************#
if (self.loops == 0):
# compute desired input ua = ua_ref + ua_e
# ue_ref : from dif flatness
# ua_e = -Kp*(p - p_ref) - Kd * (v - v_ref) using PID controller
pos = np.array([[x], [y], [z]])
pos_ref = np.array([[x_r], [y_r], [z_r]])
v = np.array([[vx], [vy], [vz]])
v_ref = np.array([[vx_r], [vy_r], [vz_r]])
Kp = np.diag([lqrg.Kpx2, lqrg.Kpy2, lqrg.Kpz2])
Kd = np.diag([lqrg.Kdx2, lqrg.Kdy2, lqrg.Kdz2])
Ki = np.diag([lqrg.Kix2, lqrg.Kiy2, lqrg.Kiz2])
self.pos_err = self.pos_err + (pos - pos_ref)
ua_e = -1.0 * np.dot(Kp, pos - pos_ref) - 1.0 * np.dot(
Kd, v - v_ref) - 1.0 * np.dot(Ki,
self.pos_err) # PID control law
#print("pos error mag: {}".format(np.linalg.norm(pos - pos_ref)))
#print("vel error mag: {}".format(np.linalg.norm(v-v_ref)))
ua_ref = np.array([[traj_msg.ua.x], [traj_msg.ua.y],
[traj_msg.ua.z]])
ua = ua_e + ua_ref # desired acceleration
#print("ua mag {}".format(np.linalg.norm(ua)))
ua = self.clip_vector(ua, self.max_thrust) # saturate input
# compute Thrust -T- as
# T = m*wzb.T*(ua + g*zw)
#
e3 = np.array([[0.0], [0.0],
[1.0]]) # z axis of body expressed in body frame
zw = np.array(
[[0.0], [0.0],
[1.0]]) # z axis of world frame expressed in world frame
#Rwb = tf.transformations.euler_matrix(psi, theta, phi, axes='rzyx') # world to body frame rotation
#print("Rbw real: \n{}".format(Rbw))
wzb = np.dot(Rbw, e3) # zb expressed in world frame
self.T = self.m * np.dot(wzb.T, (ua + g * zw))[0][0]
#print("T : {}".format(self.T))
# Following Fassler, Fontana, Theory and Math Behing RPG Quadrotor Control
# Rbw_des = [xb_des yb_des zb_des] with zb_des = ua + g*zw / ||ua + g*zw||
# represents the desired orientation (and not the one coming from dif flatness)
zb_des = (ua + g * zw) / np.linalg.norm(ua + g * zw)
#T = self.m*np.dot(zb_des.T,(-ua + g*zw))[0][0]
#print("PSI value: {}".format(psi_r))
yc_des = np.array(df_flat.get_yc(
psi_r)) #transform to np.array 'cause comes as np.matrix
xb_des = np.cross(yc_des, zb_des, axis=0)
xb_des = xb_des / np.linalg.norm(xb_des)
yb_des = np.cross(zb_des, xb_des, axis=0)
self.Rbw_des = np.concatenate((xb_des, yb_des, zb_des), axis=1)
# Orientation error matrix is
# Rbw.T*Rbw_des because iff Rbw = Rbw_des, then Rbw.T*Rbw_des = I_3
Kp = np.diag([lqrg.Kp1, lqrg.Kp1, lqrg.Kp1])
Kd = np.diag([lqrg.Kd1, lqrg.Kd1, lqrg.Kd1])
#w_b_in = -np.dot(Kp,or_e) - np.dot(Kd,w_b_e)
self.loops = self.loops + 1
else:
self.loops = self.loops + 1
if (self.loops == self.pos_loop_freq):
self.loops = 0
#print("Position Error: {}".format(np.linalg.norm(self.pos_err)))
#print("T computed: {}".format(self.T))
# ******************************************#
# ORIENTATION CONTROL LOOP
# ******************************************#
# ********************************** #
# USING ROTATION MATRIX #
# ********************************** #
# Calculate input using Propietary method
#self.w_b_in = self.get_wdes_1(Rbw, self.Rbw_des, self.w_b_des, self.Kw)
#self.w_b_in = self.get_wdes_2(Rbw, self.Rbw_des, self.Katt)
#w_b_e = w_b - np.dot(self.Rbw_des.T, np.dot(Rbw_ref, w_b_r))
#self.w_b_des = -1.0*np.dot(self.Kw, np.dot(Rbw.T, np.dot(Rbw_ref,w_b_r)))
#or_e = 0.5*self.vex( (np.dot(self.Rbw_des.T,Rbw) - np.dot(Rbw.T,self.Rbw_des)) )
#or_e = -1.0*np.dot(self.Ko, or_e)
# compute feedback angular velocity
self.Rbw_ref_dot = np.dot(Rbw_ref, self.hat(w_b_r))
w_fb = self.get_wdes_4(Rbw, self.Rbw_des, self.Rbw_ref_dot, w_b_r)
delta_t = rospy.Time.now() - self.t1
delta_t = delta_t.to_sec()
print("Delta t: {}".format(delta_t))
#w_fb = w_fb / 1.0
self.t1 = rospy.Time.now()
# create and fill message
rt_msg = RateThrust()
rt_msg.header.stamp = rospy.Time.now()
rt_msg.header.frame_id = 'uav/imu'
rt_msg.angular_rates.x = w_fb[0][
0] #+ or_e[0][0] #self.w_b_in[0][0] + w_b_r[0][0]
rt_msg.angular_rates.y = w_fb[1][
0] #+ or_e[1][0] #self.w_b_in[1][0] + w_b_r[1][0]
rt_msg.angular_rates.z = w_fb[2][
0] #+ or_e[2][0] #self.w_b_in[2][0] + w_b_r[2][0]
rt_msg.thrust.x = 0.0
rt_msg.thrust.y = 0.0
rt_msg.thrust.z = self.T # self.m*np.linalg.norm(t)
# publish
self.input_publisher.publish(rt_msg)
rospy.loginfo(rt_msg)
# target_i = 1 - target_i
# thetad = theta_targets[target_i]
# xref_i = 1 - xref_i
# xref = targets[xref_i]
# print("New target: (%d,%d,%d)" % (xref[0], xref[1], xref[2]))
# generate desired roll, pitch rates, minimize error btwn desired and current
dphi = pid_phi(phi - phid)
dtheta = pid_theta(theta - thetad)
# print(dtheta)
dpsi = u[3]
# convert rotation quaternion to euler angles and rotation matrix
# rpy rates around around body axis
new_ctrl = RateThrust()
new_ctrl.angular_rates.x = dphi
new_ctrl.angular_rates.y = dtheta
new_ctrl.angular_rates.z = dpsi
new_ctrl.thrust.z = thrustd
ctrl_pub.publish(new_ctrl)
# oscillate btwn two points to get step responses of angles
error = np.linalg.norm((x - xref)[:3])
# Plot results
time_axis.append(iter)
roll_data.append(phi)
rolld_data.append(phid)
pitch_data.append(theta)
pitchd_data.append(thetad)