def test_input_restrict_freespace(self):
# With single input l_max and A_max
order=dict(x=9, y=9, z=5, yaw=5)
temp_waypoints = utils.load_waypoints('share/sample_data/waypoints_two_ellipse.yaml')
waypoints = utils.form_waypoints_polytraj(temp_waypoints,order)
time_penalty = 100.0
l_max = 1.0
A_max = 10.0
qr_p = quadrotor_polytraj.QRPolyTraj(waypoints, time_penalty, order=order,
restrict_freespace=True,l_max=l_max,A_max=A_max)
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_input_restrict_freespace_vector_l_A(self):
order=dict(x=9, y=9, z=9, yaw=5)
temp_waypoints = utils.load_waypoints('share/sample_data/waypoints_two_ellipse.yaml')
waypoints = utils.form_waypoints_polytraj(temp_waypoints,order)
time_penalty = 100.0
l_max = np.ones(waypoints['x'].shape[1]-1)*0.5
l_max[0] = 3.0
l_max[5] = 2.0
l_max[-1] = 1.0
A_max = np.ones(waypoints['x'].shape[1]-1)*20.0
A_max[0] = 10.0
A_max[-1] = 10.0
qr_p = quadrotor_polytraj.QRPolyTraj(waypoints, time_penalty, order=order,
restrict_freespace=True,l_max=l_max,A_max=A_max)
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
self.yaw = qr_p.quad_traj['yaw'].piece_poly(t)
# accel
self.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)
])
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 test_basic_input_order(self):
order=dict(x=9, y=9, z=5, yaw=5)
temp_waypoints = utils.load_waypoints('share/sample_data/waypoints_two_ellipse.yaml')
waypoints = utils.form_waypoints_polytraj(temp_waypoints,order)
time_penalty = 100.0
qr_p = quadrotor_polytraj.QRPolyTraj(waypoints, time_penalty, order=order)
