def test_get_body_frame_quat(self):
yaw = 0.
accel = np.array([1.0, 2.34, 5.67])
expect_eul, expect_q, expect_R, data = body_frame.body_frame_from_yaw_and_accel(
yaw, accel, 'all')
eul, q, R, data = body_frame.body_frame_from_q3_and_accel(
expect_q[3], accel, 'all')
def test_yaw_singularity_180_yaw(self):
accel = np.array([1.0, 0, -settings.GRAVITY])
yaw = np.array(np.pi)
q3 = np.sin(yaw / 2)
x_b_current = np.array([-np.sqrt(0.5), 0, np.sqrt(0.5)])
expect = np.array([[0., 0., 1.], [0., -1., 0.], [1, 0., 0.]])
R1, data = body_frame.body_frame_from_yaw_and_accel(
yaw, accel, 'matrix', 'yaw_only', x_b_current=x_b_current)
R2, data = body_frame.body_frame_from_yaw_and_accel(yaw,
accel,
'matrix',
'yaw_only',
x_b_current=None)
R3, data = body_frame.body_frame_from_yaw_and_accel(
yaw, accel, 'matrix', 'second_angle')
R4, data = body_frame.body_frame_from_q3_and_accel(q3, accel, 'matrix')
np.testing.assert_allclose(expect, R1)
np.testing.assert_allclose(expect, R2)
def test_after_yaw_singularity(self):
accel = np.array([1.0, 0, -0.1 - settings.GRAVITY])
yaw = np.array(0.0)
q3 = np.sin(yaw / 2)
x_b_current = np.array([np.sqrt(0.5), 0, -np.sqrt(0.5)])
expect = np.array([[-0.09950372, 0., 0.99503719], [0., 1., 0.],
[-0.99503719, 0., -0.09950372]])
R1, data = body_frame.body_frame_from_yaw_and_accel(
yaw, accel, 'matrix', 'yaw_only', x_b_current=x_b_current)
R2, data = body_frame.body_frame_from_yaw_and_accel(yaw,
accel,
'matrix',
'yaw_only',
x_b_current=None)
R3, data = body_frame.body_frame_from_yaw_and_accel(
yaw, accel, 'matrix', 'second_angle')
R4, data = body_frame.body_frame_from_q3_and_accel(q3, accel, 'matrix')
np.testing.assert_allclose(expect, R1)
np.testing.assert_allclose(expect, R2)
np.testing.assert_allclose(expect, R4)
def test_get_body_frame_second_angle(self):
yaw = 0.
accel = np.array([0., 0., 0.])
expect_eul = np.array([0.0, 0.0, 0.0])
expect_q = np.array([1.0, 0.0, 0.0, 0.0])
expect_R = np.identity(3)
eul, q, R, data = body_frame.body_frame_from_yaw_and_accel(
yaw, accel, 'all', 'second_angle')
np.testing.assert_allclose(expect_eul, eul)
np.testing.assert_allclose(expect_q, q)
np.testing.assert_allclose(expect_R, R)
def update_controls(self,
waypoints,
indices=None,
closed_loop=False,
acc_wp=None):
n_waypoints = len(waypoints['x'][0, :])
if indices is None:
indices = range(0, n_waypoints)
if closed_loop:
# Do not create a marker for the last waypoint
indices = np.delete(indices, np.where(indices == n_waypoints - 1))
for i in indices:
if closed_loop and i == (len(waypoints['x'][0, :]) - 1):
print("ignoring last waypoint")
continue
# Do not create last waypoint control
if self.qr_type == 'entry' and i == (len(waypoints['x'][0, :]) -
1):
# Don't move the waypoint that is fixed to the trajectory
continue
if self.qr_type == 'exit' and i == 0:
# Don't move the waypoint that is fixed to the trajectory
continue
pose = geometry_msgs.msg.Pose()
pose.position.x = waypoints['x'][0, i]
pose.position.y = waypoints['y'][0, i]
pose.position.z = waypoints['z'][0, i]
if acc_wp is None:
q = tf.transformations.quaternion_from_euler(
0.0, 0.0, waypoints['yaw'][0, i])
pose.orientation.w = q[3]
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
else:
q, data = body_frame.body_frame_from_yaw_and_accel(
waypoints['yaw'][0, i],
acc_wp[:, i],
out_format='quaternion',
deriv_type='yaw_only')
pose.orientation.w = q[0]
pose.orientation.x = q[1]
pose.orientation.y = q[2]
pose.orientation.z = q[3]
self.marker_server.setPose(str(i) + self.qr_type, pose)
self.marker_server.applyChanges()
def setUp(self):
waypoints = dict(x=np.array([0.0, 1.0, -1.0, 1.0, -1.0, 0.0]),
y=np.array([0.0, 1.0, 1.0, -1.0, -1.0, 0.0]),
z=np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]),
yaw=np.array([0., 0., 0., 0., 0., 0.]))
qr_p = quadrotor_polytraj.QRPolyTraj(waypoints, 5000)
# 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]
# Yaw
yaw = qr_p.quad_traj['yaw'].piece_poly(t)
self.yaw_dot = qr_p.quad_traj['yaw'].piece_poly.derivative()(t)
self.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
self.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
self.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, self.quat, self.R, data = body_frame.body_frame_from_yaw_and_accel(
yaw, accel, 'all')
# Thrust
thrust, self.thrust_mag = body_frame.get_thrust(accel)
def test_sequence(self):
params = controls.load_params("TestingCode/test_load_params.yaml")
params['k1'] = params['Cq']*1e2
accel = np.array([1,2,3])
jerk = np.array([0.3,0.1,0.4])
snap = np.array([0.01,0.02,0.05])
yaw = np.pi/4
yaw_dot = 0.01
yaw_ddot = 0.001
# 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)
thrust[2] = thrust[2] - settings.GRAVITY + 9.81
thrust_mag = np.linalg.norm(thrust)
# 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)
# torques
torques = controls.get_torques(ang_vel, ang_accel, params)
# Get rotor speeds
rpm = controls.get_rotor_speeds(torques,thrust_mag*params['mass'],params)
expect_ang_vel = np.array([0.01270282,0.01643325,0.009851])
expect_ang_acc = np.array([-0.00154177,0.00060613,0.0009851])
expect_torques = np.array([-4.30155261e-6,2.04911597e-6,6.11937597e-6])
expect_rpm = np.array([8794.2645461,8793.64597287,8794.3097302,8793.74083079])
np.testing.assert_allclose(expect_ang_vel, ang_vel,rtol=1e-3)
np.testing.assert_allclose(expect_ang_acc, ang_accel,rtol=1e-3)
np.testing.assert_allclose(expect_torques, torques,rtol=1e-3)
np.testing.assert_allclose(expect_rpm, rpm,rtol=1e-5)
def setUp(self):
waypoints = dict(x= np.array([0.0, 1.0, -1.0, 1.0, -1.0, 0.0]),
y= np.array([0.0, 1.0, 1.0, -1.0, -1.0, 0.0]),
z= np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]),
yaw=np.array([ 0., 0., 0., 0., 0., 0.]))
qr_p = quadrotor_polytraj.QRPolyTraj(waypoints, 5000)
self.params = dict()
self.params['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
self.params['Inertia'] = np.array([[Lxx,Lxy,Lxz],[Lyx,Lyy,Lyz],[Lzx,Lzy,Lzz]])*unit_conv
# Thrust coefficeint
self.params['k1'] = 2.25*10**-6 # dow 5045 x3 bullnose prop
self.params['k2'] = 0.0 # dow 5045 x3 bullnose prop
self.params['k4'] = 0.0# dow 5045 x3 bullnose prop
self.params['Cq'] = self.params['k1']*10**-2
self.params['Dx'] = 0.088 # meters
self.params['Dy'] = 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[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, self.thrust_mag = body_frame.get_thrust(accel)
# Angular rates
self.ang_vel = angular_rates_accel.get_angular_vel(self.thrust_mag,jerk,R,yaw_dot)
# Angular accelerations
self.ang_accel = angular_rates_accel.get_angular_accel(self.thrust_mag,jerk,snap,R,self.ang_vel,yaw_ddot)
def main():
from diffeo import body_frame
from minsnap 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))
def main():
from diffeo import body_frame
from diffeo import angular_rates_accel
from minsnap 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")
