Coverage for cli/src/xyfigure/three_points_angular_velocity.py: 95%

1#!/usr/bin/env python 

2import numpy as np 

3 

4# import os 

5# import sys 

6 

7 

8def cross_matrix(vec): 

9 """ 

10 This function computes the cross matrix to execute the cross product. 

11 """ 

12 # return np.array([[0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]]) 

13 return np.array( 

14 [ 

15 [ 

16 [0.0, -vec[i, 2], vec[i, 1]], 

17 [vec[i, 2], 0.0, -vec[i, 0]], 

18 [-vec[i, 1], vec[i, 0], 0.0], 

19 ] 

20 for i in range(len(vec)) 

21 ] 

22 ) 

23 

24 

25class ThreePointsAngularVelocity: 

26 """ 

27 Given a time-series of three points' current position and velocity, 

28 both three-dimensional, calculates the rigid body angular velocity 

29 using a least-squares methodology. 

30 

31 : params : 

32 : rP Position vector to point P, shape (m, 3) in E3 as (x, y, z). 

33 : rQ Position vector to point Q, same shape as rP. 

34 : rR Position vector to point R, same shape as rP. 

35 All position vectors measured from the origin O. 

36 : vP Velocity vector of point P in inertial frame F, same shape as rP. 

37 : vQ Velocity vector of point Q in inertial frame F, same shape as rQ. 

38 : vR Velocity vector of point R in inertial frame F, same shape as rR. 

39 

40 : returns : 

41 : wB Angular velocity vector of rigid body B i frame F, with 

42 shape (m, 3). 

43 

44 : reference : 

45 : Laflin, J. (2019) compute_omega_test.py. 

46 """ 

47 

48 def __init__(self, rP, rQ, rR, vP, vQ, vR, verbose=0): 

49 

50 self.verbose = verbose 

51 

52 if self.verbose: # pragma: no cover 

53 print("-------------------------------------------") 

54 print("Three Points Angular Velocity server start.") 

55 

56 nts, _ = rP.shape # number of time steps, number of space dimensions 

57 

58 # position vector r from point P to point Q 

59 rPQ = rQ - rP 

60 

61 # position vector r from point P to point R 

62 rPR = rR - rP 

63 

64 # cross_rPQ = self.cross_matrix(rPQ) 

65 # cross_rPR = self.cross_matrix(rPR) 

66 cross_rPQ = cross_matrix(rPQ) 

67 cross_rPR = cross_matrix(rPR) 

68 

69 # self.A = np.vstack((-cross_rPQ, -cross_rPR)) 

70 self.A = [ 

71 np.vstack((-cross_rPQ[i], -cross_rPR[i])).tolist() for i in range(nts) 

72 ] 

73 

74 # self.b = np.vstack(((vQ - vP).reshape(3, 1), (vR - vP).reshape(3, 1))) 

75 self.b = [ 

76 np.vstack( 

77 ((vQ[i] - vP[i]).reshape(3, 1), (vR[i] - vP[i]).reshape(3, 1)) 

78 ).tolist() 

79 for i in range(nts) 

80 ] 

81 

82 # self.wB = np.linalg.solve(np.transpose(self.A) @ self.A, np.transpose(self.A) @ self.b) 

83 # self.wB = [np.linalg.solve(np.transpose(self.A[i]) @ self.A[i], np.transpose(self.A[i]) @ self.b[i]) for i in range(nts)] 

84 self.wB = [ 

85 np.linalg.solve( 

86 np.transpose(self.A[i]) @ self.A[i], np.transpose(self.A[i]) @ self.b[i] 

87 ).tolist() 

88 for i in range(nts) 

89 ] 

90 

91 self.wB = [ 

92 np.squeeze(np.reshape(self.wB[i], (1, 3))).tolist() for i in range(nts) 

93 ] 

94 

95 if self.verbose: # pragma: no cover 

96 print(f"rP = {rP}") 

97 print(f"rQ = {rQ}") 

98 print(f"rR = {rR}") 

99 print("") 

100 print(f"vP = {vP}") 

101 print(f"vQ = {vQ}") 

102 print(f"vR = {vR}") 

103 print("") 

104 print(f"rPQ = {rPQ}") 

105 print(f"rPR = {rPR}") 

106 print("") 

107 rPQ_length = [np.linalg.norm(rPQ[i]) for i in range(nts)] 

108 rPR_length = [np.linalg.norm(rPR[i]) for i in range(nts)] 

109 

110 if rPQ_length[0] > 0 and rPR_length[0] > 0: 

111 rPQ_hat = [np.array(rPQ[i]) / rPQ_length[i] for i in range(nts)] 

112 rPR_hat = [np.array(rPR[i]) / rPR_length[i] for i in range(nts)] 

113 

114 print(f"rPQ_hat = {rPQ_hat}") 

115 print(f"rPR_hat = {rPR_hat}") 

116 

117 print("Dot product between unit vectors rPQ_hat and rPR_hat [0, 1]:") 

118 print(" 0 is best, vectors are perpendicular, adds rank") 

119 print(" 1 is worst, vectors are parallel, fails to add rank") 

120 

121 dot_product = [np.dot(rPQ_hat[i], rPR_hat[i]) for i in range(nts)] 

122 print(f" (rPQ_hat . rPR_hat) = {dot_product}") 

123 

124 AtransposeA = [np.transpose(self.A[i]) @ self.A[i] for i in range(nts)] 

125 print("[A^T A] matrix =") 

126 print(f"{AtransposeA}") 

127 print("") 

128 

129 rank = [np.linalg.matrix_rank(AtransposeA[i]) for i in range(nts)] 

130 print(f"Rank of [A^T A] matrix = {rank}") 

131 print("") 

132 

133 print(f"A = {self.A}") 

134 print("") 

135 print(f"wB = {self.wB}") 

136 print("") 

137 print(f"b = {self.b}") 

138 print("") 

139 

140 print("Three Points Angular Velocity server stop.") 

141 print("------------------------------------------") 

142 

143 def angular_velocity(self): 

144 """ 

145 Computes the estimates angular velocity of rigid body B in frame F. 

146 """ 

147 if self.verbose: # pragma: no cover 

148 print(f"calculated angular velocity = {self.wB}") 

149 return self.wB 

150 

151 def A_matrix(self): 

152 """ 

153 Computes the least-squares matrix A, in A w = b, used to compute 

154 the angular velocity w. 

155 """ 

156 return self.A 

157 

158 def b_vector(self): 

159 """ 

160 Computes the vector right-hand-side, in A w = b, used to compute 

161 the angular velocity w. 

162 """ 

163 return self.b 

164 

165 

166""" 

167Copyright 2023 Sandia National Laboratories 

168 

169Notice: This computer software was prepared by National Technology and Engineering Solutions of 

170Sandia, LLC, hereinafter the Contractor, under Contract DE-NA0003525 with the Department of Energy 

171(DOE). All rights in the computer software are reserved by DOE on behalf of the United States 

172Government and the Contractor as provided in the Contract. You are authorized to use this computer 

173software for Governmental purposes but it is not to be released or distributed to the public. 

174NEITHER THE U.S. GOVERNMENT NOR THE CONTRACTOR MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES 

175ANY LIABILITY FOR THE USE OF THIS SOFTWARE. This notice including this sentence must appear on any 

176copies of this computer software. Export of this data may require a license from the United States 

177Government. 

178"""