def get_piece_poly(self):
"""Get the piecewise polynomial from the coefficients
Uses:
self.
coeffs: piecewise polynomial coefficients
times: time in which to complete each segment (not the transition
times)
Modifies:
self.
piece_poly: An instance of scipy.interpolate.PPoly
Args:
Returns:
Raises:
"""
trans_times = utils.seg_times_to_trans_times(self.times)
self.piece_poly = sp.interpolate.PPoly(self.coeffs,
trans_times,
extrapolate=False)
def create_n_laps(self, n_laps, entry_ID, exit_ID):
"""
Behaviour: Will no n_laps, where the last lap will not be a complete loop,
instead it will finish at the exit_ID waypoint
"""
coeffs = self.coeffs.copy()
times = self.times.copy()
if entry_ID < exit_ID or (entry_ID == exit_ID and entry_ID == 0):
# Entry ID earlier - repeat for number of laps
new_coeffs = np.tile(coeffs, (1, n_laps))
new_times = np.tile(times, n_laps)
else:
# Entry ID later, so need to add another lap
new_coeffs = np.tile(coeffs, (1, n_laps + 1))
new_times = np.tile(times, n_laps + 1)
# new_coeffs = np.append(new_coeffs,new_coeffs[:,0],axis=1)
# Delete from the start to get correct start ID
new_coeffs = np.delete(new_coeffs, np.arange(entry_ID), axis=1)
new_times = np.delete(new_times, np.arange(entry_ID))
# Delete from the end to get the correct exit ID
if exit_ID != 0:
new_coeffs = np.delete(
new_coeffs,
np.arange(new_coeffs.shape[1] - (coeffs.shape[1] - exit_ID),
new_coeffs.shape[1]),
axis=1)
new_times = np.delete(
new_times,
np.arange(new_times.shape[0] - (times.shape[0] - exit_ID),
new_times.shape[0]))
# Do nothing if zero is the exit ID (zero is by default the final term)
# Get the piecewise polynomial
trans_times = utils.seg_times_to_trans_times(new_times)
piece_poly = sp.interpolate.PPoly(new_coeffs,
trans_times,
extrapolate=False)
#
# # Check continuity
return piece_poly
def check_continuity(self, eps=1e-6, equal_eps=1e-3):
"""Checks that the piecewise polynomial and its derivatives are continuous
Uses:
self.
piece_poly: piecewise-continuous polynomial of segments
times: time in which to complete each segment (not the transition
times)
Modifies:
self.
piece_poly: An instance of scipy.interpolate.PPoly
Args:
eps: epsilon value specifying how far on each side of each
transition time to set the data point for continuity checking
equal_eps: epsilon value specifying how close the values have to be
in order to be considered equal
Returns:
Raises:
"""
temp_ppoly = self.piece_poly
# ppoly has no data before and after first transition points, so we
# can't check those
trans_times = utils.seg_times_to_trans_times(self.times)[1:-1]
for i in range(self.n_der):
if not np.allclose(temp_ppoly(trans_times - eps),
temp_ppoly(trans_times + eps), equal_eps):
return False
temp_ppoly = temp_ppoly.derivative()
return True
def append(self, new_poly):
"""Utility function for PPoly of different order"""
times = np.r_[self.times, new_poly.times]
trans_times = utils.seg_times_to_trans_times(times)
order_delta = np.shape(self.coeffs)[0] - np.shape(new_poly.coeffs)[0]
if order_delta < 0:
coeffs = np.c_[self.coeffs,
np.pad(new_poly.coeffs, ((-order_delta, 0), (0, 0)),
'constant',
constant_values=(0))]
elif order_delta > 0:
coeffs = np.c_[np.pad(self.coeffs, ((order_delta, 0), (0, 0)),
'constant',
constant_values=(0)), new_poly.coeffs]
else:
coeffs = np.c_[self.coeffs, new_poly.coeffs]
piece_poly = sp.interpolate.PPoly(coeffs,
trans_times,
extrapolate=False)
return piece_poly
def main():
from quest.diffeo import body_frame
from quest.diffeo import angular_rates_accel
from quest import utils
params = load_params("TestingCode/test_load_params.yaml")
import pdb
pdb.set_trace()
# Load a trajectory
import pickle
f = open('share/sample_output/traj.pickle', 'rb')
qr_p = pickle.load(f)
f.close
mass = 0.48446 # kg
Lxx = 1131729.47
Lxy = -729.36
Lxz = -5473.45
Lyx = -729.36
Lyy = 1862761.14
Lyz = -2056.74
Lzx = -5473.45
Lzy = -2056.74
Lzz = 2622183.34
unit_conv = 1 * 10**-9
Inertia = np.array([[Lxx, Lxy, Lxz], [Lyx, Lyy, Lyz], [Lzx, Lzy, Lzz]
]) * unit_conv
# Thrust coefficeint
k_f = 2.25 * 10**-6 # dow 5045 x3 bullnose prop
k_m = k_f * 10**-2
L = 0.088 # meters
# Compute times
trans_times = utils.seg_times_to_trans_times(qr_p.times)
t1 = np.linspace(trans_times[0], trans_times[-1], 100)
# To test single input:
t = t1[9] #:4]
# Yaw
yaw = qr_p.quad_traj['yaw'].piece_poly(t)
yaw_dot = qr_p.quad_traj['yaw'].piece_poly.derivative()(t)
yaw_ddot = qr_p.quad_traj['yaw'].piece_poly.derivative().derivative()(t)
# accel
accel = np.array([
qr_p.quad_traj['x'].piece_poly.derivative().derivative()(t),
qr_p.quad_traj['y'].piece_poly.derivative().derivative()(t),
qr_p.quad_traj['z'].piece_poly.derivative().derivative()(t)
])
# jerk
jerk = np.array([
qr_p.quad_traj['x'].piece_poly.derivative().derivative().derivative()(
t),
qr_p.quad_traj['y'].piece_poly.derivative().derivative().derivative()(
t),
qr_p.quad_traj['z'].piece_poly.derivative().derivative().derivative()(
t)
])
# snap
snap = np.array([
qr_p.quad_traj['x'].piece_poly.derivative().derivative().derivative(
).derivative()(t), qr_p.quad_traj['y'].piece_poly.derivative(
).derivative().derivative().derivative()(t), qr_p.quad_traj['z'].
piece_poly.derivative().derivative().derivative().derivative()(t)
])
# Get rotation matrix
R, data = body_frame.body_frame_from_yaw_and_accel(yaw, accel, 'matrix')
# Thrust
thrust, thrust_mag = body_frame.get_thrust(accel)
# Angular rates
ang_vel = angular_rates_accel.get_angular_vel(thrust_mag, jerk, R, yaw_dot)
# Angular accelerations
ang_accel = angular_rates_accel.get_angular_accel(thrust_mag, jerk, snap,
R, ang_vel, yaw_ddot)
#--------------------------------------------------------------------------#
# Get torque
print("ang vel:\n {}\n ang_accel: \n{}\n Inertia: \n{}".format(
ang_vel, ang_accel, params['Inertia']))
torques = get_torques(ang_vel, ang_accel, params)
print("Torques: {}\n".format(torques))
print('Net Force: {}\n'.format(thrust_mag * params['mass']))
# Get rotor speeds
rpm = get_rotor_speeds(torques, thrust_mag * params['mass'], params)
print("Rotor speeds:\n{}".format(rpm))
import pdb
pdb.set_trace()
test = dict()
test['x'] = np.flip(
np.array([[
2.5000, -0.0000, 0.0000, -0.0000, -0.0000, -210.0247, 579.8746,
-636.6504, 328.7759, -66.9753
],
[
-2.5000, 0.0000, 22.8251, -0.0000, -59.3519, 127.8808,
-296.8831, 417.5559, -274.0022, 66.9753
]]).T, 0)
test['y'] = np.flip(
np.array([[
5.0000, -0.0000, 0.0000, -0.0000, 0.0000, 70.0364, -165.7848,
166.1452, -81.4554, 16.0586
],
[
10.0000, 11.3739, -0.0000, -12.8287, -0.0000, 26.2608,
-65.4048, 92.6125, -63.0722, 16.0586
]]).T, 0)
test['z'] = np.zeros([10, 2])
test['z'][-1, :] = 3.0
test['yaw'] = np.zeros([10, 2])
test_case = 1
for key in qr_p.quad_traj.keys():
if test_case == 1:
qr_p.quad_traj[key].piece_poly.x = np.array([0.0, 1.0, 2.0])
qr_p.quad_traj[key].piece_poly.c = test[key]
elif test_case == 2:
qr_p.quad_traj[key].piece_poly.x = qr_p.quad_traj[
key].piece_poly.x[0:3]
qr_p.quad_traj[key].piece_poly.c = qr_p.quad_traj[
key].piece_poly.c[:, 0:2]
f_in = open("/media/sf_shared_torq_vm/Results/test_diffeo_in_good.txt",
'w')
f_in.write("Input:\nC_x: {}\nC_y: {}\nC_z: {}\n".format(
qr_p.quad_traj['x'].piece_poly.c, qr_p.quad_traj['y'].piece_poly.c,
qr_p.quad_traj['z'].piece_poly.c))
f_in.write("Time at waypoints: {}".format(
qr_p.quad_traj['x'].piece_poly.x))
f_in.close()
f = open("/media/sf_shared_torq_vm/Results/test_diffeo_good.txt", 'w')
# t = np.linspace(trans_times[0], trans_times[-1], n_steps)
t = 0.5
z = qr_p.quad_traj['z'].piece_poly(t)
x = qr_p.quad_traj['x'].piece_poly(t)
y = qr_p.quad_traj['y'].piece_poly(t)
yaw = qr_p.quad_traj['yaw'].piece_poly(t)
yaw_dot = qr_p.quad_traj['yaw'].piece_poly.derivative()(t)
yaw_ddot = qr_p.quad_traj['yaw'].piece_poly.derivative().derivative()(t)
f.write("t: {}\nX: {},Y: {},Z: {}\n".format(t, x, y, z))
Vel = np.array([
qr_p.quad_traj['x'].piece_poly.derivative().derivative()(t),
qr_p.quad_traj['y'].piece_poly.derivative().derivative()(t),
qr_p.quad_traj['z'].piece_poly.derivative().derivative()(t)
])
accel = np.array([
qr_p.quad_traj['x'].piece_poly.derivative().derivative()(t),
qr_p.quad_traj['y'].piece_poly.derivative().derivative()(t),
qr_p.quad_traj['z'].piece_poly.derivative().derivative()(t)
])
jerk = np.array([
qr_p.quad_traj['x'].piece_poly.derivative().derivative().derivative()(
t),
qr_p.quad_traj['y'].piece_poly.derivative().derivative().derivative()(
t),
qr_p.quad_traj['z'].piece_poly.derivative().derivative().derivative()(
t)
])
snap = np.array([
qr_p.quad_traj['x'].piece_poly.derivative().derivative().derivative(
).derivative()(t), qr_p.quad_traj['y'].piece_poly.derivative(
).derivative().derivative().derivative()(t), qr_p.quad_traj['z'].
piece_poly.derivative().derivative().derivative().derivative()(t)
])
f.write("Accel: {}\nJerk: {}\nSnap: {}\n".format(accel, jerk, snap))
# Get rotation matrix
R, data = body_frame.body_frame_from_yaw_and_accel(yaw, accel, 'matrix')
# Thrust
thrust, thrust_mag = body_frame.get_thrust(accel)
import pdb
pdb.set_trace()
# Angular rates
ang_vel = angular_rates_accel.get_angular_vel(thrust_mag, jerk, R, yaw_dot)
# Angular accelerations
ang_accel = angular_rates_accel.get_angular_accel(thrust_mag, jerk, snap,
R, ang_vel, yaw_ddot)
params = controls.load_params("TestingCode/test_load_params.yaml")
# torques
torques = get_torques(ang_vel, ang_accel, params)
# rpm
rpm = get_rotor_speeds(torques, thrust_mag * mass, params)
f.write(
"R: {}\nang_vel: {}\nang_accel: {}\nThrust: {}\ntorques: {}\nrpm: {}\n"
.format(R, ang_vel, ang_accel, thrust * mass, torques, rpm))
f.close()
import pdb
pdb.set_trace()
print("Done")
def main():
from quest import utils
# Load a trajectory
import pickle
f = open('share/sample_output/traj.pickle','rb')
qr_p = pickle.load(f)
f.close
# Compute times
trans_times = utils.seg_times_to_trans_times(qr_p.times)
t1 = np.linspace(trans_times[0], trans_times[-1], 100)
# To test single input:
t = t1[15]#:4]
# Yaw
yaw = 0.#qr_p.quad_traj['yaw'].piece_poly(t)
print(yaw)
# accel
accel = np.array([qr_p.quad_traj['x'].piece_poly.derivative().derivative()(t),qr_p.quad_traj['y'].piece_poly.derivative().derivative()(t),
qr_p.quad_traj['z'].piece_poly.derivative().derivative()(t)])
print(accel.shape)
eul, quat, R, data = body_frame_from_yaw_and_accel(yaw, accel,'all')
eul2, quat2, R2 , data = body_frame_from_q3_and_accel(yaw, accel, 'all')
#
# print("\n EULER DERIVATION \n")
# print("Rotation Matrix is\n {}".format(R))
# print("Euler angles are: \n{}".format(eul))
# print("Quaternion is: \n{}".format(quat))
#
# print("\n QUATERNION DERIVATION \n")
# print("Rotation Matrix is\n {}".format(R2))
# print("Euler angles are: \n{}".format(eul2))
# print("Quaternion is: \n{}".format(quat2))
#
#
# #_--------------------------------------------#
# # Test singularity cases
# accel = np.array([0.,0.,1.0])#np.random.randn(3)
# yaw = np.array(0.0)
# eul, quat, R, data = body_frame_from_yaw_and_accel(yaw, accel,'all')
#
# # for i in range(20):
# # accel = np.random.randn(3,1)
# eul2, quat2, R2, data = body_frame_from_q3_and_accel(yaw, accel, 'all')
#
# print("\n EULER DERIVATION \n")
# print("Rotation Matrix is\n {}".format(R))
# print("Euler angles are: \n{}".format(eul))
# print("Quaternion is: \n{}".format(quat))
#
# print("\n QUATERNION DERIVATION \n")
# print("Rotation Matrix is\n {}".format(R2))
# print("Euler angles are: \n{}".format(eul2))
# print("Quaternion is: \n{}".format(quat2))
method_list = ['yaw_only','check_negative_set','second_angle','x_or_y_furthest','check_x_b_current','quaternions']
x_b_current = np.array([0.0995,0,-0.995])
x_b_current = np.array([-0.0995,0,0.995])
print('Zero Thrust \n')
accel = np.array([0,0,-settings.GRAVITY])
yaw = np.array(np.pi)
q3 = np.sin(yaw/2)
for item in method_list:
if item == 'quaternions':
R, data = body_frame_from_q3_and_accel(q3, accel, 'matrix')
else:
R, data = body_frame_from_yaw_and_accel(yaw, accel,'matrix',deriv_type=item,x_b_current=x_b_current)
print("Method: {}\n{}\n".format(item,R))
print('Testing singularities \n')
print('Yaw singularities \n')
accel = np.array([1.0,0,-settings.GRAVITY])
# yaw = np.array(0.0)
q3 = np.sin(yaw/2)
for item in method_list:
if item == 'quaternions':
R, data = body_frame_from_q3_and_accel(q3, accel, 'matrix')
else:
R, data = body_frame_from_yaw_and_accel(yaw, accel,'matrix',deriv_type=item,x_b_current=x_b_current)
print("Method: {}\n{}\n".format(item,R))
print('Before Yaw singularities \n')
accel = np.array([1.0,0,0.1-settings.GRAVITY])
# yaw = np.array(0.0)
q3 = np.sin(yaw/2)
for item in method_list:
if item == 'quaternions':
R, data = body_frame_from_q3_and_accel(q3, accel, 'matrix')
else:
R, data = body_frame_from_yaw_and_accel(yaw, accel,'matrix',deriv_type=item,x_b_current=x_b_current)
print("Method: {}\n{}\n".format(item,R))
print('After Yaw singularities \n')
accel = np.array([1.0,0,-0.1-settings.GRAVITY])
# yaw = np.array(0.0)
q3 = np.sin(yaw/2)
for item in method_list:
if item == 'quaternions':
R, data = body_frame_from_q3_and_accel(q3, accel, 'matrix')
else:
R, data = body_frame_from_yaw_and_accel(yaw, accel,'matrix',deriv_type=item,x_b_current=x_b_current)
print("Method: {}\n{}\n".format(item,R))
print('Quaternion singularity \n')
accel = np.array([0,0,-1.0-settings.GRAVITY])
# yaw = np.array(0.0)
q3 = np.sin(yaw/2)
for item in method_list:
if item == 'quaternions':
R, data = body_frame_from_q3_and_accel(q3, accel, 'matrix')
else:
R = body_frame_from_yaw_and_accel(yaw, accel,'matrix',deriv_type=item,x_b_current=x_b_current)
print("Method: {}\n{}\n".format(item,R))
def main():
from quest.diffeo import body_frame
from quest import utils
# Load a trajectory
import pickle
f = open('share/sample_output/traj.pickle','rb')
qr_p = pickle.load(f)
f.close
mass = 1.0
# Compute times
trans_times = utils.seg_times_to_trans_times(qr_p.times)
t1 = np.linspace(trans_times[0], trans_times[-1], 100)
# To test single input:
t = t1[4]#:4]
# Yaw
yaw = qr_p.quad_traj['yaw'].piece_poly(t)
yaw_dot = qr_p.quad_traj['yaw'].piece_poly.derivative()(t)
yaw_ddot = qr_p.quad_traj['yaw'].piece_poly.derivative().derivative()(t)
# accel
accel = np.array([qr_p.quad_traj['x'].piece_poly.derivative().derivative()(t),
qr_p.quad_traj['y'].piece_poly.derivative().derivative()(t),
qr_p.quad_traj['z'].piece_poly.derivative().derivative()(t)])
# jerk
jerk = np.array([qr_p.quad_traj['x'].piece_poly.derivative().derivative().derivative()(t),
qr_p.quad_traj['y'].piece_poly.derivative().derivative().derivative()(t),
qr_p.quad_traj['z'].piece_poly.derivative().derivative().derivative()(t)])
# snap
snap = np.array([qr_p.quad_traj['x'].piece_poly.derivative().derivative().derivative().derivative()(t),
qr_p.quad_traj['y'].piece_poly.derivative().derivative().derivative().derivative()(t),
qr_p.quad_traj['z'].piece_poly.derivative().derivative().derivative().derivative()(t)])
# Get rotation matrix
euler, quat, R = body_frame.body_frame_from_yaw_and_accel(yaw, accel,'all')
# Thrust
thrust, thrust_mag = body_frame.get_thrust(accel)
print("\n EULER DERIVATION")
# Angular rates
ang_vel = get_angular_vel(thrust_mag,jerk,R,yaw_dot)
print("angular rates are: \n{}".format(ang_vel))
# Angular accelerations
ang_accel = get_angular_accel(thrust_mag,jerk,snap,R,ang_vel,yaw_ddot)
print("angular accels are: \n{}".format(ang_accel))
print("\n QUATERNION DERIVATION")
# Angular rates
q3_dot = yaw_dot
q3_ddot = yaw_ddot
ang_vel = get_angular_vel_quat(thrust_mag,jerk,R,q3_dot,quat)
print("angular rates are: \n{}".format(ang_vel))
# Angular accelerations
ang_accel = get_angular_accel_quat(thrust_mag,jerk,snap,R,ang_vel,q3_ddot,quat)
print("angular accels are: \n{}".format(ang_accel))