def f(self, x, uu):
u = np.clip(uu, -6, 6)
g = x[0:16].reshape((4, 4))
R, p = TransToRp(g)
omega = x[16:19]
v = x[19:]
twist = x[16:]
# u[0] *= 0.8
F = self.kt * (u[0] + u[1] + u[2] + u[3])
M = np.array([
self.kt * self.arm_length * (u[1] - u[3]),
self.kt * self.arm_length * (u[2] - u[0]),
self.km * (u[0] - u[1] + u[2] - u[3])
])
inertia_dot_omega = np.dot(self.inertia_matrix, omega)
inner_rot = M + cross(inertia_dot_omega, omega)
omegadot = np.dot(self.inv_inertia_matrix,
inner_rot) - self.ang_damping * omega
vdot = self.inv_mass * F * np.array([0., 0., 1.]) - cross(
omega, v) - 9.81 * np.dot(R.T, np.array([0., 0., 1.
])) - self.vel_damping * v
dg = np.dot(g, VecTose3(twist)).ravel()
return np.concatenate((dg, omegadot, vdot))
def look_at(self, world_position):
#%%
world_position = self.get_world_position()[:3]
eye = world_position
target = np.array([0, 0, 0])
up = np.array([0, 1, 0])
zaxis = (target - eye) / np.linalg.norm(target - eye)
xaxis = (np.cross(up, zaxis)) / np.linalg.norm(np.cross(up, zaxis))
yaxis = np.cross(zaxis, xaxis)
R = np.eye(4)
R = np.array([[xaxis[0], yaxis[0], zaxis[0], 0],
[xaxis[1], yaxis[1], zaxis[1], 0],
[xaxis[2], yaxis[2], zaxis[1], 0], [0, 0, 0, 1]])
R = np.array([[xaxis[0], xaxis[1], xaxis[2], 0],
[yaxis[0], yaxis[1], yaxis[2], 0],
[zaxis[0], zaxis[1], zaxis[2], 0], [0, 0, 0, 1]])
t = np.eye(4, dtype=np.float32) # translation
t[:3, 3] = -eye
self.R = R
self.Rt = np.dot(R, t)
self.t = np.eye(4, dtype=np.float32)
self.t[:3, 3] = self.Rt[:3, 3]
def calc_dihedral(coords3d, dihedral_ind, cos_tol=1e-9):
m, o, p, n = dihedral_ind
u_dash = coords3d[m] - coords3d[o]
v_dash = coords3d[n] - coords3d[p]
w_dash = coords3d[p] - coords3d[o]
u_norm = np.linalg.norm(u_dash)
v_norm = np.linalg.norm(v_dash)
w_norm = np.linalg.norm(w_dash)
u = u_dash / u_norm
v = v_dash / v_norm
w = w_dash / w_norm
phi_u = np.arccos(np.dot(u, w))
phi_v = np.arccos(-np.dot(w, v))
uxw = np.cross(u, w)
vxw = np.cross(v, w)
cos_dihed = np.dot(uxw, vxw) / (np.sin(phi_u) * np.sin(phi_v))
# Restrict cos_dihed to [-1, 1]
if cos_dihed >= 1 - cos_tol:
dihedral_rad = 0
elif cos_dihed <= -1 + cos_tol:
dihedral_rad = np.arccos(-1)
else:
dihedral_rad = np.arccos(cos_dihed)
if dihedral_rad != np.pi:
# wxv = np.cross(w, v)
# if wxv.dot(u) < 0:
if vxw.dot(u) < 0:
dihedral_rad *= -1
return dihedral_rad
def dihedrals(self):
d_angles = []
for quad in self.psf.dihedrals:
a_1, a_2, a_3, a_4 = quad.atom1, quad.atom2, quad.atom3, quad.atom4
v1 = np.array(self.positions[a_2]) - np.array(self.positions[a_1])
v2 = np.array(self.positions[a_3]) - np.array(self.positions[a_1])
n1 = np.cross(v1, v2)
v3 = np.array(self.positions[a_4]) - np.array(self.positions[a_3])
v4 = -v2
n2 = np.cross(v3, v4)
prod = np.dot(n1, n2)
if prod == 0:
angle = 0
else:
prod /= (np.linalg.norm(n1) * np.linalg.norm(n2))
prod = np.clip(prod, -1, 1)
angle = np.arccos(prod)
d_angles.append([(a_1.type, a_2.type, a_3.type, a_4.type),
np.degrees(angle)])
return d_angles
def look_at(self, world_position):
"""
:param world_position:
:return:
"""
cam_world_position = self.get_world_position() # used to set new cam.t
#self.set_R_mat(Rt_matrix_from_euler_t.R_matrix_from_euler_t(0.0, 0, 0)) # first we need to set new R
world_position = self.get_world_position()[:3]
eye = world_position
target = np.array([0, 0, 0])
up = np.array([0, 1, 0])
if np.linalg.norm(target - eye) == 0: # avoid cam position in [0,0,0]
zaxis = np.array([0., 0., 1.])
else:
zaxis = (target - eye) / np.linalg.norm(target - eye)
if zaxis[1] == -1.: # handle cam position on Y-axis
xaxis = np.array([-1., 0., 0.])
else:
xaxis = (np.cross(up, zaxis)) / np.linalg.norm(np.cross(up, zaxis))
yaxis = np.cross(zaxis, xaxis)
R = np.eye(4)
R = np.array([[xaxis[0], yaxis[0], zaxis[0], 0],
[xaxis[1], yaxis[1], zaxis[1], 0],
[xaxis[2], yaxis[2], zaxis[2], 0], [0, 0, 0, 1]])
# R = np.array([[xaxis[0], xaxis[1], xaxis[2], 0],
# [yaxis[0], yaxis[1], yaxis[2], 0],
# [zaxis[0], zaxis[1], zaxis[2], 0],
# [ 0, 0, 0, 1]])
R = R.T # R is rotation matrix from camera to world coordinate not from world to camera coordinate!!!
self.R = R
self.set_t(cam_world_position[0], cam_world_position[1],
cam_world_position[2], 'world') # !!! set new t
def getHOD_basic(self, a_norm, z_body, j_des, s_des):
a_norm_dot = j_des.dot(z_body)
z_body_dot = (j_des - a_norm_dot * z_body) / a_norm
theta_vel = np.cross(z_body, z_body_dot)
a_norm_ddot = s_des.dot(z_body) + j_des.dot(z_body_dot)
z_body_ddot = (s_des - a_norm_ddot * z_body -
2 * a_norm_dot * z_body_dot) / a_norm
theta_acc = np.cross(z_body, z_body_ddot)
return theta_vel, theta_acc
def dVelocityVectordh(self, x, mu):
"""
derivatives of Velocity vector wrt angular momentum vector
typed
"""
TA = x[5]
sTA = np.sin(TA)
cTA = np.cos(TA)
h = self.hVector(x)
e = self.eVector(x, mu)
eCross = self.mathUtil.crossmat(e)
hMag = np.linalg.norm(h)
eMag = np.linalg.norm(e)
eUnit = e / eMag
crossProduct = np.cross(h, e)
crossProductMag = np.linalg.norm(crossProduct)
factor1 = 1.0 / hMag**3
term1 = factor1 * (-np.outer(
sTA * eUnit - ((eMag + cTA) / crossProductMag) * crossProduct, h))
factor2 = (-(eMag + cTA)) / (hMag * crossProductMag)
term2 = factor2 * (-eCross + (1.0 / crossProductMag**2) * np.dot(
np.outer(crossProduct, crossProduct), eCross))
dvdh = -mu * (term1 + term2)
return dvdh
def f_scp(self, s, a):
# input:
# s, nd array, (n,)
# a, nd array, (m,1)
# output
# dsdt, nd array, (n,1)
dsdt = agnp.zeros(self.n)
q = s[6:10]
omega = s[10:]
# get input
a = agnp.reshape(a, (self.m, ))
eta = agnp.dot(self.B0, a)
f_u = agnp.array([0, 0, eta[0]])
tau_u = agnp.array([eta[1], eta[2], eta[3]])
# dynamics
# dot{p} = v
dpdt = s[3:6]
# mv = mg + R f_u
dvdt = self.g + qrotate(q, f_u) / self.mass
# dot{R} = R S(w)
# to integrate the dynamics, see
# https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/, and
# https://arxiv.org/pdf/1604.08139.pdf
qnew = qintegrate(q, omega, self.ave_dt)
qnew = qnormalize(qnew)
# transform qnew to a "delta q" that works with the usual euler integration
dqdt = (qnew - q) / self.ave_dt
# mJ = Jw x w + tau_u
dwdt = self.inv_J * (agnp.cross(self.J * omega, omega) + tau_u)
return agnp.concatenate((dpdt, dvdt, dqdt, dwdt))
def solve(self, angles0, target):
joint_indexes = list(reversed(self._fk_solver.joint_indexes))
angles = angles0[:]
pmap = {
'x': [1., 0., 0., 0.],
'y': [0., 1., 0., 0.],
'z': [0., 0., 1., 0.]
}
prev_dist = sys.float_info.max
for _ in range(self.maxiter):
for i, idx in enumerate(joint_indexes):
axis = self._fk_solver.solve(
angles,
p=pmap[self._fk_solver.components[idx].axis],
index=idx)
pj = self._fk_solver.solve(angles, index=idx)
ee = self._fk_solver.solve(angles)
eorp = self.p_on_rot_plane(ee, pj, axis)
torp = self.p_on_rot_plane(target, pj, axis)
ve = (eorp - pj) / np.linalg.norm(eorp - pj)
vt = (torp - pj) / np.linalg.norm(torp - pj)
a = np.arccos(np.dot(vt, ve))
sign = 1 if np.dot(axis, np.cross(ve, vt)) > 0 else -1
angles[-(i + 1)] += (a * sign)
ee = self._fk_solver.solve(angles)
dist = np.linalg.norm(target - ee)
delta = prev_dist - dist
if delta < self.tol:
break
prev_dist = dist
return angles
def tVector_derivs(self, x):
"""
derivatives of the T unit vector
typed
"""
# choose the reference vector here
k = self.mathUtil.k_unit()
referenceVector = k
dreferenceVectordx = np.zeros(
(3,
6)) # for k reference vector (or any constant reference vector)
s = self.sVector(x)
crossProduct = np.cross(s, referenceVector)
crossProductMag = np.linalg.norm(crossProduct)
dcrossProductds, dcrossProductdreferenceVector = self.mathUtil.d_crossproduct(
s, referenceVector)
dsdx = self.sVector_derivs(x)
dtdxNotUnit = np.dot(dcrossProductds, dsdx) + np.dot(
dcrossProductdreferenceVector, dreferenceVectordx)
dOneByCrossProductMagdx = (-1.0 / crossProductMag**3) * np.dot(
crossProduct, dtdxNotUnit)
dtdx = (1.0 / crossProductMag) * dtdxNotUnit + np.outer(
crossProduct, dOneByCrossProductMagdx)
return dtdx
def hVector(self, r, v):
"""
angular momentum vector
"""
h = np.cross(r, v)
return h
def rVector_derivs(self, x):
"""
bplane R unit vector derivatives
typed
"""
s = self.sVector(x)
t = self.tVector(x)
crossProduct = np.cross(s, t)
crossProductMag = np.linalg.norm(crossProduct)
dcrossProductds, dcrossProductdt = self.mathUtil.d_crossproduct(s, t)
dsdx = self.sVector_derivs(x)
dtdx = self.tVector_derivs(x)
dRdxNotUnit = np.dot(dcrossProductds, dsdx) + np.dot(
dcrossProductdt, dtdx)
dOneByCrossProductMagdx = (-1.0 / crossProductMag**3) * np.dot(
crossProduct, dRdxNotUnit)
dRdx = (1.0 / crossProductMag) * dRdxNotUnit + np.outer(
crossProduct, dOneByCrossProductMagdx)
#term1 = (1.0 / crossProductMag) * dRdxNotUnit
#term2 = np.outer(crossProduct, dOneByCrossProductMagdx)
#print('***')
#print('dRdx')
#print('crossProduct: ', crossProduct)
#print('dRdxNotUnit column1: ', dRdxNotUnit[0:3,1])
#print(crossProduct[0]*dRdxNotUnit[0,1], crossProduct[1]*dRdxNotUnit[1,1])
#print(crossProduct[2], dOneByCrossProductMagdx[1])
#print(dRdxNotUnit[2,1], term2[2,1])
#print('***')
return dRdx
def dVelocityVectorde(self, x, mu):
"""
derivatives of Velocity vector wrt eccentricity vector
typed
"""
TA = x[5]
sTA = np.sin(TA)
cTA = np.cos(TA)
h = self.hVector(x)
e = self.eVector(x, mu)
hCross = self.mathUtil.crossmat(h)
hMag = np.linalg.norm(h)
eMag = np.linalg.norm(e)
crossProduct = np.cross(h, e)
crossProductMag = np.linalg.norm(crossProduct)
term1 = sTA * (1.0 / eMag) * (np.identity(3) -
(1.0 / eMag**2) * np.outer(e, e))
term21 = np.outer((1.0 / crossProductMag) * crossProduct,
(1.0 / eMag) * e)
term22 = (eMag + cTA) * (1.0 / crossProductMag) * (
hCross - (1.0 / crossProductMag**2) *
np.dot(np.outer(crossProduct, crossProduct), hCross))
term2 = -(term21 + term22)
factor = -mu / hMag
dvde = factor * (term1 + term2)
return dvde
def dPositionVectordh(self, x, mu):
"""
derivatives of position vector wrt angular momentum vector
typed
"""
TA = x[5]
cTA = np.cos(TA)
sTA = np.sin(TA)
h = self.hVector(x)
e = self.eVector(x, mu)
hMag = self.hMag(x)
eMag = self.eMag(x, mu)
eUnit = e / eMag
crossProduct = np.cross(h, e)
crossProductMag = np.linalg.norm(crossProduct)
factor = 1.0 / (mu * (1.0 + eMag * cTA))
term1 = cTA * np.outer(eUnit, 2.0 * h)
#term2 = sTA * np.dot((1.0 / crossProductMag) * (np.identity(3) - (1.0 / crossProductMag**2) * np.outer(crossProduct, crossProduct)), -self.mathUtil.crossmat(e))
term2 = (sTA / crossProductMag) * (
2. * np.outer(crossProduct, h) + hMag**2 * np.dot(
(-np.identity(3) + (1. / crossProductMag**2) * np.outer(
crossProduct, crossProduct)), self.mathUtil.crossmat(e)))
drdh = factor * (term1 + term2)
return drdh
def rVectorNotUnit(self, r, v, mu):
"""
bplane R vector
"""
s = self.sVector(r, v, mu)
t = self.tVector(r, v, mu)
R = np.cross(s, t)
return R
def nVector(self, r, v, mu):
"""
not the node vector: h cross e; i think battin calls it p
"""
h = self.hVector(r, v)
e = self.eVector(r, v, mu)
n = np.cross(h, e)
return n
def rVectorNotUnit(self, x):
"""
bplane R vector
"""
s = self.sVector(x)
t = self.tVector(x)
R = np.cross(s, t)
return R
def hUnit(self, x):
"""
angular momentum unit vector
"""
b = self.bVector(x)
s = self.sVector(x)
hUnit = np.cross(b, s)
hUnit = hUnit / np.linalg.norm(hUnit)
return hUnit
def get_neighs(vertex, r):
verts = list()
# level_0
verts.append(vertex)
v = vertex
# find closest point in level 1
if sparse.issparse(adj_mtx):
nz = adj_mtx.tolil().rows
ix_list = nz[v]
else:
row = adj_mtx[v]
ix_list = np.nonzero(row)
ix_list = ix_list[0]
dists = []
for j in ix_list:
d = get_dist(coords, v, j)
dists.append(d)
ix_min = ix_list[dists.index(min(dists))]
closest_ix = ix_min
# levels_>=1
for i in range(1, r + 1):
# this is the closest vertex of the new level
# find the ordering of the level
arr = get_order(adj_mtx, coords, ix_list, closest_ix, verts)
verts = verts + arr
# get next level: for each in ix_list, get neighbors that are not in <verts>, then add them to the new list
next_list = []
for j in ix_list:
if sparse.issparse(adj_mtx):
new_row = nz[j]
else:
new_row = adj_mtx[j]
new_row = np.nonzero(new_row)
new_row = new_row[0]
for k in new_row:
if k not in verts:
next_list.append(k)
next_list = list(set(next_list))
# find starting point of next level using line eq
c1 = coords[vertex]
c2 = coords[closest_ix]
line_dists = []
for j in next_list:
c3 = coords[j]
line_dist = LA.norm(np.cross(c2 - c1, c1 - c3)) / LA.norm(
c2 - c1) # calculate distance to line
line_dists.append(line_dist)
ix_list = next_list
closest_ix = next_list[line_dists.index(min(line_dists))]
return verts
def tVectorNotUnit(self, x):
"""
T vector, not necessarily as a unit vector
"""
s = self.sVector(x)
k = self.mathUtil.k_unit()
#h = self.hVector(x)
referenceVector = k
t = np.cross(s, referenceVector)
return t
def calculate_normal(self, tri_ind_info, centroid, q):
# normal_xyz = np.zeros((h, w, 3))
# normal_sph = np.zeros((h, w, 2))
normal_xyz = {}
normal_sph = {}
for i in range(self.h):
for j in range(self.w):
normal_xyz[(i,j)] = 0
normal_sph[(i,j)] = 0
tri_ver = q[self.tri_mesh_data[tri_ind_info[i, j]-1, :],:]
a = tri_ver[0,:]
b = tri_ver[1,:]
c = tri_ver[2,:]
normal_xyz[(i,j)] = np.cross(a-b, b-c)/np.linalg.norm(np.cross(a-b, b-c))
if np.dot(np.mean(tri_ver, 0)-centroid, normal_xyz[(i,j)])<0:
normal_xyz[(i,j)] *= -1
normal_sph[(i,j)] = self.cart2sph(normal_xyz[(i,j)])
return normal_sph
def tVectorNotUnit(self, r, v, mu):
"""
t vector
"""
# reference vector is chosen here
k = self.mathUtil.k_unit()
referenceVector = k
h = self.hVector(r, v)
s = self.sVector(r, v, mu)
t = np.cross(s, referenceVector)
return t
def syncRot(T):
'''
returns the rotation matrix of the approximation of the projector created by proj_deformable_approx
:param T : the motion matrix calculated in PoseFromKpts_WP
:return : [R,C] the rotation matrix and the values of a sorte of norm that is do not really understand
'''
[_, L, Q] = proj_deformable_approx(np.transpose(T))
s = np.sign(L[0])
C = s * L[0]
R = np.zeros((3, 3))
R[0:2, 0:3] = s * np.transpose(Q)
R[2, 0:3] = np.cross(Q[0:3, 0], Q[0:3, 1])
return R, C
def dPositionVectorde(self, x, mu):
"""
derivatives of position vector wrt eccentricity vector
typed
"""
TA = x[5]
cTA = np.cos(TA)
sTA = np.sin(TA)
h = self.hVector(x)
e = self.eVector(x, mu)
hMag = self.hMag(x)
eMag = self.eMag(x, mu)
eUnit = e / eMag
crossProduct = np.cross(h, e)
crossProductMag = np.linalg.norm(crossProduct)
crossProductUnit = crossProduct / crossProductMag
factor = hMag**2 / mu
#term1_old = np.outer((-cTA / ((1.0 + eMag * cTA)**2)) * eUnit, eUnit * cTA + crossProduct / crossProductMag * sTA)
term1 = (-cTA / (1. + eMag * cTA)**2) * np.outer(
cTA * eUnit + sTA * crossProductUnit, eUnit)
factor2 = 1.0 / (1.0 + eMag * cTA)
#term2_old = factor2 * ((cTA / eMag) * (np.identity(3) - (1.0 / eMag**2) * np.outer(e, e)) + (sTA / crossProductMag) * np.dot((np.identity(3) - (1.0 / crossProductMag**2) * np.outer(crossProduct, crossProduct)), self.mathUtil.crossmat(h)))
term2 = factor2 * (
(cTA / eMag) * (np.identity(3) - (1. / eMag**2) * np.outer(e, e)) +
(sTA / crossProductMag) * np.dot(
(np.identity(3) - (1. / crossProductMag**2) * np.outer(
crossProduct, crossProduct)), self.mathUtil.crossmat(h)))
#term2 = term2_old
# print(term1_old)
# print("\n")
# print(term1)
# print("\n")
# print("\n")
# print("\n")
# print("\n")
# print(term2_old)
# print("\n")
# print(term2)
# print("\n")
# print(term3)
# print("\n")
drde = factor * (term1 + term2)
return drde
def compute_rotation_velocity_geometry_axes(self, points):
# Computes the effective velocity due to rotation at a set of points.
# Input: a Nx3 array of points
# Output: a Nx3 array of effective velocities
angular_velocity_vector_geometry_axes = np.array(
[-self.p, self.q,
-self.r]) # signs convert from body axes to geometry axes
angular_velocity_vector_geometry_axes = np.expand_dims(
angular_velocity_vector_geometry_axes, axis=0)
rotation_velocity_geometry_axes = np.cross(
angular_velocity_vector_geometry_axes, points, axis=1)
rotation_velocity_geometry_axes = -rotation_velocity_geometry_axes # negative sign, since we care about the velocity the WING SEES, not the velocity of the wing.
return rotation_velocity_geometry_axes
def initial_state(Xa, Xg):
""" estimate initial orientation and biases given a series of measurements
during which the IMU was stationary """
g = 9.81
bg = np.mean(Xg, axis=0)
a = np.mean(Xa, axis=0)
na = np.linalg.norm(a)
sa = g / na
a /= na
rot = np.cross(np.array([0, 0, -1.0]), a)
# print 'accel scale', sa
# print 'gyro bias', bg
# print 'initial orientation', rot
# print np.dot(so3.exp(rot), np.array([0, 0, -1.0])), a
return sa, bg, rot
def initial_state(Xa, Xg):
""" estimate initial orientation and biases given a series of measurements
during which the IMU was stationary """
g = 9.81
bg = np.mean(Xg, axis=0)
a = np.mean(Xa, axis=0)
na = np.linalg.norm(a)
sa = g / na
a /= na
rot = np.cross(np.array([0, 0, -1.0]), a)
# print 'accel scale', sa
# print 'gyro bias', bg
# print 'initial orientation', rot
# print np.dot(so3.exp(rot), np.array([0, 0, -1.0])), a
return sa, bg, rot
def hUnit_derivs(self, x):
"""
derivatives of angular momentum unit vector
typed
"""
b = self.bVector(x)
s = self.sVector(x)
crossProduct = np.cross(b, s)
crossProductMag = np.linalg.norm(crossProduct)
dcrossProductdb, dcrossProductds = self.mathUtil.d_crossproduct(b, s)
dbdx = self.bVector_derivs(x)
dsdx = self.sVector_derivs(x)
dhdxNotUnit = np.dot(dcrossProductdb, dbdx) + np.dot(
dcrossProductds, dsdx)
dOneByCrossProductMagdx = (-1.0 / crossProductMag**3) * np.dot(
crossProduct, dhdxNotUnit)
dhUnitdx = (1.0 / crossProductMag) * dhdxNotUnit + np.outer(
crossProduct, dOneByCrossProductMagdx)
return dhUnitdx
def get_ceq(x):
n = (x.shape[0] - 1) // 4
tk, xk, yk, thetak = split_state(x)
xk = np.concatenate(([start_x], xk, [goal_x]))
yk = np.concatenate(([start_y], yk, [goal_y]))
thetak = np.concatenate(([start_theta], thetak, [goal_theta]))
dx = xk[1:] - xk[0:-1]
dy = yk[1:] - yk[0:-1]
dk = np.array([dx, dy, np.zeros(n + 1)]).T
# constant curvature (trapezoidal collocation, see teb paper)
ceq = np.array([
np.cos(thetak[0:-1]) + np.cos(thetak[1:]),
np.sin(thetak[0:-1]) + np.sin(thetak[1:]),
np.zeros(n + 1)
]).T
ceq = np.fabs(np.cross(ceq, dk, axisa=1, axisb=1)[:, 2])
return ceq
def impropers(self):
i_angles = []
for quad in self.psf.impropers: # a_1 bonded to a_2, a_1 is central atom
a_1, a_2, a_3, a_4 = quad.atom1, quad.atom2, quad.atom3, quad.atom4
v1 = np.array(self.positions[a_3]) - np.array(self.positions[a_1])
v2 = np.array(self.positions[a_4]) - np.array(self.positions[a_1])
n = np.cross(v1, v2)
v3 = np.array(self.positions[a_2]) - np.array(self.positions[a_1])
prod = np.dot(n, v3)
if prod == 0:
angle = np.pi / 2
else:
prod /= (np.linalg.norm(n) * np.linalg.norm(v3))
prod = np.clip(prod, -1, 1)
angle = np.pi / 2 - np.arccos(prod)
i_angles.append([(a_1.type, a_2.type, a_3.type, a_4.type),
np.degrees(angle)])
return i_angles
def f(self, s, a):
# input:
# s, nd array, (n,)
# a, nd array, (m,1)
# output
# dsdt, nd array, (n,1)
dsdt = np.zeros(self.n)
q = s[6:10]
omega = s[10:]
# get input
a = np.reshape(a, (self.m, ))
eta = np.dot(self.B0, a)
f_u = np.array([0, 0, eta[0]])
tau_u = np.array([eta[1], eta[2], eta[3]])
# dynamics
# dot{p} = v
dsdt[0:3] = s[3:6]
# mv = mg + R f_u
dsdt[3:6] = self.g + rowan.rotate(q, f_u) / self.mass
# dot{R} = R S(w)
# to integrate the dynamics, see
# https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/, and
# https://arxiv.org/pdf/1604.08139.pdf
qnew = rowan.calculus.integrate(q, omega, self.ave_dt)
qnew = rowan.normalize(qnew)
if qnew[0] < 0:
qnew = -qnew
# transform qnew to a "delta q" that works with the usual euler integration
dsdt[6:10] = (qnew - q) / self.ave_dt
# mJ = Jw x w + tau_u
dsdt[10:] = self.inv_J * (np.cross(self.J * omega, omega) + tau_u)
return dsdt.reshape((len(dsdt), 1))
def __init__(self, NSIDE, npix, clv=True):
"""
Args:
NSIDE (int) : the healpix NSIDE parameter, must be a power of 2, less than 2**30
npix (int) : number of pixel in the X and Y axis of the final projected map
rot_velocity (float) : rotation velocity of the star in the equator in km/s
Returns:
None
"""
self.NSIDE = int(NSIDE)
self.npix = int(npix)
self.hp_npix = hp.nside2npix(NSIDE)
# self.rot_velocity = rot_velocity
self.clv = clv
# Generate the indices of all healpix pixels
self.indices = np.arange(hp.nside2npix(NSIDE), dtype='int')
self.n_healpix_pxl = len(self.indices)
# Define the orthographic projector that generates the maps of the star on the plane of the sky
self.projector = hp.projector.OrthographicProj(xsize=int(self.npix))
# This function returns the pixel associated with a vector (x,y,z). This is needed by the projector
self.f_vec2pix = lambda x, y, z: hp.pixelfunc.vec2pix(int(self.NSIDE), x, y, z)
# Generate a mesh grid of X and Y points in the plane of the sky that covers only the observed hemisphere of the star
x = np.linspace(-2.0,0.0,int(self.npix/2))
y = np.linspace(-1.0,1.0,int(self.npix/2))
X, Y = np.meshgrid(x,y)
# Rotational velocity vector (pointing in the z direction and unit vector)
omega = np.array([0,0,1])
# Compute the radial vector at each position in the map and the projected velocity on the plane of the sky
radial_vector = np.array(self.projector.xy2vec(X.flatten(), Y.flatten())).reshape((3,int(self.npix/2),int(self.npix/2)))
self.vel_projection = np.cross(omega[:,None,None], radial_vector, axisa=0, axisb=0)[:,:,0]
# Compute the mu angle (astrocentric angle)
self.mu_angle = radial_vector[0,:,:]
# Read all Kurucz models from the database. Hardwired temperature and mu angles
print("Reading Kurucz spectra...")
self.T = 3500 + 250 * np.arange(27)
self.mus = np.array([1.0,0.9,0.8,0.7,0.6,0.5,0.4,0.3,0.2,0.1,0.05,0.02])[::-1]
for i in tqdm(range(27)):
f = 'kurucz_models/RESULTS/T_{0:d}_logg4.0_feh0.0.spec'.format(self.T[i])
vel, _, spec = _read_kurucz_spec(f)
if (i == 0):
self.nlambda, self.nmus = spec.shape
self.velocity = np.zeros((self.nlambda))
self.spectrum = np.zeros((27,self.nmus,self.nlambda))
self.velocity = vel
self.spectrum[i,:,:] = spec[:,::-1].T
# Generate a fake temperature map in the star using spherical harmonics
# self.temperature_map = 5000 * np.ones(self.npix)
# self.temperature_map = 5000 + 250 * hp.sphtfunc.alm2map(np.ones(10,dtype='complex'),self.NSIDE) #np.random.rand(self.n_healpix_pxl) * 2000 + 5000 #
self.temperature_map = 5000 * np.ones(self.hp_npix)
self.coeffs = hp.sphtfunc.map2alm(self.temperature_map)
self.velocity_per_pxl = self.velocity[1] - self.velocity[0]
self.freq_grid = np.fft.fftfreq(self.nlambda)
self.gradient = value_and_grad(self.loss)
