def predict_state(self):
"""predict quaternion orientation. Use a First order finite difference
for integration of sensor motion. The integration input is the
gyroscope signal"""
qDelta = qt.angular_rate_to_quaternion_rotation(self.wm, self.dt)
self.o = qt.quaternion_product(self.o, qDelta)
if self.verbose > 2:
print("PRINT NEW STATE ###")
self.print_state(self.o)
def euler_from_quat(q):
qxprime = q * Quaternions.Quaternion(0,1,0,0) * q.conjugate()
qyprime = q * Quaternions.Quaternion(0,0,1,0) * q.conjugate()
qzprime = q * Quaternions.Quaternion(0,0,0,1) * q.conjugate()
x = np.array([1,0,0]); y = np.array([0,1,0]); z = np.array([0,0,1]);
xprime = np.array([qxprime[1],qxprime[2],qxprime[3]])
yprime = np.array([qyprime[1],qyprime[2],qyprime[3]])
zprime = np.array([qzprime[1],qzprime[2],qzprime[3]])
alpha = np.arctan2(np.dot(zprime, y), np.dot(zprime, x)) #beware the reversal of arguments
if np.dot(zprime, z) > 1: # protection against numerical noise that can lead to values off-range by a tiny amount
beta = 0.
elif np.dot(zprime, z) < -1:
beta = pi
else:
beta = np.arccos(np.dot(zprime, z))
gamma = np.arctan2(np.dot(yprime, z), -np.dot(xprime, z)) #beware the reversal of arguments
return np.array([alpha, beta, gamma])
def get_crp(c):
if not (c.shape == (3, 3)):
print("Error. DCM matrix must be of shape (3, 3)")
return -1
b = Quaternions.get_quaternions(c)
q = np.zeros((3,1))
for i in range(q.size):
q[i][0] = b[i + 1][0] / b[0][0]
print(q)
return q
def __init__(self, ReadFromArduino_instance, dt, offset_GYRO, verbose=0):
self.dt = dt
self.verbose = verbose
self.ReadFromArduino_instance = ReadFromArduino_instance
self.list_measurements = []
self.offset_GYRO = offset_GYRO
# initial state: assume resting, Z downwards, still --------------------
# no rotation to Earth referential
self.o = qt.normalise_quaternion(
qt.Quaternion(1, 0.0001, 0.0001, 0.0001))
# reference vectors; can be used to print assumed orientation ----------
self.X_IMU_ref_IMU = qt.Vector(1, 0, 0)
self.Y_IMU_ref_IMU = qt.Vector(0, 1, 0)
self.Z_IMU_ref_IMU = qt.Vector(0, 0, 1)
self.read_and_update_measurement()
self.cycle = 0
def print_state(self, o):
"""Print information about state of the quaternion"""
current_X_IMU = qt.apply_rotation_on_vector(o, self.X_IMU_ref_IMU)
current_Y_IMU = qt.apply_rotation_on_vector(o, self.Y_IMU_ref_IMU)
current_Z_IMU = qt.apply_rotation_on_vector(o, self.Z_IMU_ref_IMU)
print(
"Print state of quaternion --------------------------------------------"
)
print(
"state orientation: current X, Y, Z of the IMU, in referential Earth:"
)
qt.print_vector(current_X_IMU)
qt.print_vector(current_Y_IMU)
qt.print_vector(current_Z_IMU)
return (current_X_IMU, current_Y_IMU, current_Z_IMU)
def read_and_update_measurement(self):
"""Read and update measurement from Arduino."""
self.ReadFromArduino_instance.read_one_value()
self.latest_measurement = self.ReadFromArduino_instance.latest_values
self.wm = qt.Vector(self.latest_measurement[3] - self.offset_GYRO[0],
self.latest_measurement[4] - self.offset_GYRO[1],
self.latest_measurement[5] - self.offset_GYRO[2])
if self.verbose > 2:
print("Obtained measurements in SI:")
print(self.latest_measurement)
self.list_measurements.append(self.latest_measurement)
def ik_t(anim, joint_names, target_joint, target_pos, order, fk, N = 300, no_change = [], rate = 0.0001):
curr_eulers = Quaternions(anim.rotations.qs[0]).euler(order = order)
ret_eulers = []
for i in range(N):
for k in range(len(target_joint)):
ret_eulers.append(np.copy(curr_eulers))
j, l , ip = build_jacobian(anim, joint_names, target_joint[k], target_pos[k], curr_eulers, order, fk, mode = "analytical")
de = target_pos[k] - np.array(ip)
shifts = [email protected]
shifts = np.array([shifts]).reshape((len(shifts)//3, 3))
curr_eulers = modify_eulers(curr_eulers, shifts, l, no_change = no_change, rate = rate)
anim = add_frame(anim, np.array([Quaternions.from_euler(curr_eulers, order = order[::-1]).qs]), order)
ret_eulers.append(curr_eulers)
return anim, np.array(ret_eulers)
def perform_one_iteration(self):
"""Perform one integration iteration: read, integrate to compute update,
and print if necessary."""
if self.verbose > 0:
print("\n### NEW CYCLE " + str(self.cycle) + " ###\n")
self.predict_state()
self.read_and_update_measurement()
# normalise orientation quaternion: this can be necessary if some noise
# due to for exemple nearly singular matrix that get inverted
self.o = qt.normalise_quaternion(self.o)
if self.verbose > 0:
print("PRINT STATE AT END INTEGRATION CYCLE ###")
self.print_state(self.o)
self.cycle += 1
def rotateQ(self, q):
"""Apply rotation described by quaternion q to this 3D point"""
v_rotated = qt.apply_rotation_on_vector(q, self.v)
return Point3D(v_rotated.vx, v_rotated.vy, v_rotated.vz)
def Hybridize(PathToSystem, Waveform='rhOverM_Asymptotic_GeometricUnits.h5/Extrapolated_N2.dir',
t1=1000.0, t2=-sys.float_info.max, InitialOmega_orb=0.0, DirectAlignmentEvaluations=0, Approximant='TaylorT1',
PlotFileName='Hybridization.pdf', HybridFileName='Hybrid.h5', MinStepsPerOrbit=32, PNWaveformModeOrder=3.5, PNOrbitalEvolutionOrder=4.0):
print("Reading and transforming NR data"); sys.stdout.flush()
# Read the Waveform from the file, and transform to co-rotating frame
W_NR_corot = GWFrames.ReadFromNRAR(os.path.join(PathToSystem, Waveform))
W_NR_corot.TransformToCorotatingFrame();
# Place the merger (moment of greatest waveform norm) at t=0, as
# the only generally meaningful time.
t0 = -W_NR_corot.MaxNormTime()
W_NR_corot.SetT(W_NR_corot.T()+t0)
t1 = t1 + t0
if(t2==-sys.float_info.max):
t2 = 0.75*t1
else:
t2 = t2 + t0
print("Reading and analyzing Horizons.h5 data"); sys.stdout.flush()
# Get the BH coordinate data from the h5 file
with h5py.File(os.path.join(PathToSystem,'Horizons.h5'), 'r') as f:
tA = f['AhA.dir/CoordCenterInertial.dat'][:,0]+t0
xA = f['AhA.dir/CoordCenterInertial.dat'][:,1:]
mA = f['AhA.dir/ArealMass.dat'][:,1]
chiA = f['AhA.dir/chiInertial.dat'][:,1:]
tB = f['AhB.dir/CoordCenterInertial.dat'][:,0]+t0
xB = f['AhB.dir/CoordCenterInertial.dat'][:,1:]
mB = f['AhB.dir/ArealMass.dat'][:,1]
chiB = f['AhB.dir/chiInertial.dat'][:,1:]
# Some calculations with the BH coordinates
d = xA-xB
nHat = Quaternions.normalized(Quaternions.QuaternionArray(d))
dnHatdt = Quaternions.QuaternionDerivative(nHat, tA)
lambdaHat = Quaternions.normalized(dnHatdt)
Omega = np.array([np.cross(n.vec(),ndot.vec()) for n,ndot in zip(nHat,dnHatdt)])
# Take initial data to be simply the coordinate quantities at the relaxation time
i_1 = abs(tA-t1).argmin()
t_i = tA[i_1]
ma = mA[i_1]
mb = mB[i_1]
delta = (ma-mb)/(ma+mb)
chia_0 = chiA[i_1]
chib_0 = chiB[i_1]
Omega_orb_0 = np.sqrt(Omega[i_1,0]**2+Omega[i_1,1]**2+Omega[i_1,2]**2)
R_frame_i = Quaternions.FrameFromXY([nHat[i_1],],[lambdaHat[i_1],])[0]
print("Constructing PN data"); sys.stdout.flush()
# Construct complete PN waveform
if(InitialOmega_orb==0.0):
InitialOmega_orb = 0.5*Omega_orb_0
W_PN_corot = GWFrames.PNWaveform(Approximant, delta, chia_0, chib_0, Omega_orb_0, InitialOmega_orb, R_frame_i,
MinStepsPerOrbit, PNWaveformModeOrder, PNOrbitalEvolutionOrder)
W_PN_corot.SetT(W_PN_corot.T()+t_i);
W_PN_corot.TransformToCorotatingFrame();
print("Aligning PN and NR waveforms"); sys.stdout.flush()
# Minimize the distance between the PN and NR rotors in their
# co-rotating frames. Note that NR is kept fixed here, because
# its merger time is set to t=0.0, which is the only physically
# meaningful fiducial quantity present. That quality will
# therefore also be present in the output waveform.
GWFrames.AlignWaveforms(W_NR_corot, W_PN_corot, t1, t2, DirectAlignmentEvaluations)
print("Constructing PN-NR hybrid"); sys.stdout.flush()
# Construct the hybrid
W_hyb_corot = GWFrames.Waveform(W_PN_corot.Hybridize(W_NR_corot, t1, t2))
# Optionally, make a plot showing the results. It is strongly
# recommended that you look at this plot to make sure things worked
# alright.
if(PlotFileName):
print("Plotting to {0}".format(os.path.join(PathToSystem, PlotFileName))); sys.stdout.flush()
plt.axvline(t1, linestyle='dotted')
plt.axvline(t2, linestyle='dotted')
plt.title(r'$(2,2)$ modes of the component waveforms')
W_hyb_corot.plot('LogAbs', [2,2], lw=3, label='Hybrid')
W_PN_corot.plot('LogAbs', [2,2], label='PN')
W_NR_corot.plot('LogAbs', [2,2], label='NR')
plt.ylim((1e-2,1.))
plt.xlim((W_NR_corot.T(0), W_NR_corot.T()[-1]))
plt.legend(loc='upper left')
try:
tight_layout(pad=0.1)
except:
pass
plt.savefig(os.path.join(PathToSystem, PlotFileName))
print("Transforming hybrid to inertial frame and outputting to {0}".format(os.path.join(PathToSystem, HybridFileName))); sys.stdout.flush()
W_hyb_corot.TransformToInertialFrame()
W_hyb_corot.OutputToNRAR(os.path.join(PathToSystem, HybridFileName))
print("All done"); sys.stdout.flush()
def Cal_angle(scale, data):
# Scale is a dictionary, containing the length information
# data shape: (7 * N) * 9 list
height = 180.0 # scale['height']
Upper_leg_length = 0.232 * height # scale['Upper_leg_length']
Lower_leg_length = 0.247 * height # scale['Lower_leg_length']
foot_size = 0.05 * height # scale['foot_size']
width = 0.203 * height # scale['width']
# Read the data
data = np.array(data)
m = data.shape[0]
l = m / 7
K = np.delete(data, [1, 2, 3, 8], 1)
# Initial pose
O1 = np.array([0, 0, 0])
O1_q0 = np.array([np.sqrt(2) / 2, 0, 0, -np.sqrt(2) / 2])
O2 = np.array([0, 0.5 * width, -0.25 * width])
O2_q0 = np.array([np.sqrt(2) / 2, 0, 0, -np.sqrt(2) / 2])
O1O2_ini = np.array([0, 0, -0.5 * width, -0.25 * width])
O3 = np.array([0, 0.5 * width, -Upper_leg_length])
O3_q0 = np.array([np.sqrt(2) / 2, 0, 0, -np.sqrt(2) / 2])
O2O3_ini = np.array([0, 0, 0, -Upper_leg_length])
O4 = np.array([0, 0.5 * width, -(Upper_leg_length + Lower_leg_length)])
O4_q0 = np.array([np.sqrt(2) / 2, 0, 0, -np.sqrt(2) / 2])
O3O4_ini = np.array([0, 0, 0, -Lower_leg_length])
O5 = np.array(
[foot_size, 0.5 * width, -(Upper_leg_length + Lower_leg_length)])
O4O5_ini = np.array([0, foot_size, 0, 0])
O6 = np.array([0, -0.5 * width, -0.25 * width])
O6_q0 = np.array([np.sqrt(2) / 2, 0, 0, -np.sqrt(2) / 2])
O1O6_ini = np.array([0, 0, 0.5 * width, -0.25 * width])
O7 = np.array([0, -0.5 * width, -Upper_leg_length])
O7_q0 = np.array([np.sqrt(2) / 2, 0, 0, -np.sqrt(2) / 2])
O6O7_ini = np.array([0, 0, 0, -Upper_leg_length])
O8 = np.array([0, -0.5 * width, -(Upper_leg_length + Lower_leg_length)])
O8_q0 = np.array([np.sqrt(2) / 2, 0, 0, -np.sqrt(2) / 2])
O7O8_ini = np.array([0, 0, 0, -Lower_leg_length])
O9 = np.array(
[foot_size, -0.5 * width, -(Upper_leg_length + Lower_leg_length)])
O8O9_ini = np.array([0, foot_size, 0, 0])
# Calibration
L = K[0:35, 1:5]
H = np.zeros([5, 4])
J = np.zeros([7, 5, 4])
weights = np.ones([5, 1]) / 5
for i in range(7):
for j in range(5):
H[j, :] = L[i + j * 7, :]
J[i, j, :] = L[i + j * 7, :]
J = list(J)
Q1_mast0_ave = Qu.quatWAvgMarkley(J[3], weights)
Q1_bias = Qu.quatmultiply(Qu.quatinv(Q1_mast0_ave.T), O1_q0)
Q2_mast0_ave = Qu.quatWAvgMarkley(J[4], weights)
Q2_bias = Qu.quatmultiply(Qu.quatinv(Q2_mast0_ave.T), O2_q0)
Q3_mast0_ave = Qu.quatWAvgMarkley(J[5], weights)
Q3_bias = Qu.quatmultiply(Qu.quatinv(Q3_mast0_ave.T), O3_q0)
Q4_mast0_ave = Qu.quatWAvgMarkley(J[6], weights)
Q4_bias = Qu.quatmultiply(Qu.quatinv(Q4_mast0_ave.T), O4_q0)
Q6_mast0_ave = Qu.quatWAvgMarkley(J[2], weights)
Q6_bias = Qu.quatmultiply(Qu.quatinv(Q6_mast0_ave.T), O6_q0)
Q7_mast0_ave = Qu.quatWAvgMarkley(J[1], weights)
Q7_bias = Qu.quatmultiply(Qu.quatinv(Q7_mast0_ave.T), O7_q0)
Q8_mast0_ave = Qu.quatWAvgMarkley(J[0], weights)
Q8_bias = Qu.quatmultiply(Qu.quatinv(Q8_mast0_ave.T), O8_q0)
# Calculation
Q1_mast = []
Q2_mast = []
Q3_mast = []
Q4_mast = []
Q6_mast = []
Q7_mast = []
Q8_mast = []
for i in range(m):
if K[i, 0] == 3:
Q1_mast.append(K[i, 1:5])
elif K[i, 0] == 4:
Q2_mast.append(K[i, 1:5])
elif K[i, 0] == 5:
Q3_mast.append(K[i, 1:5])
elif K[i, 0] == 6:
Q4_mast.append(K[i, 1:5])
elif K[i, 0] == 2:
Q6_mast.append(K[i, 1:5])
elif K[i, 0] == 1:
Q7_mast.append(K[i, 1:5])
elif K[i, 0] == 0:
Q8_mast.append(K[i, 1:5])
Q1_mast = np.array(Q1_mast)
Q2_mast = np.array(Q2_mast)
Q3_mast = np.array(Q3_mast)
Q4_mast = np.array(Q4_mast)
Q6_mast = np.array(Q6_mast)
Q7_mast = np.array(Q7_mast)
Q8_mast = np.array(Q8_mast)
PPP = np.zeros([l, 8, 6])
Angle = np.zeros([l, 14])
for i in range(l):
O1_qt = Qu.quatmultiply(Q1_mast[i, :], Q1_bias)[0, :]
O2_qt = Qu.quatmultiply(Q2_mast[i, :], Q2_bias)[0, :]
O1O2 = Qu.quatmultiply(Qu.quatmultiply(O1_qt, O1O2_ini),
Qu.quatinv(O1_qt))[0, :]
P_O2 = O1O2[1:4]
O3_qt = Qu.quatmultiply(Q3_mast[i, :], Q3_bias)[0, :]
O3_t = O1O2 + Qu.quatmultiply(Qu.quatmultiply(O2_qt, O2O3_ini),
Qu.quatinv(O2_qt))[0, :]
O2O3 = Qu.quatmultiply(Qu.quatmultiply(O2_qt, O2O3_ini),
Qu.quatinv(O2_qt))[0, :]
P_O3 = O3_t[1:4]
O4_qt = Qu.quatmultiply(Q4_mast[i, :], Q4_bias)[0, :]
O4_t = O3_t + Qu.quatmultiply(Qu.quatmultiply(O3_qt, O3O4_ini),
Qu.quatinv(O3_qt))[0, :]
O3O4 = Qu.quatmultiply(Qu.quatmultiply(O3_qt, O3O4_ini),
Qu.quatinv(O3_qt))[0, :]
P_O4 = O4_t[1:4]
# angle_knee1 = acos(dot(O2O3(2:4), O3O4(2:4)) / (norm(O2O3(2:4))*norm(O3O4(2:4))))*(180 / pi);
q_knee1 = Qu.quatmultiply(Qu.quatinv(O3_qt), O2_qt)[0, :]
angle_knee1_new = abs(
np.arcsin(2 * (q_knee1[0] * q_knee1[2] - q_knee1[1] * q_knee1[3])))
O5_t = O4_t + Qu.quatmultiply(Qu.quatmultiply(O4_qt, O4O5_ini),
Qu.quatinv(O4_qt))[0, :]
O4O5 = Qu.quatmultiply(Qu.quatmultiply(O4_qt, O4O5_ini),
Qu.quatinv(O4_qt))[0, :]
P_O5 = O5_t[1:4]
# angle_foot1 = acos(dot(O3O4(2:4), O4O5(2:4)) / (norm(O3O4(2:4))*norm(O4O5(2:4))))*(180 / pi);
O6_qt = Qu.quatmultiply(Q6_mast[i, :], Q6_bias)[0, :]
O1O6 = Qu.quatmultiply(Qu.quatmultiply(O1_qt, O1O6_ini),
Qu.quatinv(O1_qt))[0, :]
P_O6 = O1O6[1:4]
O7_qt = Qu.quatmultiply(Q7_mast[i, :], Q7_bias)[0, :]
O7_t = O1O6 + Qu.quatmultiply(Qu.quatmultiply(O6_qt, O6O7_ini),
Qu.quatinv(O6_qt))[0, :]
O6O7 = Qu.quatmultiply(Qu.quatmultiply(O6_qt, O6O7_ini),
Qu.quatinv(O6_qt))[0, :]
P_O7 = O7_t[1:4]
O8_qt = Qu.quatmultiply(Q8_mast[i, :], Q8_bias)[0, :]
O8_t = O7_t + Qu.quatmultiply(Qu.quatmultiply(O7_qt, O7O8_ini),
Qu.quatinv(O7_qt))[0, :]
O7O8 = Qu.quatmultiply(Qu.quatmultiply(O7_qt, O7O8_ini),
Qu.quatinv(O7_qt))[0, :]
P_O8 = O8_t[1:4]
q_knee2 = Qu.quatmultiply(Qu.quatinv(O7_qt), O6_qt)[0, :]
angle_knee2_new = np.arcsin(
2 * (q_knee2[0] * q_knee2[2] - q_knee2[1] * q_knee2[3]))
O9_t = O8_t + Qu.quatmultiply(Qu.quatmultiply(O8_qt, O8O9_ini),
Qu.quatinv(O8_qt))[0, :]
P_O9 = O9_t[1:4]
q_up1 = Qu.quatmultiply(Qu.quatinv(O2_qt), O1_qt)[0, :]
angle_up1_a = np.arctan2(
2 * (q_up1[0] * q_up1[1] + q_up1[2] * q_up1[3]),
1 - 2 * (np.square(q_up1[1]) + np.square(q_up1[2])))
angle_up1_b = np.arcsin(2 *
(q_up1[0] * q_up1[2] - q_up1[1] * q_up1[3]))
angle_up1_c = np.arctan2(
2 * (q_up1[0] * q_up1[3] + q_up1[1] * q_up1[2]),
1 - 2 * (np.square(q_up1[2]) + np.square(q_up1[3])))
q_knee1 = Qu.quatmultiply(Qu.quatinv(O3_qt), O2_qt)[0, :]
angle_knee1_new = abs(
np.arcsin(2 * (q_knee1[0] * q_knee1[2] - q_knee1[1] * q_knee1[3])))
q_ft1 = Qu.quatmultiply(Qu.quatinv(O4_qt), O3_qt)[0, :]
angle_ft1_a = np.arctan2(
2 * (q_ft1[0] * q_ft1[1] + q_ft1[2] * q_ft1[3]),
1 - 2 * (np.square(q_ft1[1]) + np.square(q_ft1[2])))
angle_ft1_b = np.arcsin(2 *
(q_ft1[0] * q_ft1[2] - q_ft1[1] * q_ft1[3]))
angle_ft1_c = np.arctan2(
2 * (q_ft1[0] * q_ft1[3] + q_ft1[1] * q_ft1[2]),
1 - 2 * (np.square(q_ft1[2]) + np.square(q_ft1[3])))
q_up2 = Qu.quatmultiply(Qu.quatinv(O6_qt), O1_qt)[0, :]
angle_up2_a = np.arctan2(
2 * (q_up2[0] * q_up2[1] + q_up2[2] * q_up2[3]),
1 - 2 * (np.square(q_up2[1]) + np.square(q_up2[2])))
angle_up2_b = np.arcsin(2 *
(q_up2[0] * q_up2[2] - q_up2[1] * q_up2[3]))
angle_up2_c = np.arctan2(
2 * (q_up2[0] * q_up2[3] + q_up2[1] * q_up2[2]),
1 - 2 * (np.square(q_up2[2]) + np.square(q_up2[3])))
q_knee2 = Qu.quatmultiply(Qu.quatinv(O7_qt), O6_qt)[0, :]
angle_knee2_new = np.arcsin(
2 * (q_knee2[0] * q_knee2[2] - q_knee2[1] * q_knee2[3]))
q_ft2 = Qu.quatmultiply(Qu.quatinv(O8_qt), O7_qt)[0, :]
angle_ft2_a = np.arctan2(
2 * (q_ft2[0] * q_ft2[1] + q_ft2[2] * q_ft2[3]),
1 - 2 * (np.square(q_ft2[1]) + np.square(q_ft2[2])))
angle_ft2_b = np.arcsin(2 *
(q_ft2[0] * q_ft2[2] - q_ft2[1] * q_ft2[3]))
angle_ft2_c = np.arctan2(
2 * (q_ft2[0] * q_ft2[3] + q_ft2[1] * q_ft2[2]),
1 - 2 * (np.square(q_ft2[2]) + np.square(q_ft2[3])))
PPP[i, 0, 0] = P_O2[0]
PPP[i, 0, 1] = P_O2[1]
PPP[i, 0, 2] = P_O2[2]
PPP[i, 1, 0] = P_O3[0]
PPP[i, 1, 1] = P_O3[1]
PPP[i, 1, 2] = P_O3[2]
PPP[i, 1, 3] = Angle[i, 0] = angle_up1_a
PPP[i, 1, 4] = Angle[i, 1] = angle_up1_b
PPP[i, 1, 5] = Angle[i, 2] = angle_up1_c
PPP[i, 2, 0] = P_O4[0]
PPP[i, 2, 1] = P_O4[1]
PPP[i, 2, 2] = P_O4[2]
PPP[i, 2, 3] = Angle[i, 3] = angle_knee1_new
PPP[i, 3, 0] = P_O5[0]
PPP[i, 3, 1] = P_O5[1]
PPP[i, 3, 2] = P_O5[2]
PPP[i, 3, 3] = Angle[i, 4] = angle_ft1_a
PPP[i, 3, 4] = Angle[i, 5] = angle_ft1_b - np.pi * 90 / 180
PPP[i, 3, 5] = Angle[i, 6] = angle_ft1_c
PPP[i, 4, 0] = P_O6[0]
PPP[i, 4, 1] = P_O6[1]
PPP[i, 4, 2] = P_O6[2]
PPP[i, 5, 0] = P_O7[0]
PPP[i, 5, 1] = P_O7[1]
PPP[i, 5, 2] = P_O7[2]
PPP[i, 5, 3] = Angle[i, 7] = angle_up2_a
PPP[i, 5, 4] = Angle[i, 8] = angle_up2_b
PPP[i, 5, 5] = Angle[i, 9] = angle_up2_c
PPP[i, 6, 0] = P_O8[0]
PPP[i, 6, 1] = P_O8[1]
PPP[i, 6, 2] = P_O8[2]
PPP[i, 6, 3] = Angle[i, 10] = angle_knee2_new
PPP[i, 7, 0] = P_O9[0]
PPP[i, 7, 1] = P_O9[1]
PPP[i, 7, 2] = P_O9[2]
PPP[i, 7, 3] = Angle[i, 11] = angle_ft2_a
PPP[i, 7, 4] = Angle[i, 12] = angle_ft2_b - np.pi * 90 / 180
PPP[i, 7, 5] = Angle[i, 13] = angle_ft2_c
return Angle
def Vf_from_quat(q):
qvf = q * Quaternions.Quaternion(0,0,0,1) * q.conjugate()
return np.array([qvf[1],qvf[2],qvf[3]])
def get_coord(l, q, o):
ret = o[l[0]]
for i in l[1:]:
ret = Quaternions(q[i]) * ret
ret = ret + o[i]
return ret
def Hybridize(
PathToSystem,
Waveform='rhOverM_Asymptotic_GeometricUnits.h5/Extrapolated_N2.dir',
t1=200.0,
t2=-sys.float_info.max,
InitialOmega_orb=0.0,
DirectAlignmentEvaluations=0,
Approximant='TaylorT1',
PlotFileName='Hybridization.pdf',
HybridFileName='Hybrid.h5',
MinStepsPerOrbit=32,
PNWaveformModeOrder=3.5,
PNOrbitalEvolutionOrder=4.0,
Debug=False):
print("Reading and transforming NR data")
sys.stdout.flush()
# Read the Waveform from the file, and transform to co-rotating frame
W_NR_corot = GWFrames.ReadFromNRAR(os.path.join(PathToSystem, Waveform))
W_NR_corot.DropTimesOutside(0.0, W_NR_corot.T(W_NR_corot.NTimes() - 1))
if Debug:
W_NR_orig = GWFrames.Waveform(W_NR_corot)
W_NR_corot.TransformToCorotatingFrame()
# Place the merger (moment of greatest waveform norm) at t=0, as
# the only generally meaningful time.
t0 = -W_NR_corot.MaxNormTime()
W_NR_corot.SetT(W_NR_corot.T() + t0)
if Debug:
W_NR_orig.SetT(W_NR_orig.T() + t0)
t1 = t1 + t0
if (t2 == -sys.float_info.max):
t2 = 0.75 * t1
else:
t2 = t2 + t0
print("Reading and analyzing Horizons.h5 data")
sys.stdout.flush()
# Get the BH coordinate data from the h5 file
with h5py.File(os.path.join(PathToSystem, 'Horizons.h5'), 'r') as f:
tA = f['AhA.dir/CoordCenterInertial.dat'][:, 0] + t0
xA = f['AhA.dir/CoordCenterInertial.dat'][:, 1:]
mA = f['AhA.dir/ArealMass.dat'][:, 1]
chiA = f['AhA.dir/chiInertial.dat'][:, 1:]
tB = f['AhB.dir/CoordCenterInertial.dat'][:, 0] + t0
xB = f['AhB.dir/CoordCenterInertial.dat'][:, 1:]
mB = f['AhB.dir/ArealMass.dat'][:, 1]
chiB = f['AhB.dir/chiInertial.dat'][:, 1:]
# Some calculations with the BH coordinates
d = xA - xB
nHat = Quaternions.normalized(Quaternions.QuaternionArray(d))
dnHatdt = Quaternions.QuaternionDerivative(nHat, tA)
lambdaHat = Quaternions.normalized(dnHatdt)
Omega = np.array(
[np.cross(n.vec(), ndot.vec()) for n, ndot in zip(nHat, dnHatdt)])
# Take initial data to be simply the coordinate quantities at the relaxation time
i_1 = abs(tA - t1).argmin()
t_i = tA[i_1]
ma = mA[i_1]
mb = mB[i_1]
delta = (ma - mb) / (ma + mb)
chia_0 = chiA[i_1]
chib_0 = chiB[i_1]
Omega_orb_0 = np.sqrt(Omega[i_1, 0]**2 + Omega[i_1, 1]**2 +
Omega[i_1, 2]**2)
R_frame_i = Quaternions.FrameFromXY([
nHat[i_1],
], [
lambdaHat[i_1],
])[0]
if (InitialOmega_orb == 0.0):
InitialOmega_orb = 0.5 * Omega_orb_0
# Construct complete PN waveform
print("Constructing PN data:")
print(
"GWFrames.PNWaveform(Approximant={0}, delta={1},\n"
" chia_0={2}, chib_0={3},\n"
" Omega_orb_0={4}, InitialOmega_orb={5},\n"
" R_frame_i={6},\n"
" MinStepsPerOrbit={7}, PNWaveformModeOrder={8}, PNOrbitalEvolutionOrder={9}"
")".format(Approximant, delta, chia_0, chib_0, Omega_orb_0,
InitialOmega_orb, R_frame_i, MinStepsPerOrbit,
PNWaveformModeOrder, PNOrbitalEvolutionOrder))
sys.stdout.flush()
W_PN_corot = GWFrames.PNWaveform(Approximant, delta, chia_0, chib_0,
Omega_orb_0, InitialOmega_orb, R_frame_i,
MinStepsPerOrbit, PNWaveformModeOrder,
PNOrbitalEvolutionOrder)
W_PN_corot.SetT(W_PN_corot.T() + t_i)
if Debug:
W_PN_orig = GWFrames.PNWaveform(W_PN_corot)
W_PN_corot.TransformToCorotatingFrame()
print("Aligning PN and NR waveforms")
sys.stdout.flush()
# Minimize the distance between the PN and NR rotors in their
# co-rotating frames. Note that NR is kept fixed here, because
# its merger time is set to t=0.0, which is the only physically
# meaningful fiducial quantity present. That quality will
# therefore also be present in the output waveform.
GWFrames.AlignWaveforms(W_NR_corot, W_PN_corot, t1, t2,
DirectAlignmentEvaluations, [], Debug)
print("Constructing PN-NR hybrid")
sys.stdout.flush()
# Construct the hybrid
W_hyb_corot = GWFrames.Waveform(W_PN_corot.Hybridize(W_NR_corot, t1, t2))
# Optionally, make a plot showing the results. It is strongly
# recommended that you look at this plot to make sure things worked
# alright.
if (PlotFileName):
print("Plotting to {0}".format(os.path.join(PathToSystem,
PlotFileName)))
sys.stdout.flush()
plt.axvline(t1, linestyle='dotted')
plt.axvline(t2, linestyle='dotted')
plt.title(r'Modes of the component waveforms')
W_hyb_corot.plot('LogAbs', [2, 2], lw=3, label='Hybrid (2,2)')
W_PN_corot.plot('LogAbs', [2, 2], label='PN (2,2)')
W_NR_corot.plot('LogAbs', [2, 2], label='NR (2,2)')
W_hyb_corot.plot('LogAbs', [2, 1], lw=3, label='Hybrid (2,1)')
W_PN_corot.plot('LogAbs', [2, 1], label='PN (2,1)')
W_NR_corot.plot('LogAbs', [2, 1], label='NR (2,1)')
if Debug:
W_NR_orig.plot('LogAbs', [2, 2], label='NR$_0$ (2,2)', ls='--')
W_NR_orig.plot('LogAbs', [2, 1], label='NR$_0$ (2,1)', ls='--')
W_PN_orig.plot('LogAbs', [2, 2], label='PN$_0$ (2,2)', ls='--')
W_PN_orig.plot('LogAbs', [2, 1], label='PN$_0$ (2,1)', ls='--')
plt.ylim((1e-8, 1.))
plt.xlim((W_NR_corot.T(0), W_NR_corot.T()[-1]))
plt.legend(loc='center')
try:
tight_layout(pad=0.1)
except:
pass
plt.savefig(os.path.join(PathToSystem, PlotFileName))
print("Transforming hybrid to inertial frame and outputting to\n\t"
"{0}".format(os.path.join(PathToSystem, HybridFileName)))
sys.stdout.flush()
W_hyb_corot.TransformToInertialFrame()
W_hyb_corot.OutputToNRAR(os.path.join(PathToSystem, HybridFileName))
print("All done")
sys.stdout.flush()
def __init__(self, x=0, y=0, z=0):
self.x, self.y, self.z = float(x), float(y), float(z)
self.v = qt.Vector(self.x, self.y, self.z)
import GWFrames
import Quaternions
# Read in the data files
print("Reading waveforms from data files...")
Ws = [None]*len(Files)
for i,File in enumerate(Files):
print("\tReading '{0}'".format(File))
Ws[i] = GWFrames.ReadFromNRAR(File)
print("Finished!\n")
W_0 = Ws[0]
# Align to original Waveform
print("Aligning all following waveforms to first...")
xHat = Quaternions.Quaternion(0,1,0,0)
# R0_mid = Quaternions.Squad(W_0.Frame(), W_0.T(), [tmid])[0]
for i,W in enumerate(Ws[1:]):
print("\tAligning {0} of {1}...".format(i+1, len(Ws[1:])))
GWFrames.AlignWaveforms(W_0, W, t1, t2)
# # Rotate by pi if necessary
# Ri_mid = Quaternions.Squad(W.Frame(), W.T(), [tmid])[0]
# if((R0_mid*xHat*R0_mid.inverse()).dot(Ri_mid*xHat*Ri_mid.inverse())<0) :
# W.RotateDecompositionBasis(Quaternions.exp(Quaternions.Quaternion(0,0,0,pi/2)))
# # Align corotating frame of `W[1:]` to corotating frame of `W[0]`
# W.AlignTimeAndFrame(W_0, t1, t2);
Diff = W_0.Compare(W)
plot(Diff.T(), Quaternions.angle(Diff.Frame()))
print("Finished!\n")
def Cal_foot(data):
SensorNumber = 7
K = np.array(data)
# print("K.shape: " + str(K.shape))
m = len(K)
# print("m: " + str(m))
Length = m / SensorNumber - 1
# print("Length: " + str(Length))
# Time = np.delete(K, range(8), 1)
# time_step = sum(Time) / Length / 1000.0
# print("time_step: " + str(time_step))
Time_index = [0.0]
for i in range(Length):
# Time_index.append(Time_index[-1] + Time[7 * i + 6] / 1000.0)
Time_index.append(Time_index[-1] + 1 / 30.0)
Time_index = np.array(Time_index)
# print("Time_index: " + str(Time_index.shape))
# Update the acc to the absolute coordinate axis
for i in range(len(K)):
q = np.array([K[i, 4], K[i, 5], K[i, 6], K[i, 7]])
RotateMat = Qu.quat2dcm(q)
temp_acc = np.dot([K[i, 1], K[i, 2], K[i, 3]], RotateMat)
for j in range(3):
K[i, j + 1] = temp_acc[j]
# Divide the data by seven sensors
K = K.tolist()
rawData = [[], [], [], [], [], [], []]
# print("K[1][0]: " + str(K[1][0]))
for i in range(m):
rawData[int(K[i][0])].append(K[i])
K = np.array(K)
filterData = np.array(rawData)
# print("filterData: " + str(filterData.shape))
start_thread = 0.04
ini_time = np.zeros([1, SensorNumber])
'''
for i in range(SensorNumber):
time = 0
while time < m:
if filterData[i, time, 3] - filterData[i, time+1, 3] > start_thread:
ini_time[0, i] = time
time = time + 1
'''
staticBias = np.zeros([SensorNumber, 3])
for i in range(SensorNumber):
ini_time[0, i] = 47
for j in range(3):
for k in range(int(ini_time[0, i])):
staticBias[i, j] = filterData[i, k, j + 1] + staticBias[i, j]
staticBias[i, j] = staticBias[i, j] / ini_time[0, i]
for i in range(SensorNumber):
for j in range(3):
for k in range(Length):
filterData[i, k, j + 1] = filterData[i, k, j + 1] - staticBias[i, j]
sizeofdata = filterData.shape
# print("sizeofdata: " + str(sizeofdata))
add_acc = np.zeros([SensorNumber, sizeofdata[1]])
# print("add_acc: " + str(add_acc.shape))
for j in range(SensorNumber):
for i in range(sizeofdata[1]):
add_acc[j, i] = np.sqrt(np.square(filterData[j, i, 1])
+ np.square(filterData[j, i, 2])
+ np.square(filterData[j, i, 3]))
add_accFilt_r = add_acc[0, :]
add_accFilt_l = add_acc[6, :]
# print("add_accFilt: " + str(add_accFilt.shape))
station_mark_r = 0.142
station_mark_l = 0.142
stationary_r = []
for i in add_accFilt_r:
if i < station_mark_r:
stationary_r.append(1)
else:
stationary_r.append(0)
stationary_r = np.array(stationary_r)
# print("stationary.shape: " + str(stationary.shape))
stationary_l = []
for i in add_accFilt_l:
if i < station_mark_l:
stationary_l.append(1)
else:
stationary_l.append(0)
stationary_l = np.array(stationary_l)
vel_reight = np.zeros(filterData[0, :, 1:4].shape)
# print("vel_reight.shape: " + str(vel_reight.shape))
for i in range(1, len(vel_reight)):
vel_reight[i, :] = vel_reight[i - 1, :] + filterData[0, i, 1:4] * 9.8 * (Time_index[i] - Time_index[i - 1])
if stationary_r[i] == 1:
vel_reight[i, :] = [0.0, 0.0, 0.0]
vel_left = np.zeros(filterData[6, :, 1:4].shape)
# print("vel_left.shape: " + str(vel_left.shape))
for i in range(1, len(vel_left)):
vel_left[i, :] = vel_left[i - 1, :] + filterData[6, i, 1:4] * 9.8 * (Time_index[i] - Time_index[i - 1])
if stationary_l[i] == 1:
vel_left[i, :] = [0.0, 0.0, 0.0]
velDrift_r = np.zeros(vel_reight.shape)
stationaryStart_r = otf.findval(stationary_r, -1)
# print("stationaryStart: " + str(stationaryStart[0:6]))
stationaryEnd_r = otf.findval(stationary_r, 1)
# print("stationaryEnd: " + str(stationaryEnd[0:6]))
velDrift_l = np.zeros(vel_left.shape)
stationaryStart_l = otf.findval(stationary_l, -1)
stationaryEnd_l = otf.findval(stationary_l, 1)
delStart_r = []
delEnd_r = []
for i in range(len(stationaryEnd_r)):
if stationaryStart_r[i + 1] - stationaryEnd_r[i] <= 0:
delStart_r.append(i + 1)
delEnd_r.append(i)
stationaryStart_r = np.delete(stationaryStart_r, delStart_r, axis=0)
stationaryEnd_r = np.delete(stationaryEnd_r, delEnd_r, axis=0)
delStart_l = []
delEnd_l = []
for i in range(len(stationaryEnd_l)):
if stationaryStart_l[i + 1] - stationaryEnd_l[i] <= 0:
delStart_l.append(i + 1)
delEnd_l.append(i)
stationaryStart_l = np.delete(stationaryStart_l, delStart_l, axis=0)
stationaryEnd_l = np.delete(stationaryEnd_l, delEnd_l, axis=0)
station_r = stationaryStart_r.tolist() + stationaryEnd_r.tolist()
station_r.sort()
station_r.append(None)
index, val, j = 0, 1, 0
stationary_sort_r = []
for i in range(int(len(stationary_r))):
if i == station_r[index]:
if j % 2 == 0:
val = val - 1
else:
val = val + 1
j = j + 1
index = index + 1
stationary_sort_r.append(val)
for i in range(len(stationaryEnd_r)):
driftRate = vel_reight[stationaryEnd_r[i] - 1, :] / (stationaryEnd_r[i] - stationaryStart_r[i])
enum = np.array(range(1, 1 + int(stationaryEnd_r[i] - stationaryStart_r[i])))
enum = enum.reshape([enum.shape[0], 1])
drift = np.array([np.dot(enum, driftRate[0]), np.dot(enum, driftRate[1]), np.dot(enum, driftRate[2])])
drift = drift.reshape([3, drift.shape[1]]).T
velDrift_r[stationaryStart_r[i]:stationaryEnd_r[i], :] = drift
vel_reight = vel_reight - velDrift_r
pos_right = np.zeros(vel_reight.shape)
for i in range(1, len(vel_reight)):
pos_right[i, :] = pos_right[i - 1, :] + vel_reight[i, :] * (Time_index[i] - Time_index[i - 1])
# Left foot
station_l = stationaryStart_l.tolist() + stationaryEnd_l.tolist()
station_l.sort()
station_l.append(None)
index, val, j = 0, 1, 0
stationary_sort_l = []
for i in range(int(len(stationary_l))):
if i == station_l[index]:
if j % 2 == 0:
val = val - 1
else:
val = val + 1
j = j + 1
index = index + 1
stationary_sort_l.append(val)
for i in range(len(stationaryEnd_l)):
driftRate = vel_left[stationaryEnd_l[i] - 1, :] / (stationaryEnd_l[i] - stationaryStart_l[i])
enum = np.array(range(1, 1 + int(stationaryEnd_l[i] - stationaryStart_l[i])))
enum = enum.reshape([enum.shape[0], 1])
drift = np.array([np.dot(enum, driftRate[0]), np.dot(enum, driftRate[1]), np.dot(enum, driftRate[2])])
drift = drift.reshape([3, drift.shape[1]]).T
velDrift_l[stationaryStart_l[i]:stationaryEnd_l[i], :] = drift
vel_left = vel_left - velDrift_l
pos_left = np.zeros(vel_left.shape)
for i in range(1, len(vel_left)):
pos_left[i, :] = pos_left[i - 1, :] + vel_left[i, :] * (Time_index[i] - Time_index[i - 1])
'''posi_r = []
for i in range(len(pos_right)):
posi_r.append(np.sqrt(np.square(pos_right[i, 0]) + np.square(pos_right[i, 1]) + np.square(pos_right[i, 2])))
station_r.pop()
support = []
swing = []
for i in range(len(stationaryEnd_r) - 1):
support.append(Time_index[stationaryEnd_r[i]] - Time_index[stationaryStart_r[i]])
swing.append(Time_index[stationaryStart_r[i + 1]] - Time_index[stationaryEnd_r[i]])
displacement = []
for i in range(len(stationaryEnd_r)):
pos_start = pos_right[stationaryEnd_r[i]]
pos_end = pos_right[stationaryStart_r[i]]
displacement.append(otf.displace(pos_start, pos_end))'''
res = {'Stationary_r': stationary_sort_r, 'Position_r': pos_right,
'Stationary_l': stationary_sort_l, 'Position_l': pos_left}
'''frep = open('report.txt', 'w')
frep.write(filename + '\r\n')
frep.write('Support Phase of Right Leg Displacement of Right Leg' + '\r\n')
for i in range(len(support)):
temp = 100 * support[i] / (support[i] + swing[i])
frep.write('%.2f' % temp + '%' + ' %.3f' % (displacement[i]) + ' m' + '\r\n')
frep.close()'''
return res
