def compute_rotation_matrix_wind_to_geometry(self):
# Computes the 3x3 rotation matrix required to go from wind axes to geometry axes.
sinalpha = np.sin(np.radians(self.alpha))
cosalpha = np.cos(np.radians(self.alpha))
sinbeta = np.sin(np.radians(self.beta))
cosbeta = np.cos(np.radians(self.beta))
# r=-1*np.array([
# [cosbeta*cosalpha, -sinbeta, cosbeta*sinalpha],
# [sinbeta*cosalpha, cosbeta, sinbeta*sinalpha],
# [-sinalpha, 0, cosalpha]
# ])
eye = np.eye(3)
alpharotation = np.array([[cosalpha, 0, -sinalpha], [0, 1, 0],
[sinalpha, 0, cosalpha]])
betarotation = np.array([[cosbeta, -sinbeta, 0], [sinbeta, cosbeta, 0],
[0, 0, 1]])
axesflip = np.array(
[[-1, 0, 0], [
0,
1,
0,
], [0, 0, -1]]
) # Since in geometry axes, X is downstream by convention, while in wind axes, X is upstream by convetion. Same with Z being up/down respectively.
r = axesflip @ alpharotation @ betarotation @ eye # where "@" is the matrix multiplication operator
return r
def xyz_te(self):
xyz_te = self.xyz_le + self.chord * np.array(
[np.cos(np.radians(self.twist)),
0,
-np.sin(np.radians(self.twist))
])
return xyz_te
def add_control_surface(self, deflection=0., hinge_point=0.75):
# Returns a version of the airfoil with a control surface added at a given point.
# Inputs:
# # deflection: the deflection angle, in degrees. Downwards-positive.
# # hinge_point: the location of the hinge, as a fraction of chord.
# Make the rotation matrix for the given angle.
sintheta = np.sin(np.radians(-deflection))
costheta = np.cos(np.radians(-deflection))
rotation_matrix = np.array([[costheta, -sintheta],
[sintheta, costheta]])
# Find the hinge point
hinge_point = np.array(
(hinge_point, self.get_camber_at_chord_fraction(hinge_point)
)) # Make hinge_point a vector.
# Split the airfoil into the sections before and after the hinge
split_index = np.where(
self.mcl_coordinates[:, 0] > hinge_point[0])[0][0]
mcl_coordinates_before = self.mcl_coordinates[:split_index, :]
mcl_coordinates_after = self.mcl_coordinates[split_index:, :]
upper_minus_mcl_before = self.upper_minus_mcl[:split_index, :]
upper_minus_mcl_after = self.upper_minus_mcl[split_index:, :]
# Rotate the mean camber line (MCL) and "upper minus mcl"
new_mcl_coordinates_after = np.transpose(
rotation_matrix
@ np.transpose(mcl_coordinates_after - hinge_point)) + hinge_point
new_upper_minus_mcl_after = np.transpose(
rotation_matrix @ np.transpose(upper_minus_mcl_after))
# Do blending
# Assemble airfoil
new_mcl_coordinates = np.vstack(
(mcl_coordinates_before, new_mcl_coordinates_after))
new_upper_minus_mcl = np.vstack(
(upper_minus_mcl_before, new_upper_minus_mcl_after))
upper_coordinates = np.flipud(new_mcl_coordinates +
new_upper_minus_mcl)
lower_coordinates = new_mcl_coordinates - new_upper_minus_mcl
coordinates = np.vstack((upper_coordinates, lower_coordinates[1:, :]))
new_airfoil = Airfoil(name=self.name + " flapped",
coordinates=coordinates,
repanel=False)
return new_airfoil # TODO fix self-intersecting airfoils at high deflections
def visualize(self, states, dt):
import meshcat
import meshcat.geometry as g
import meshcat.transformations as tf
import time
# Create a new visualizer
vis = meshcat.Visualizer()
vis.open()
vis["/Cameras/default"].set_transform(
tf.translation_matrix([0, 0, 0]).dot(
tf.euler_matrix(0, np.radians(-30), -np.pi / 2)))
vis["/Cameras/default/rotated/<object>"].set_transform(
tf.translation_matrix([1, 0, 0]))
vis["Quadrotor"].set_object(
g.StlMeshGeometry.from_file('systems/crazyflie2.stl'))
while True:
for state in states:
vis["Quadrotor"].set_transform(
tf.translation_matrix([state[0], state[1], state[2]]).dot(
tf.quaternion_matrix(state[6:10])))
time.sleep(dt)
def getBlenderProj(az, el, distance_ratio, img_w=IMG_W, img_h=IMG_H):
"""Calculate 4x3 3D to 2D projection matrix given viewpoint parameters."""
# Calculate intrinsic matrix.
scale = RESOLUTION_PCT / 100
f_u = F_MM * img_w * scale / SENSOR_SIZE_MM
f_v = F_MM * img_h * scale * PIXEL_ASPECT_RATIO / SENSOR_SIZE_MM
u_0 = img_w * scale / 2
v_0 = img_h * scale / 2
K = np.matrix(((f_u, SKEW, u_0), (0, f_v, v_0), (0, 0, 1)))
# Calculate rotation and translation matrices.
# Step 1: World coordinate to object coordinate.
sa = np.sin(np.radians(-az))
ca = np.cos(np.radians(-az))
se = np.sin(np.radians(-el))
ce = np.cos(np.radians(-el))
R_world2obj = np.transpose(np.matrix(((ca * ce, -sa, ca * se),
(sa * ce, ca, sa * se),
(-se, 0, ce))))
# Step 2: Object coordinate to camera coordinate.
R_obj2cam = np.transpose(np.matrix(CAM_ROT))
R_world2cam = R_obj2cam * R_world2obj
cam_location = np.transpose(np.matrix((distance_ratio * CAM_MAX_DIST,
0,
0)))
T_world2cam = -1 * R_obj2cam * cam_location
# Step 3: Fix blender camera's y and z axis direction.
R_camfix = np.matrix(((1, 0, 0), (0, -1, 0), (0, 0, -1)))
R_world2cam = R_camfix * R_world2cam
T_world2cam = R_camfix * T_world2cam
RT = np.hstack((R_world2cam, T_world2cam))
return K, RT
def reset(self, initial_state=None):
if initial_state is None:
self.s = np.empty(self.n)
# position and velocity
limits = np.array([0.5, 0.5, 0.5, 1, 1, 1, 0, 0, 0, 0, 12, 12, 12])
self.s[0:6] = np.random.uniform(-limits[0:6], limits[0:6], 6)
# rotation
rpy = np.radians(
np.random.uniform(-self.rpy_limit, self.rpy_limit, 3))
q = rowan.from_euler(rpy[0], rpy[1], rpy[2], 'xyz')
self.s[6:10] = q
# angular velocity
self.s[10:13] = np.random.uniform(-limits[10:13], limits[10:13], 3)
else:
self.s = initial_state
self.time_step = 0
return np.array(self.s)
def reset(self, initial_state=None):
if initial_state is None:
self.s = np.empty(self.n)
# position and velocity
self.s[0:6] = np.random.uniform(-self.limits[0:6],
self.limits[0:6], 6)
# rotation
if self.rpy_limit is None:
q = rowan.random.random_sample()
else:
rpy = np.radians(
np.random.uniform(-self.rpy_limit, self.rpy_limit, 3))
q = rowan.from_euler(rpy[0], rpy[1], rpy[2], 'xyz')
if q[0] < 0:
q = -q
self.s[6:10] = q
# angular velocity
self.s[10:13] = np.random.uniform(-self.limits[10:13],
self.limits[10:13], 3)
else:
self.s = initial_state
self.time_step = 0
return np.array(self.s)
def test_radians():
fun = lambda x : 3.0 * np.radians(x)
d_fun = grad(fun)
check_grads(fun, 10.0*npr.rand())
check_grads(d_fun, 10.0*npr.rand())
def test_radians():
fun = lambda x : 3.0 * np.radians(x)
d_fun = grad(fun)
check_grads(fun, 10.0*npr.rand())
check_grads(d_fun, 10.0*npr.rand())
def test_radians():
fun = lambda x: 3.0 * np.radians(x)
check_grads(fun)(10.0 * npr.rand())
def test_radians():
fun = lambda x : 3.0 * np.radians(x)
check_grads(fun)(10.0*npr.rand())
