def body2local(q, x):
"""Convert a vector in the body space to the local inertial reference
frame."""
q1 = Quaternion(q)
x = Quaternion([0, x[0], x[1], x[2]])
return (q1.inverse() * x * q1).q[1:4]
def body2local(q, x): """Convert a vector in the body space to the local inertial reference frame.""" q1 = Quaternion(q) x = Quaternion([0, x[0], x[1], x[2]]) return (q1.inverse() * x * q1).q[1:4]
def openPastCameraData():
global positions, orientations, recordingTimes, originalQuart
with open('/home/pi/RealSenseProjects/coordinateData.csv') as csv_file:
csv_reader = csv.reader(csv_file, delimiter=',')
line_count=0
i = 0
for row in csv_reader:
#Mapping of coordinates is done here
positions[0][0] = float(row[8]) #x
positions[0][1] = float(row[6]) #y
positions[0][2] = float(row[7]) #z
if(originalQuart.x == 0 and originalQuart.y == 0 and originalQuart.z ==0 and originalQuart.w == 0):
originalQuart = Quaternion(float(row[2]), float(row[3]), float(row[4]), float(row[5]))
currQuart = Quaternion(float(row[2]), float(row[3]), float(row[4]), float(row[5]))
#Following operation makes the current quaternion the delta between the current orintation vector and the original
transform = originalQuart.inverse().multiplication(currQuart)
orientations[0] = [transform.x, transform.y, transform.z, transform.w]
trackingFlags[0] = True
recordingTimes[0] = recordingTimes[1]
recordingTimes[1] = int(row[1]) #This gives time in microseconds
#Used to sync the threads
i=i+1
time.sleep(0.005)
event.set()
def get_Lxyz_global(self):
"""Return angular momentum in global (nonrotating) reference frame."""
q = Quaternion(self.get_Q())
# Note that local angular momentum doesn't really make sense, but it's part
# of our conversion process
I_cm = self.f_Icm(self.state_vector, self.t)
L_local = I_cm * np.asmatrix(self.get_wxyz()).T
L_global = q * Quaternion([0] + list(L_local)) * q.inverse()
return L_global[1:4]
def get_Lxyz_global(self): """Return angular momentum in global (nonrotating) reference frame.""" q = Quaternion(self.get_Q()) # Note that local angular momentum doesn't really make sense, but it's part # of our conversion process I_cm = self.f_Icm(self.state_vector, self.t) L_local = I_cm * np.asmatrix(self.get_wxyz()).T L_global = q * Quaternion([0] + list(L_local)) * q.inverse() return L_global[1:4]
def force_torque(y, t):
# Force is actually zero (at least for this case, where the center of
# mass isn't moving).
force = [0, 0, 0]
# Torque is constantly applied from
q = Quaternion(y[6:10])
q.normalize()
F = Quaternion([0, 0, 0, -1])
F_lcl = (q.inverse() * F * q)[1:4]
torque = np.cross([1, 0, 0], F_lcl)
return (force, torque)
def force_torque(y, t): # Force is actually zero (at least for this case, where the center of # mass isn't moving). force = [0,0,0] # Torque is constantly applied from q = Quaternion(y[6:10]) q.normalize() F = Quaternion([0,0,0,-1]) F_lcl = (q.inverse() * F * q)[1:4] torque = np.cross([1,0,0], F_lcl) return (force, torque)
def receiveRigidBodyFrame():
i=0
global positions, orientations, recordingTimes, originalQuart, totalRowList, originalPosition
firstPosSet = False
while True:
i=i+1
try:
csvFile = open("/home/pi/t265/coordinateData.csv", "r")
content = csvFile.readline()
row = content.split(",")
if(not(firstPosSet)):
originalPosition[0][0] = float(row[8])
originalPosition[0][1] = float(row[6])
originalPosition[0][2] = float(row[7])
firstPosSet = True
#Calculates the delta in current position from initial position
#Mapping of coordinate systems is done here
positions[0][0] = float(row[8]) - originalPosition[0][0] #z
positions[0][1] = float(row[6]) - originalPosition[0][1]#x
positions[0][2] = float(row[7]) - originalPosition[0][2] #y
#The quaternion set here is the difference between the current quaternion and the initial quaternion
if(originalQuart.x == 0 and originalQuart.y == 0 and originalQuart.z ==0 and originalQuart.w == 0):
originalQuart = Quaternion(float(row[2]), float(row[3]), float(row[4]), float(row[5]))
currQuart = Quaternion(float(row[2]), float(row[3]), float(row[4]), float(row[5]))
#Following operation makes the current quaternion the delta between the current orintation vector and the original
transform = originalQuart.inverse().multiplication(currQuart)
orientations[0] = [transform.x, transform.y, transform.z, transform.w]
trackingFlags[0] = True
#Keeps track of recording times so that velocity between two corresponding positions can be calculated
recordingTimes[0] = recordingTimes[1]
recordingTimes[1] = int(row[1]) #This gives time in microseconds
#Used to sync the threads
event.set()
except:
continue
def get_wxyz_global(self):
"""Return angular velocity in global (nonrotating) reference frame."""
q = Quaternion(self.get_Q())
w_global = q * Quaternion([0] + list(self.get_wxyz())) * q.inverse()
return w_global[1:4]
def get_wxyz_global(self): """Return angular velocity in global (nonrotating) reference frame.""" q = Quaternion(self.get_Q()) w_global = q * Quaternion([0] + list(self.get_wxyz())) * q.inverse() return w_global[1:4]
