def test_Quaternions(self):
q = quat.Quaternion(np.array([0, 0, 0.5]))
p = quat.Quaternion(np.array([[0, 0, 0.5], [0, 0, 0.1]]))
print(p * q)
print(q * 3)
print(q * np.pi)
print(q / p)
q5 = q / 5
error = q5.values[0, 3] - 0.1
self.assertTrue(error < self.delta)
self.assertEqual(p[0].values[0, 3], 0.5)
def __init__(self):
self.n = 6
# Maybe change this to start with current state * eye
self.Y = []
self.X = []
self.Z = []
self.Y_raw = []
self.Y_qmean = quat.Quaternion([1, 0, 0, 0])
self.e = quat.Quaternion([1, 0, 0, 0])
self.P_v = []
self.P_vv = []
self.P = np.eye(6)
self.R = np.eye(6)*0.00001
self.Q = np.eye(6)*0.001
self.Y_cov = np.zeros((6, 6))
self.cross_cov = np.zeros((6, 6))
self.ev_i =[]
self.Z_cov = np.zeros((6, 6))
def Norm_Cordinate():
'''
description: 两个传感器初始姿态存在偏差，需要进行统一
param {None}
return {}大臂传感器相对于肩关节传感器的四元数
'''
print("请将两个IMU放置为同一姿态")
time.sleep(2)
temp = np.zeros([1, 32])
i = 0
while i < Initialize_Num:
Get_Quat(Quat)
for j in range(32):
temp[i, j] = Quat[j]
print("The %d time initialization:" % (i + 1))
# print(temp)
# Print_Quat_Info()
Max_Dif = np.max(temp, axis=0, keepdims=True) - np.min(
temp, axis=0, keepdims=True)
assert (Max_Dif.shape == (1, 32))
if ((Max_Dif > Initialize_Threshold).any()):
i = 0
print(Max_Dif > Initialize_Threshold)
print("Initialization Failure!Please stay still!")
temp = np.zeros([1, 32])
continue
time.sleep(0.5)
temp = np.row_stack((temp, np.zeros([1, 32])))
i = i + 1
temp = np.delete(temp, -1, 0)
average = np.average(temp, axis=0)
average = np.reshape(average, (1, 32))
# RSs2Es = Quat2R(average[0][0],average[0][1],average[0][2],average[0][3])
# RSu2Eu = Quat2R(average[0][4],average[0][5],average[0][6],average[0][7])
# REu2Es = np.dot(RSs2Es,np.linalg.inv(RSu2Eu))
# return REu2Es
qSs2Es = quat.Quaternion(average[0][0], average[0][1], average[0][2],
average[0][3])
qSu2Eu = quat.Quaternion(average[0][4], average[0][5], average[0][6],
average[0][7])
qEu2Es = qSs2Es * qSu2Eu.conj()
return qEu2Es
def estimate_rot(data_num=1):
# Read the data
imu_data = read_data_imu(data_num)
q = quat.Quaternion(np.array([1, 0, 0, 0]))
omega = np.array([0, 0, 0])
curr_state = State(q, omega)
param = params.Params()
ts = np.squeeze(imu_data['ts'])
real_measurement = np.squeeze(imu_data['vals']).transpose()
size_ts = ts.shape[0]
sigma = Sigma_pts()
rpy = []
for i in range(1, size_ts):
dt = ts[i] - ts[i - 1]
sigma.find_points(curr_state, dt)
sigma.find_measurements()
corrected_measurements = convert_measurements(real_measurement[i, :3],
real_measurement[i, 3:])
curr_state.kalman_update(sigma, corrected_measurements)
rpy.append(curr_state.q.quat2rpy())
# plot vicon data
vicon_data = read_data_vicon(data_num)
v_ts = np.squeeze(vicon_data['ts'])
x, y, z = rotationMatrixToEulerAngles(vicon_data['rots'])
# plt.plot(v_ts, x, 'r')
plt.figure()
vicon, = plt.plot(v_ts, x, label="Vicon")
ukf, = plt.plot(ts[1:], np.asarray(rpy)[:, 0], label="UKF")
plt.legend(handles=[vicon, ukf])
plt.title("Roll")
plt.figure()
vicon, = plt.plot(v_ts, y, label="Vicon")
ukf, = plt.plot(ts[1:], np.asarray(rpy)[:, 1], label="UKF")
plt.legend(handles=[vicon, ukf])
plt.title("Pitch")
plt.figure()
vicon, = plt.plot(v_ts, z, label="Vicon")
ukf, = plt.plot(ts[1:], np.asarray(rpy)[:, 2], label="UKF")
plt.legend(handles=[vicon, ukf])
plt.title("Yaw")
plt.show()
out = quat.vel2quat(np.deg2rad(vel), [0., 0., 0.], rate, 'sf')
result = out[-1:]
correct = [[-0.76040597, 0., 0., 0.64944805]]
error = norm(result - correct)
self.assertTrue(error < self.delta)
def test_quat2vel(self):
rate = 1000
t = np.arange(0, 10, 1. / rate)
x = 0.1 * np.sin(t)
y = 0.2 * np.sin(t)
z = np.zeros_like(t)
q = np.column_stack((x, y, z))
vel = quat.quat2vel(q, rate, 5, 2)
qReturn = quat.vel2quat(vel, q[0], rate, 'sf')
error = np.max(np.abs(q - qReturn[:, 1:]))
self.assertTrue(error < 1e3)
if __name__ == '__main__':
q = quat.Quaternion(np.array([0, 0, 0.5]))
p = quat.Quaternion(np.array([[0, 0, 0.5], [0, 0, 0.1]]))
print(p * q)
print(q * 3)
print(q * np.pi)
print(q / p)
#unittest.main()
#print('Thanks for using programs from Thomas!')
#sleep(2)
def Joint2Sensor():
'''
description: 得到人体关节相对于传感器的四元数
param {*}
return {*}
'''
# 先获得Npose姿态下第一个姿态的四元数和偏航角
print("ORIGIN POSITION")
print("请保持双臂紧贴于身体两侧，手掌贴于身体两侧且拇指指向身体前方")
position_0 = Stable_Position()
qSs02Es = quat.Quaternion(position_0[0][0], position_0[0][1],
position_0[0][2], position_0[0][3])
qSu02Eu = quat.Quaternion(position_0[0][4], position_0[0][5],
position_0[0][6], position_0[0][7])
# 使用者保持Npose原地转动一定角度，获取当前姿态下四元数和偏航角
print("身体转过一定角度")
print("请保持双臂紧贴于身体两侧，手掌贴于身体两侧且拇指指向身体前方")
Hold_On(5)
position_1 = Stable_Position()
qSs12Es = quat.Quaternion(position_1[0][0], position_1[0][1],
position_1[0][2], position_1[0][3])
qSu12Eu = quat.Quaternion(position_1[0][4], position_1[0][5],
position_1[0][6], position_1[0][7])
# 计算初始偏航角
delta_theta_s = (qSs12Es.Euler321())[0] - (qSs02Es.Euler321())[0]
delta_theta_u = (qSu12Eu.Euler321())[0] - (qSu02Eu.Euler321())[0]
w1 = qSs02Es.s
z1 = qSs02Es.z
w2 = qSs12Es.s
z2 = qSs12Es.z
theta = delta_theta_s
theta_s = -2 * math.atan2(
w1 - w2 * math.cos(theta / 2) + z2 * math.sin(theta / 2),
z2 * math.cos(theta / 2) - z1 + w2 * math.sin(theta / 2))
# theta_s = -cmath.log(-cmath.sqrt(-(w1*1j - z1 - w2*cmath.exp((theta*1j)/2)*1j + z2*cmath.exp((theta*1j)/2))*(w2*1j + z2 - w1*cmath.exp((theta*1j)/2)*1j - z1*cmath.exp((theta*1j)/2)))/(w1*cmath.exp((theta*1j)/4) - w2*cmath.exp((theta*3j)/4) + z1*cmath.exp((theta*1j)/4)*1j - z2*cmath.exp((theta*3j)/4)*1j))*2j
# theta_s = theta_s.real
w1 = qSu02Eu.s
z1 = qSu02Eu.z
w2 = qSu12Eu.s
z2 = qSu12Eu.z
theta = delta_theta_u
theta_u = -2 * math.atan2(
w1 - w2 * math.cos(theta / 2) + z2 * math.sin(theta / 2),
z2 * math.cos(theta / 2) - z1 + w2 * math.sin(theta / 2))
# theta_u = -cmath.log(-cmath.sqrt(-(w1*1j - z1 - w2*cmath.exp((theta*1j)/2)*1j + z2*cmath.exp((theta*1j)/2))*(w2*1j + z2 - w1*cmath.exp((theta*1j)/2)*1j - z1*cmath.exp((theta*1j)/2)))/(w1*cmath.exp((theta*1j)/4) - w2*cmath.exp((theta*3j)/4) + z1*cmath.exp((theta*1j)/4)*1j - z2*cmath.exp((theta*3j)/4)*1j))*2j
# theta_u = theta_u.real
#计算人体关节相对于传感器的四元数
qJs02Es = quat.Quaternion(cmath.cos(theta_s / 2), 0, 0,
cmath.sin(theta_s / 2))
qJu02Eu = quat.Quaternion(cmath.cos(theta_s / 2), 0, 0,
cmath.sin(theta_s / 2))
qJs2Ss = qSs02Es.conj() * qJs02Es
qJu2Su = qSu02Eu.conj() * qJu02Eu
return [qJs2Ss, qJu2Su]
temp = np.zeros([1, 32])
print("Initialization Processing")
print("请保持双臂紧贴于身体两侧，手掌贴于身体两侧且拇指指向身体前方")
i = 0
while i < Initialize_Num:
Get_Quat(Quat)
for j in range(32):
temp[i, j] = Quat[j]
print("The %d time initialization:" % (i + 1))
#print(temp)
#Print_Quat_Info()
Max_Dif = np.max(temp, axis=0, keepdims=True) - np.min(
temp, axis=0, keepdims=True)
assert (Max_Dif.shape == (1, 32))
if ((Max_Dif > Initialize_Threshold).any()):
i = 0
# print(Max_Dif > Initialize_Threshold)
print("\nInitialization Failure!Please stay still!\n")
temp = np.zeros([1, 32])
continue
time.sleep(0.5)
temp = np.row_stack((temp, np.zeros([1, 32])))
i = i + 1
temp = np.delete(temp, -1, 0)
assert (temp.shape == (Initialize_Num, 32))
average = np.average(temp, axis=0)
average = np.reshape(average, (1, 32))
assert (average.shape == (1, 32))
print("Initialization Completed!")
print('The 1 IMU initial Quat is:', average[0][0:4])
print('The 2 IMU initial Quat is:', average[0][4:8])
return average
except:
raise EOFError
ts.start() #start reading the data from the Serai port
# 初始化ROS节点和发布者
# pub = rospy.Publisher('pos_topic', Limbpos, queue_size=10)
pub_f = rospy.Publisher('motor_force_topic', Motor_Force, queue_size=10)
rospy.init_node('pos_talker', anonymous=True)
rate = rospy.Rate(100)
# 统一IMU的参考系
temp = input("统一IMU参考系?(Y/N)")
if temp == 'Y' or temp == 'y':
qEu2Es = Norm_Cordinate()
else:
qEu2Es = quat.Quaternion(1, 0, 0, 0)
# 设置初始姿态，Npose，即为双手自然下垂，紧贴大腿两侧
while 1:
temp = input("Set the initial position?(Y/N)")
if temp == 'Y' or temp == 'y':
break
[qJs2Ss, qJu2Su] = Joint2Sensor() #关节相对于传感器的四元数
while not rospy.is_shutdown():
Get_Quat(Quat)
qSs2Es = quat.Quaternion(Quat[0], Quat[1], Quat[2], Quat[3])
qSu2Eu = quat.Quaternion(Quat[4], Quat[5], Quat[6], Quat[7])
qJs2Es = qSs2Es * qJs2Ss
qJu2Eu = qSu2Eu * qJu2Su
qJu2Js = qJs2Es.conj() * qEu2Es
def __init__(self,
in_file=None,
q_type='analytical',
R_init=np.eye(3),
calculate_position=True,
pos_init=np.zeros(3),
in_data=None,
yaw_pitch_roll=None):
"""Initialize an IMU-object.
Note that this includes a number of activities:
- Read in the data (which have to be either in "in_file" or in "in_data")
- Make acceleration, angular_velocity etc. accessible, in a
sensor-independent way
- Calculates duration, totalSamples, etc
- If q_type==None, data are only read in; otherwise, 3-D orientation is
calculated with the method specified in "q_type", and stored in the
property "quat".
- If position==True, the method "calc_position" is automatically called,
and the 3D position stored in the propery "pos". (Note that
if q_type==None, then "position" is set to "False".)
in_file : string
Location of infile / input
q_type : string
Determines how the orientation gets calculated:
- 'analytical' .... quaternion integration of angular velocity [default]
- 'kalman' ..... quaternion Kalman filter
- 'madgwick' ... gradient descent method, efficient
- 'mahony' .... formula from Mahony, as implemented by Madgwick
- 'None' ... data are only read in, no orientation calculated
R_init : 3x3 array
approximate alignment of sensor-CS with space-fixed CS
currently only used in "analytical"
calculate_position : Boolean
If "True", position is calculated, and stored in property "pos".
pos_init : (,3) vector
initial position
currently only used in "analytical"
in_data : dictionary
If the data are provided directly, not from a file
Also used to provide "rate" for "polulu" sensors.
"""
if in_data is None and in_file is None:
raise ValueError('Either in_data or in_file must be provided.')
elif in_file is None:
# Get the data from "in_data"
# In that case, "in_data"
# - must contain the fields ['acc', 'omega']
# - can contain the fields ['rate', 'mag']
# self.source is set to "None"
self._set_data(in_data)
else:
# Set rate, acc, omega, mag
# Note: this is implemented in the concrete class, implenented in
# the corresponding module in "sensors"
self.source = in_file
self.get_data(in_file, in_data)
# Set information not determined by the IMU-data
self.R_init = R_init
self.pos_init = pos_init
override_r_init = False
if yaw_pitch_roll is None:
yaw_pitch_roll = np.array([0, 0, 0])
else:
override_r_init = True
self.quat_init = quat.Quaternion(inData=yaw_pitch_roll,
inType='Helmholtz').values[0]
if override_r_init:
print(f"overriding with rotvec = {quat.convert(self.quat_init)}")
self.R_init = quat.convert(self.quat_init)
# Set the analysis method, and consolidate the object (see below)
# This also means calculating the orientation quaternion!!
self.set_qtype(q_type)
if q_type != None:
if calculate_position:
self.calc_position()
def acc_model(self, noise):
# maybe should be a unit quat
# maybe +ve g
g = quat.Quaternion([0, 0, 0, 9.8])
g_body = (self.q * g * self.q.inv())
return g_body.v + noise