def f_vel(self, x, u, f_d):
"""
Time-derivative of the velocity vector
:param x: 4-length array of input state with components: 3D pos, quaternion angle, 3D vel, 3D rate
:param u: control input vector (4-dimensional): [trust_motor_1, ..., thrust_motor_4]
:param f_d: disturbance force vector (3-dimensional)
:return: 3D velocity differential increment (vector): d[vel_x; vel_y; vel_z]/dt
"""
a_thrust = np.array([[0], [0], [np.sum(u)]]) / self.mass
if self.drag:
# Transform velocity to body frame
v_b = v_dot_q(x[2], quaternion_inverse(x[1]))[:, np.newaxis]
# Compute aerodynamic drag acceleration in world frame
a_drag = -self.aero_drag * v_b**2 * np.sign(v_b) / self.mass
# Add rotor drag
a_drag -= self.rotor_drag * v_b / self.mass
# Transform drag acceleration to world frame
a_drag = v_dot_q(a_drag, x[1])
else:
a_drag = np.zeros((3, 1))
angle_quaternion = x[1]
a_payload = -self.payload_mass * self.g / self.mass
return np.squeeze(-self.g + a_payload + a_drag +
v_dot_q(a_thrust +
f_d / self.mass, angle_quaternion))
def set_reference_state(self, x_target=None, u_target=None):
"""
Sets the target state and pre-computes the integration dynamics with cost equations
:param x_target: 13-dimensional target state (p_xyz, a_wxyz, v_xyz, r_xyz)
:param u_target: 4-dimensional target control input vector (u_1, u_2, u_3, u_4)
"""
if x_target is None:
x_target = [[0, 0, 0], [1, 0, 0, 0], [0, 0, 0], [0, 0, 0]]
if u_target is None:
u_target = [0, 0, 0, 0]
# Set new target state
self.target = copy(x_target)
ref = np.concatenate([x_target[i] for i in range(4)])
# Transform velocity to body frame
v_b = v_dot_q(ref[7:10], quaternion_inverse(ref[3:7]))
ref = np.concatenate((ref[:7], v_b, ref[10:]))
# Determine which dynamics model to use based on the GP optimal input feature region. Should get one for each
# output dimension of the GP
if self.gp_reg_ensemble is not None:
gp_ind = self.gp_reg_ensemble.select_gp(dim=None, x=ref, u=u_target)
else:
gp_ind = 0
ref = np.concatenate((ref, u_target))
for j in range(self.N):
self.acados_ocp_solver[gp_ind].set(j, "yref", ref)
self.acados_ocp_solver[gp_ind].set(self.N, "yref", ref[:-4])
return gp_ind
def draw_drone(pos, q_rot, x_f, y_f):
# Define quadrotor extremities in body reference frame
x1 = np.array([x_f[0], y_f[0], 0])
x2 = np.array([x_f[1], y_f[1], 0])
x3 = np.array([x_f[2], y_f[2], 0])
x4 = np.array([x_f[3], y_f[3], 0])
# Convert to world reference frame and add quadrotor center point:
x1 = v_dot_q(x1, q_rot) + pos
x2 = v_dot_q(x2, q_rot) + pos
x3 = v_dot_q(x3, q_rot) + pos
x4 = v_dot_q(x4, q_rot) + pos
# Build set of coordinates for plotting
return ([x1[0], x3[0], pos[0], x2[0],
x4[0]], [x1[1], x3[1], pos[1], x2[1],
x4[1]], [x1[2], x3[2], pos[2], x2[2], x4[2]])
def v_dynamics(self, rdrv_d):
"""
:param rdrv_d: a 3x3 diagonal matrix containing the D matrix coefficients for a linear drag model as proposed
by Faessler et al. None, if no linear compensation is to be used.
"""
f_thrust = self.u * self.quad.max_thrust
g = cs.vertcat(0.0, 0.0, 9.81)
a_thrust = cs.vertcat(0.0, 0.0, f_thrust[0] + f_thrust[1] + f_thrust[2] + f_thrust[3]) / self.quad.mass
v_dynamics = v_dot_q(a_thrust, self.q) - g
if rdrv_d is not None:
# Velocity in body frame:
v_b = v_dot_q(self.v, quaternion_inverse(self.q))
rdrv_drag = v_dot_q(cs.mtimes(rdrv_d, v_b), self.q)
v_dynamics += rdrv_drag
return v_dynamics
def world_to_body_velocity_mapping(state_sequence):
"""
:param state_sequence: N x 13 state array, where N is the number of states in the sequence.
:return: An N x 13 sequence of states, but where the velocities (assumed to be in positions 7, 8, 9) have been
rotated from world to body frame. The rotation is made using the quaternion in positions 3, 4, 5, 6.
"""
p, q, v_w, w = separate_variables(state_sequence)
v_b = []
for i in range(len(q)):
v_b.append(v_dot_q(v_w[i], quaternion_inverse(q[i])))
v_b = np.stack(v_b)
return np.concatenate((p, q, v_b, w), 1)
def set_reference_trajectory(self, x_target, u_target):
"""
Sets the reference trajectory and pre-computes the cost equations for each point in the reference sequence.
:param x_target: Nx13-dimensional reference trajectory (p_xyz, angle_wxyz, v_xyz, rate_xyz). It is passed in the
form of a 4-length list, where the first element is a Nx3 numpy array referring to the position targets, the
second is a Nx4 array referring to the quaternion, two more Nx3 arrays for the velocity and body rate targets.
:param u_target: Nx4-dimensional target control input vector (u1, u2, u3, u4)
"""
if u_target is not None:
assert x_target[0].shape[0] == (u_target.shape[0] + 1) or x_target[0].shape[0] == u_target.shape[0]
# If not enough states in target sequence, append last state until required length is met
while x_target[0].shape[0] < self.N + 1:
x_target = [np.concatenate((x, np.expand_dims(x[-1, :], 0)), 0) for x in x_target]
if u_target is not None:
u_target = np.concatenate((u_target, np.expand_dims(u_target[-1, :], 0)), 0)
stacked_x_target = np.concatenate([x for x in x_target], 1)
# Transform velocity to body frame
x_mean = stacked_x_target[int(self.N / 2)]
v_b = v_dot_q(x_mean[7:10], quaternion_inverse(x_mean[3:7]))
x_target_mean = np.concatenate((x_mean[:7], v_b, x_mean[10:]))
# Determine which dynamics model to use based on the GP optimal input feature region
if self.gp_reg_ensemble is not None:
gp_ind = self.gp_reg_ensemble.select_gp(dim=None, x=x_target_mean, u=u_target[int(self.N / 2)])
else:
gp_ind = 0
self.target = copy(x_target)
for j in range(self.N):
ref = stacked_x_target[j, :]
ref = np.concatenate((ref, u_target[j, :]))
self.acados_ocp_solver[gp_ind].set(j, "yref", ref)
# the last MPC node has only a state reference but no input reference
self.acados_ocp_solver[gp_ind].set(self.N, "yref", stacked_x_target[self.N, :])
return gp_ind
def odometry_callback(self, msg):
"""
Callback function for Odometry subscriber
:param msg: message from subscriber.
:type msg: Odometry
"""
if self.controller_off:
return
p, q, v, w = odometry_parse(msg)
# Change velocity to world frame if in gazebo environment
if self.environment == "gazebo":
v_w = v_dot_q(np.array(v), np.array(q)).tolist()
else:
v_w = v
self.x = p + q + v_w + w
try:
# Update the state estimate of the quad
self.gp_mpc.set_state(self.x)
# If an estimate specifically for the GP's is available, also update it
if self.gp_odom is not None:
self.gp_mpc.set_gp_state(self.gp_odom)
except AttributeError:
# The GP MPC object instantiation is still not finished
return
if self.override_land:
return
# If the above try passed then MPC is ready
self.odom_available = True
# We only optimize once every two odometry messages
if not self.optimize_next:
self.mpc_thread.join()
# If currently on trajectory tracking, pay close attention to any skipped messages.
if self.x_initial_reached:
# Count how many messages were skipped (ideally 0)
skipped_messages = int(msg.header.seq - self.last_odom_seq_number - 1)
if skipped_messages > 0:
warn_msg = "Recording time skipped messages: %d" % skipped_messages
rospy.logwarn(warn_msg)
# Adjust current index in trajectory
self.current_idx += divmod(skipped_messages, 2)[0]
# If odd number of skipped messages, do optimization
if skipped_messages > 0 and skipped_messages % 2 == 1:
if self.recording_options["recording"]:
self.check_out_initial_state(msg)
# Run MPC now
self.run_mpc(msg)
self.last_odom_seq_number = msg.header.seq
self.optimize_next = False
return
self.optimize_next = True
if self.recording_options["recording"] and self.x_initial_reached:
self.check_out_initial_state(msg)
return
# Run MPC
if msg.header.seq > self.last_odom_seq_number + 2 and self.last_odom_seq_number > 0 and self.x_initial_reached:
# If one message was skipped at this point, then the reference is already late. Compensate by
# optimizing twice in a row and hope to do it fast...
if self.recording_options["recording"] and self.x_initial_reached:
self.check_out_initial_state(msg)
self.run_mpc(msg)
self.optimize_next = True
self.last_odom_seq_number = msg.header.seq
odometry_skipped_warning(self.last_odom_seq_number, msg.header.seq, "optimization")
return
def _thread_func():
self.run_mpc(msg)
self.mpc_thread = threading.Thread(target=_thread_func(), args=(), daemon=True)
self.mpc_thread.start()
self.last_odom_seq_number = msg.header.seq
self.optimize_next = False
def main(model_name,
features,
quad_sim_options,
dataset_name,
x_cap,
hist_bins,
hist_thresh,
plot=False):
df_train = read_dataset(dataset_name, True, quad_sim_options)
gp_dataset = GPDataset(df_train,
features, [],
features,
cap=x_cap,
n_bins=hist_bins,
thresh=hist_thresh,
visualize_data=False)
# Get X,Y datasets for the specified regression dimensions (subset of [7, 8, 9])
a_err_b = gp_dataset.y
v_b = gp_dataset.x
drag_coeffs = linear_rdrv_fitting(v_b, a_err_b, np.array(features) - 7)
# Get git commit hash for saving the model
git_version = ''
try:
git_version = subprocess.check_output(['git', 'describe', '--always'
]).strip().decode("utf-8")
except subprocess.CalledProcessError as e:
print(e.returncode, e.output)
gp_name_dict = {
"git": git_version,
"model_name": model_name,
"params": quad_sim_options
}
save_file_path, save_file_name = get_model_dir_and_file(gp_name_dict)
safe_mknode_recursive(save_file_path,
save_file_name + '.pkl',
overwrite=True)
file_name = os.path.join(save_file_path, save_file_name + '.pkl')
joblib.dump(drag_coeffs, file_name)
if not plot:
return drag_coeffs
# Get X,Y datasets for velocity dimensions [7, 8, 9]
a_err_b = gp_dataset.get_y(pruned=True, raw=True)[:, 7:10]
v_b = gp_dataset.get_x(pruned=True, raw=True)[:, 7:10]
# Compute predictions with RDRv model
preds = []
for i in range(len(a_err_b)):
preds.append(np.matmul(drag_coeffs, v_b[i]))
preds = np.stack(preds)
ax_names = [r'$v_B^x$', r'$v_B^y$', r'$v_B^z$']
y_dim_name = [r'drag $a_B^x$', 'drag $a_B^y$', 'drag $a_B^z$']
fig, ax = plt.subplots(len(features), 1, sharex='all')
for i in range(len(features)):
ax[i].scatter(v_b[:, i], a_err_b[:, i], label='data')
ax[i].scatter(v_b[:, i], preds[:, i], label='RDRv')
ax[i].legend()
ax[i].set_ylabel(y_dim_name[features[i] - 7])
ax[i].set_xlabel(ax_names[features[i] - 7])
ax[0].set_title('Body reference frame')
# Remap error to world coordinates and predictions too
q = gp_dataset.get_x(pruned=True, raw=True)[:, 3:7]
for i in range(len(a_err_b)):
a_err_b[i] = v_dot_q(a_err_b[i], q[i])
preds[i] = v_dot_q(preds[i], q[i])
ax_names = [r'$v_W^x$', r'$v_W^y$', r'$v_W^z$']
y_dim_name = [r'drag $a_W^x$', 'drag $a_W^y$', 'drag $a_W^z$']
fig, ax = plt.subplots(len(features), 1, sharex='all')
for i in range(len(features)):
ax[i].scatter(v_b[:, i], a_err_b[:, i], label='data')
ax[i].scatter(v_b[:, i], preds[:, i], label='RDRv')
ax[i].legend()
ax[i].set_ylabel(y_dim_name[features[i] - 7])
ax[i].set_xlabel(ax_names[features[i] - 7])
ax[0].set_title('World reference frame')
plt.show()
return drag_coeffs
def acados_setup_model(self, nominal, model_name):
"""
Builds an Acados symbolic models using CasADi expressions.
:param model_name: name for the acados model. Must be different from previously used names or there may be
problems loading the right model.
:param nominal: CasADi symbolic nominal model of the quadrotor: f(self.x, self.u) = x_dot, dimensions 13x1.
:return: Returns a total of three outputs, where m is the number of GP's in the GP ensemble, or 1 if no GP:
- A dictionary of m AcadosModel of the GP-augmented quadrotor
- A dictionary of m CasADi symbolic nominal dynamics equations with GP mean value augmentations (if with GP)
:rtype: dict, dict, cs.MX
"""
def fill_in_acados_model(x, u, p, dynamics, name):
x_dot = cs.MX.sym('x_dot', dynamics.shape)
f_impl = x_dot - dynamics
# Dynamics model
model = AcadosModel()
model.f_expl_expr = dynamics
model.f_impl_expr = f_impl
model.x = x
model.xdot = x_dot
model.u = u
model.p = p
model.name = name
return model
acados_models = {}
dynamics_equations = {}
# Run GP inference if GP's available
if self.gp_reg_ensemble is not None:
# Feature vector are the elements of x and u determined by the selection matrix B_z. The trigger var is used
# to select the gp-specific state estimate in the first optimization node, and the regular integrated state
# in the rest. The computing of the features z is done within the GPEnsemble.
gp_x = self.gp_x * self.trigger_var + self.x * (1 - self.trigger_var)
# Transform velocity to body frame
v_b = v_dot_q(gp_x[7:10], quaternion_inverse(gp_x[3:7]))
gp_x = cs.vertcat(gp_x[:7], v_b, gp_x[10:])
gp_u = self.u
gp_dims = self.gp_reg_ensemble.dim_idx
# Get number of models in GP
for i in range(self.gp_reg_ensemble.n_models):
# Evaluate cluster of the GP ensemble
cluster_id = {k: [v] for k, v in zip(gp_dims, i * np.ones_like(gp_dims, dtype=int))}
outs = self.gp_reg_ensemble.predict(gp_x, gp_u, return_cov=False, gp_idx=cluster_id, return_z=False)
# Unpack prediction outputs. Transform back to world reference frame
outs = self.add_missing_states(outs)
gp_means = v_dot_q(outs["pred"], gp_x[3:7])
gp_means = self.remove_extra_states(gp_means)
# Add GP mean prediction
dynamics_equations[i] = nominal + cs.mtimes(self.B_x, gp_means)
x_ = self.x
dynamics_ = dynamics_equations[i]
# Add again the gp augmented dynamics for the GP state
dynamics_ = cs.vertcat(dynamics_)
dynamics_equations[i] = cs.vertcat(dynamics_equations[i])
i_name = model_name + "_domain_" + str(i)
params = cs.vertcat(self.gp_x, self.trigger_var)
acados_models[i] = fill_in_acados_model(x=x_, u=self.u, p=params, dynamics=dynamics_, name=i_name)
else:
# No available GP so return nominal dynamics
dynamics_equations[0] = nominal
x_ = self.x
dynamics_ = nominal
acados_models[0] = fill_in_acados_model(x=x_, u=self.u, p=[], dynamics=dynamics_, name=model_name)
return acados_models, dynamics_equations