def calc_position(self):
'''Calculate the position, assuming that the orientation is already known.'''
initialPosition = self.pos_init
# Acceleration, velocity, and position ----------------------------
# From q and the measured acceleration, get the \frac{d^2x}{dt^2}
g = constants.g
g_v = np.r_[0, 0, g]
accReSensor = self.acc - vector.rotate_vector(g_v, quat.q_inv(
self.quat))
accReSpace = vector.rotate_vector(accReSensor, self.quat)
# Position and Velocity through integration, assuming 0-velocity at t=0
vel = np.nan * np.ones_like(accReSpace)
pos = np.nan * np.ones_like(accReSpace)
for ii in range(accReSpace.shape[1]):
vel[:, ii] = cumtrapz(accReSpace[:, ii],
dx=1. / np.float(self.rate),
initial=0)
pos[:, ii] = cumtrapz(vel[:, ii],
dx=1. / np.float(self.rate),
initial=initialPosition[ii])
self.vel = vel
self.pos = pos
def analytical(R_initialOrientation=np.eye(3),
omega=np.zeros((5, 3)),
initialPosition=np.zeros(3),
accMeasured=np.column_stack((np.zeros((5, 2)), g * np.ones(5))),
rate=128):
####################################################################################
# #
# github.com/thomas-haslwanter/scikit-kinematics/blob/master/skinematics/imus.py #
# Analytically reconstructing accmtr. position and orientation, using angular #
# velocity and linear acceleration. Assumes a start in a stationary position. #
# Needs auxiliary libraries - quat.py, vector.py, rotmat.py. #
# Parameters #
# ------------------------------------------------------------------------------ #
# R_initialOrientation : ndarray(3,3) --------- Rotation matrix describing #
# the sensor's initial orientation, except for a mis-orientation w/rt gravity. #
# omega : ndarray(N,3) ------------------------ Angular velocity, in [rad/s] #
# initialPosition : ndarray(3,) --------------- Initial position, in [m] #
# accMeasured : ndarray(N,3) ------------------ Linear acceleration, in [m/s^2] #
# rate : float -------------------------------- Sampling rate, in [Hz] #
# Returns #
# ------------------------------------------------------------------------------ #
# q : ndarray(N,3) ---------------------------- Orientation - quaternion vector #
# pos : ndarray(N,3) -------------------------- Position in space [m] #
# vel : ndarray(N,3) -------------------------- Velocity in space [m/s] #
# #
####################################################################################
# Transform recordings to angVel/acceleration in space -------------------------
# ----- Find gravity's orientation on the sensor in "R_initialOrientation" -----
g0 = np.linalg.inv(R_initialOrientation).dot(np.r_[0, 0, g])
# ----- For the remaining deviation, assume the shortest rotation to there. ----
q0 = vector.q_shortest_rotation(accMeasured[0], g0)
q_initial = rotmat.convert(R_initialOrientation, to='quat')
# ----- Combine the two to form a reference orientation. -----------------------
q_ref = quat.q_mult(q_initial, q0)
# Compute orientation q by "integrating" omega ---------------------------------
q = quat.calc_quat(omega, q_ref, rate, 'bf')
# Acceleration, velocity, and position -----------------------------------------
# ----- Using q and the measured acceleration, get the \frac{d^2x}{dt^2} -------
g_v = np.r_[0, 0, g]
accReSensor = accMeasured - vector.rotate_vector(g_v, quat.q_inv(q))
accReSpace = vector.rotate_vector(accReSensor, q)
# ----- Make the first position the reference position -------------------------
q = quat.q_mult(q, quat.q_inv(q[0]))
# Done. ------------------------------------------------------------------------
return q
def test_rotate_vector(self):
x = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
result = vector.rotate_vector(x, [0, 0, sin(0.1)])
correct = array([[0.98006658, 0.19866933, 0.],
[-0.19866933, 0.98006658, 0.], [0., 0., 1.]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def test_rotate_vector(self):
x = [[1,0,0], [0, 1, 0], [0,0,1]]
result = vector.rotate_vector(x, [0, 0, sin(0.1)])
correct = array([[ 0.98006658, 0.19866933, 0. ],
[-0.19866933, 0.98006658, 0. ],
[ 0. , 0. , 1. ]])
error = norm(result - correct)
self.assertTrue(error < self.delta)
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.
Parameters
----------
Gyroscope : array, shape (,3)
Angular velocity [rad/s]
Accelerometer : array, shape (,3)
Linear acceleration (Only the direction is used, so units don't
matter.)
Magnetometer : array, shape (,3)
Orientation of local magenetic field.
(Again, only the direction is used, so units don't matter.)
'''
q = self.Quaternion
# short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2 + h[1]**2), 0, h[2]))
# Estimated direction of gravity and magnetic field
v = np.array([
2 * (q[1] * q[3] - q[0] * q[2]), 2 * (q[0] * q[1] + q[2] * q[3]),
q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2
])
w = np.array([
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2]),
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3]), 2 * b[1] *
(q[0] * q[2] + q[1] * q[3]) + 2 * b[3] * (0.5 - q[1]**2 - q[2]**2)
])
# Error is sum of cross product between estimated direction and measured
# direction of fields
e = np.cross(Accelerometer, v) + np.cross(Magnetometer, w)
if self.Ki > 0:
self._eInt += e * self.SamplePeriod
else:
self._eInt = np.array([0, 0, 0], dtype=np.float)
# Apply feedback terms
Gyroscope += self.Kp * e + self.Ki * self._eInt
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])).flatten()
# Integrate to yield quaternion
q += qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q)
def run(self, rate=100, looping=True):
'''Run the viewer
Parameters
----------
rate : integer
Sample rate for the display [Hz]. Lower numbers result in slower display.
looping : boolean
If set to "True", the display will loop until the window is closed.
'''
dt = int(1 / rate * 1000) # [msec]
# Camera properties, e.g. focal length etc
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glLoadIdentity()
glu.gluPerspective(45, (self.display[0] / self.display[1]), 0.1, 50.0)
gl.glTranslatef(0.0, 0.0, -3)
counter = 0
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
quit()
counter = np.mod(counter + 1, self.quat.shape[0])
if not looping and counter == self.quat.shape[0] - 1:
break
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glEnable(gl.GL_DEPTH_TEST)
# Camera position
gl.glMatrixMode(gl.GL_MODELVIEW)
gl.glLoadIdentity()
glu.gluLookAt(self.cam_pos[0], self.cam_pos[1], self.cam_pos[2],
self.cam_target[0], self.cam_target[1],
self.cam_target[2], self.cam_up[0], self.cam_up[1],
self.cam_up[2])
# Scene elements
gl.glPushMatrix()
cur_corners = vector.rotate_vector(
self.vertices, self.quat[counter]) @ self.openGL2skin.T
#cur_corners = cur_corners * np.r_[1, 1, -1] # This seems to be required
##to get things right - but I don't understand OpenGL at this point
self.draw_pointer(cur_corners)
gl.glPopMatrix()
self.draw_axes()
pygame.display.flip()
pygame.time.wait(dt)
def run(self, rate=100, looping=True):
'''Run the viewer
Parameters
----------
rate : integer
Sample rate for the display [Hz]. Lower numbers result in slower display.
looping : boolean
If set to "True", the display will loop until the window is closed.
'''
dt = int(1/rate*1000) # [msec]
# Camera properties, e.g. focal length etc
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glLoadIdentity()
glu.gluPerspective(45, (self.display[0]/self.display[1]), 0.1, 50.0)
gl.glTranslatef(0.0,0.0, -3)
counter = 0
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
quit()
counter = np.mod(counter+1, self.quat.shape[0])
if not looping and counter == self.quat.shape[0]-1:
break
gl.glClear(gl.GL_COLOR_BUFFER_BIT|gl.GL_DEPTH_BUFFER_BIT)
gl.glEnable(gl.GL_DEPTH_TEST)
# Camera position
gl.glMatrixMode(gl.GL_MODELVIEW)
gl.glLoadIdentity()
glu.gluLookAt(
self.cam_pos[0], self.cam_pos[1], self.cam_pos[2],
self.cam_target[0], self.cam_target[1], self.cam_target[2],
self.cam_up[0], self.cam_up[1], self.cam_up[2] )
# Scene elements
gl.glPushMatrix()
cur_corners = vector.rotate_vector(self.vertices, self.quat[counter]) @ self.openGL2skin.T
#cur_corners = cur_corners * np.r_[1, 1, -1] # This seems to be required
##to get things right - but I don't understand OpenGL at this point
self.draw_pointer(cur_corners)
gl.glPopMatrix()
self.draw_axes()
pygame.display.flip()
pygame.time.wait(dt)
def calc_position(self):
'''Calculate the position, assuming that the orientation is already known.'''
initialPosition = self.pos_init
# Acceleration, velocity, and position ----------------------------
# From q and the measured acceleration, get the \frac{d^2x}{dt^2}
g = constants.g
g_v = np.r_[0, 0, g]
accReSensor = self.acc - vector.rotate_vector(g_v, quat.q_inv(self.quat))
accReSpace = vector.rotate_vector(accReSensor, self.quat)
# Position and Velocity through integration, assuming 0-velocity at t=0
vel = np.nan*np.ones_like(accReSpace)
pos = np.nan*np.ones_like(accReSpace)
for ii in range(accReSpace.shape[1]):
vel[:,ii] = cumtrapz(accReSpace[:,ii], dx=1./np.float(self.rate), initial=0)
pos[:,ii] = cumtrapz(vel[:,ii], dx=1./np.float(self.rate), initial=initialPosition[ii])
self.vel = vel
self.pos = pos
def test_analyze3dmarkers(self):
t = np.arange(0,10,0.1)
translation = (np.c_[[1,1,0]]*t).T
M = np.empty((3,3))
M[0] = np.r_[0,0,0]
M[1]= np.r_[1,0,0]
M[2] = np.r_[1,1,0]
M -= np.mean(M, axis=0)
q = np.vstack((np.zeros_like(t), np.zeros_like(t),quat.deg2quat(100*t))).T
M0 = vector.rotate_vector(M[0], q) + translation
M1 = vector.rotate_vector(M[1], q) + translation
M2 = vector.rotate_vector(M[2], q) + translation
data = np.hstack((M0,M1,M2))
(pos, ori) = markers.analyze3Dmarkers(data, data[0])
self.assertAlmostEqual(np.max(np.abs(pos-translation)), 0)
self.assertAlmostEqual(np.max(np.abs(ori-q)), 0)
def test_analyze3dmarkers(self):
t = np.arange(0,10,0.1)
translation = (np.c_[[1,1,0]]*t).T
M = np.empty((3,3))
M[0] = np.r_[0,0,0]
M[1]= np.r_[1,0,0]
M[2] = np.r_[1,1,0]
M -= np.mean(M, axis=0)
q = np.vstack((np.zeros_like(t), np.zeros_like(t),quat.deg2quat(100*t))).T
M0 = vector.rotate_vector(M[0], q) + translation
M1 = vector.rotate_vector(M[1], q) + translation
M2 = vector.rotate_vector(M[2], q) + translation
data = np.hstack((M0,M1,M2))
(pos, ori) = markers.analyze3Dmarkers(data, data[0])
self.assertAlmostEqual(np.max(np.abs(pos-translation)), 0)
self.assertAlmostEqual(np.max(np.abs(ori-q)), 0)
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.
Parameters
----------
Gyroscope : array, shape (N,3)
Angular velocity [rad/s]
Accelerometer : array, shape (N,3)
Linear acceleration (Only the direction is used, so units don't matter.)
Magnetometer : array, shape (N,3)
Orientation of local magenetic field.
(Again, only the direction is used, so units don't matter.)
'''
q = self.Quaternion; # short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))
# Estimated direction of gravity and magnetic field
v = np.array([
2*(q[1]*q[3] - q[0]*q[2]),
2*(q[0]*q[1] + q[2]*q[3]),
q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2])
w = np.array([
2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]),
2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]),
2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2)])
# Error is sum of cross product between estimated direction and measured direction of fields
e = np.cross(Accelerometer, v) + np.cross(Magnetometer, w)
if self.Ki > 0:
self._eInt += e * self.SamplePeriod
else:
self._eInt = np.array([0, 0, 0], dtype=np.float)
# Apply feedback terms
Gyroscope += self.Kp * e + self.Ki * self._eInt;
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])).flatten()
# Integrate to yield quaternion
q += qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q)
def find_trajectory(r0, Position, Orientation):
'''
Movement trajetory of a point on an object, from the position and
orientation of a sensor, and the relative position of the point at t=0.
Parameters
----------
r0 : ndarray (3,)
Position of point relative to center of markers, when the object is
in the reference position.
Position : ndarray, shape (N,3)
x/y/z coordinates of COM, relative to the reference position
Orientation : ndarray, shape (N,3)
Orientation relative to reference orientation, expressed as quaternion
Returns
-------
mov : ndarray, shape (N,3)
x/y/z coordinates of the position on the object, relative to the
reference position of the markers
Notes
-----
.. math::
\\vec C(t) = \\vec M(t) + \\vec r(t) = \\vec M(t) +
{{\\bf{R}}_{mov}}(t) \\cdot \\vec r({t_0})
Examples
--------
>>> t = np.arange(0,10,0.1)
>>> translation = (np.c_[[1,1,0]]*t).T
>>> M = np.empty((3,3))
>>> M[0] = np.r_[0,0,0]
>>> M[1]= np.r_[1,0,0]
>>> M[2] = np.r_[1,1,0]
>>> M -= np.mean(M, axis=0)
>>> q = np.vstack((np.zeros_like(t), np.zeros_like(t),quat.deg2quat(100*t))).T
>>> M0 = vector.rotate_vector(M[0], q) + translation
>>> M1 = vector.rotate_vector(M[1], q) + translation
>>> M2 = vector.rotate_vector(M[2], q) + translation
>>> data = np.hstack((M0,M1,M2))
>>> (pos, ori) = signals.analyze_3Dmarkers(data, data[0])
>>> r0 = np.r_[1,2,3]
>>> movement = find_trajectory(r0, pos, ori)
'''
return Position + vector.rotate_vector(r0, Orientation)
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.
Parameters
----------
Gyroscope : array, shape (N,3)
Angular velocity [rad/s]
Accelerometer : array, shape (N,3)
Linear acceleration (Only the direction is used, so units don't matter.)
Magnetometer : array, shape (N,3)
Orientation of local magenetic field.
(Again, only the direction is used, so units don't matter.)
'''
q = self.Quaternion; # short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))
# Gradient decent algorithm corrective step
F = [2*(q[1]*q[3] - q[0]*q[2]) - Accelerometer[0],
2*(q[0]*q[1] + q[2]*q[3]) - Accelerometer[1],
2*(0.5 - q[1]**2 - q[2]**2) - Accelerometer[2],
2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]) - Magnetometer[0],
2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]) - Magnetometer[1],
2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - Magnetometer[2]]
J = np.array([
[-2*q[2], 2*q[3], -2*q[0], 2*q[1]],
[ 2*q[1], 2*q[0], 2*q[3], 2*q[2]],
[0, -4*q[1], -4*q[2], 0],
[-2*b[3]*q[2], 2*b[3]*q[3], -4*b[1]*q[2]-2*b[3]*q[0], -4*b[1]*q[3]+2*b[3]*q[1]],
[-2*b[1]*q[3]+2*b[3]*q[1], 2*b[1]*q[2]+2*b[3]*q[0], 2*b[1]*q[1]+2*b[3]*q[3], -2*b[1]*q[0]+2*b[3]*q[2]],
[ 2*b[1]*q[2], 2*b[1]*q[3]-4*b[3]*q[1], 2*b[1]*q[0]-4*b[3]*q[2], 2*b[1]*q[1]]])
step = J.T.dot(F)
step = vector.normalize(step) # normalise step magnitude
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])) - self.Beta * step
# Integrate to yield quaternion
q = q + qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q).flatten()
def Update(self, Gyroscope, Accelerometer, Magnetometer):
'''Calculate the best quaternion to the given measurement values.
Parameters
----------
Gyroscope : array, shape (N,3)
Angular velocity [rad/s]
Accelerometer : array, shape (N,3)
Linear acceleration (Only the direction is used, so units don't matter.)
Magnetometer : array, shape (N,3)
Orientation of local magenetic field.
(Again, only the direction is used, so units don't matter.)
'''
q = self.Quaternion; # short name local variable for readability
# Reference direction of Earth's magnetic field
h = vector.rotate_vector(Magnetometer, q)
b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))
# Gradient decent algorithm corrective step
F = [2*(q[1]*q[3] - q[0]*q[2]) - Accelerometer[0],
2*(q[0]*q[1] + q[2]*q[3]) - Accelerometer[1],
2*(0.5 - q[1]**2 - q[2]**2) - Accelerometer[2],
2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]) - Magnetometer[0],
2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]) - Magnetometer[1],
2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - Magnetometer[2]]
J = np.array([
[-2*q[2], 2*q[3], -2*q[0], 2*q[1]],
[ 2*q[1], 2*q[0], 2*q[3], 2*q[2]],
[0, -4*q[1], -4*q[2], 0],
[-2*b[3]*q[2], 2*b[3]*q[3], -4*b[1]*q[2]-2*b[3]*q[0], -4*b[1]*q[3]+2*b[3]*q[1]],
[-2*b[1]*q[3]+2*b[3]*q[1], 2*b[1]*q[2]+2*b[3]*q[0], 2*b[1]*q[1]+2*b[3]*q[3], -2*b[1]*q[0]+2*b[3]*q[2]],
[ 2*b[1]*q[2], 2*b[1]*q[3]-4*b[3]*q[1], 2*b[1]*q[0]-4*b[3]*q[2], 2*b[1]*q[1]]])
step = J.T.dot(F)
step = vector.normalize(step) # normalise step magnitude
# Compute rate of change of quaternion
qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])) - self.Beta * step
# Integrate to yield quaternion
q = q + qDot * self.SamplePeriod
self.Quaternion = vector.normalize(q).flatten()
def analytical(
R_initialOrientation=np.eye(3),
omega=np.zeros((5, 3)),
initialPosition=np.zeros(3),
accMeasured=np.column_stack((np.zeros((5, 2)), 9.81 * np.ones(5))),
rate=100):
''' Reconstruct position and orientation with an analytical solution,
from angular velocity and linear acceleration.
Assumes a start in a stationary position. No compensation for drift.
Parameters
----------
R_initialOrientation: ndarray(3,3)
Rotation matrix describing the initial orientation of the sensor,
except a mis-orienation with respect to gravity
omega : ndarray(N,3)
Angular velocity, in [rad/s]
initialPosition : ndarray(3,)
initial Position, in [m]
accMeasured : ndarray(N,3)
Linear acceleration, in [m/s^2]
rate : float
sampling rate, in [Hz]
Returns
-------
q : ndarray(N,3)
Orientation, expressed as a quaternion vector
pos : ndarray(N,3)
Position in space [m]
vel : ndarray(N,3)
Velocity in space [m/s]
Example
-------
>>> q1, pos1 = analytical(R_initialOrientation, omega, initialPosition, acc, rate)
'''
# Transform recordings to angVel/acceleration in space --------------
# Orientation of \vec{g} with the sensor in the "R_initialOrientation"
g = constants.g
g0 = np.linalg.inv(R_initialOrientation).dot(np.r_[0, 0, g])
# for the remaining deviation, assume the shortest rotation to there
q0 = vector.q_shortest_rotation(accMeasured[0], g0)
q_initial = rotmat.convert(R_initialOrientation, to='quat')
# combine the two, to form a reference orientation. Note that the sequence
# is very important!
q_ref = quat.q_mult(q_initial, q0)
# Calculate orientation q by "integrating" omega -----------------
q = quat.calc_quat(omega, q_ref, rate, 'bf')
# Acceleration, velocity, and position ----------------------------
# From q and the measured acceleration, get the \frac{d^2x}{dt^2}
g_v = np.r_[0, 0, g]
accReSensor = accMeasured - vector.rotate_vector(g_v, quat.q_inv(q))
accReSpace = vector.rotate_vector(accReSensor, q)
# Make the first position the reference position
q = quat.q_mult(q, quat.q_inv(q[0]))
# compensate for drift
#drift = np.mean(accReSpace, 0)
#accReSpace -= drift*0.7
# Position and Velocity through integration, assuming 0-velocity at t=0
vel = np.nan * np.ones_like(accReSpace)
pos = np.nan * np.ones_like(accReSpace)
for ii in range(accReSpace.shape[1]):
vel[:, ii] = cumtrapz(accReSpace[:, ii], dx=1. / rate, initial=0)
pos[:, ii] = cumtrapz(vel[:, ii],
dx=1. / rate,
initial=initialPosition[ii])
return (q, pos, vel)
print(quatmult(c,c))
print(quatmult(c,a))
print(quatmult(d,d))
print('The inverse of {0} is {1}'.format(a, quatinv(a)))
print('The inverse of {0} is {1}'.format(d, quatinv(d)))
print('The inverse of {0} is {1}'.format(e, quatinv(e)))
print(quatmult(e, quatinv(e)))
print(quat2vect(a))
print('{0} is {1} degree'.format(a, quat2deg(a)))
print('{0} is {1} degree'.format(c, quat2deg(c)))
print(quat2deg(0.2))
x = np.r_[1,0,0]
vNull = np.r_[0,0,0]
print(rotate_vector(x, a))
v0 = [0., 0., 100.]
vel = np.tile(v0, (1000,1))
rate = 100
out = vel2quat(vel, [0., 0., 0.], rate, 'sf')
print(out[-1:])
plt.plot(out[:,1:4])
plt.show()
print(deg2quat(15))
print(deg2quat(quat2deg(a)))
q = np.array([[0, 0, np.sin(0.1)],
[0, np.sin(0.01), 0]])
def orientation(quats, out_file=None, title_text=None, deltaT=100):
'''Calculates the orienation of an arrow-patch used to visualize a quaternion.
Uses "_update_func" for the display.
Parameters
----------
quats : array [(N,3) or (N,4)]
Quaterions describing the orientation.
out_file : string
Path- and file-name of the animated out-file (".mp4"). [Default=None]
title_text : string
Name of title of animation [Default=None]
deltaT : int
interval between frames [msec]. Smaller numbers make faster
animations.
Example
-------
To visualize a rotation about the (vertical) z-axis:
>>> # Set the parameters
>>> omega = np.r_[0, 10, 10] # [deg/s]
>>> duration = 2
>>> rate = 100
>>> q0 = [1, 0, 0, 0]
>>> out_file = 'demo_patch.mp4'
>>> title_text = 'Rotation Demo'
>>>
>>> # Calculate the orientation
>>> num_rep = duration*rate
>>> omegas = np.tile(omega, [num_rep, 1])
>>> q = skin.quat.calc_quat(omegas, q0, rate, 'sf')
>>>
>>> orientation(q, out_file, 'Well done!', deltaT=1000./rate)
Note
----
Seems to be slow. So unless you need a movie, better use "Orientation_OGL".
'''
# Initialize the 3D-figure
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# Define the arrow-shape and the top/bottom colors
delta = 0.01 # "Thickness" of arrow
corners = [[0, 0, 0.6],
[0.2, -0.2, 0],
[0, 0, 0]]
colors = ['r', 'b']
# Calculate the arrow corners
corner_array = np.column_stack(corners)
corner_arrays = []
corner_arrays.append( corner_array + np.r_[0., 0., delta] )
corner_arrays.append( corner_array - np.r_[0., 0., delta] )
# Calculate the new orientations, given the quaternion orientation
all_corners = []
for quat in quats:
all_corners.append([vector.rotate_vector(corner_arrays[0], quat),
vector.rotate_vector(corner_arrays[1], quat)])
# Animate the whole thing, using 'update_func'
num_frames = len(quats)
ani = animation.FuncAnimation(fig, _update_func, num_frames,
fargs=[all_corners, colors, ax, title_text],
interval=deltaT)
# If requested, save the animation to a file
if out_file is not None:
try:
ani.save(out_file)
print('Animation saved to {0}'.format(out_file))
except ValueError:
print('Sorry, no animation saved!')
print('You probably have to install "ffmpeg", and add it to your PATH.')
plt.show()
return
def analyze_3Dmarkers(MarkerPos, ReferencePos):
'''
Take recorded positions from 3 markers, and calculate center-of-mass (COM) and orientation
Can be used e.g. for the analysis of Optotrac data.
Parameters
----------
MarkerPos : ndarray, shape (N,9)
x/y/z coordinates of 3 markers
ReferencePos : ndarray, shape (1,9)
x/y/z coordinates of markers in the reference position
Returns
-------
Position : ndarray, shape (N,3)
x/y/z coordinates of COM, relative to the reference position
Orientation : ndarray, shape (N,3)
Orientation relative to reference orientation, expressed as quaternion
Example
-------
>>> (PosOut, OrientOut) = analyze_3Dmarkers(MarkerPos, ReferencePos)
'''
# Specify where the x-, y-, and z-position are located in the data
cols = {'x' : r_[(0,3,6)]}
cols['y'] = cols['x'] + 1
cols['z'] = cols['x'] + 2
# Calculate the position
cog = np.vstack(( sum(MarkerPos[:,cols['x']], axis=1),
sum(MarkerPos[:,cols['y']], axis=1),
sum(MarkerPos[:,cols['z']], axis=1) )).T/3.
cog_ref = np.vstack(( sum(ReferencePos[cols['x']]),
sum(ReferencePos[cols['y']]),
sum(ReferencePos[cols['z']]) )).T/3.
position = cog - cog_ref
# Calculate the orientation
numPoints = len(MarkerPos)
orientation = np.ones((numPoints,3))
refOrientation = vector.plane_orientation(ReferencePos[:3], ReferencePos[3:6], ReferencePos[6:])
for ii in range(numPoints):
'''The three points define a triangle. The first rotation is such
that the orientation of the reference-triangle, defined as the
direction perpendicular to the triangle, is rotated along the shortest
path to the current orientation.
In other words, this is a rotation outside the plane spanned by the three
marker points.'''
curOrientation = vector.plane_orientation( MarkerPos[ii,:3], MarkerPos[ii,3:6], MarkerPos[ii,6:])
alpha = vector.angle(refOrientation, curOrientation)
if alpha > 0:
n1 = vector.normalize( np.cross(refOrientation, curOrientation) )
q1 = n1 * np.sin(alpha/2)
else:
q1 = r_[0,0,0]
# Now rotate the triangle into this orientation ...
refPos_after_q1 = vector.rotate_vector(np.reshape(ReferencePos,(3,3)), q1)
'''Find which further rotation in the plane spanned by the three marker points
is necessary to bring the data into the measured orientation.'''
Marker_0 = MarkerPos[ii,:3]
Marker_1 = MarkerPos[ii,3:6]
Vector10 = Marker_0 - Marker_1
vector10_ref = refPos_after_q1[0]-refPos_after_q1[1]
beta = vector.angle(Vector10, vector10_ref)
q2 = curOrientation * np.sin(beta/2)
if np.cross(vector10_ref,Vector10).dot(curOrientation)<=0:
q2 = -q2
orientation[ii,:] = quat.q_mult(q2, q1)
return (position, orientation)
print(quatmult(c, c))
print(quatmult(c, a))
print(quatmult(d, d))
print('The inverse of {0} is {1}'.format(a, quatinv(a)))
print('The inverse of {0} is {1}'.format(d, quatinv(d)))
print('The inverse of {0} is {1}'.format(e, quatinv(e)))
print(quatmult(e, quatinv(e)))
print(quat2vect(a))
print('{0} is {1} degree'.format(a, quat2deg(a)))
print('{0} is {1} degree'.format(c, quat2deg(c)))
print(quat2deg(0.2))
x = np.r_[1, 0, 0]
vNull = np.r_[0, 0, 0]
print(rotate_vector(x, a))
v0 = [0., 0., 100.]
vel = np.tile(v0, (1000, 1))
rate = 100
out = vel2quat(vel, [0., 0., 0.], rate, 'sf')
print(out[-1:])
plt.plot(out[:, 1:4])
plt.show()
print(deg2quat(15))
print(deg2quat(quat2deg(a)))
q = np.array([[0, 0, np.sin(0.1)], [0, np.sin(0.01), 0]])
rMat = quat2rotmat(q)
def analytical(R_initialOrientation=np.eye(3),
omega=np.zeros((5,3)),
initialPosition=np.zeros(3),
accMeasured=np.column_stack((np.zeros((5,2)), 9.81*np.ones(5))),
rate=100):
''' Reconstruct position and orientation with an analytical solution,
from angular velocity and linear acceleration.
Assumes a start in a stationary position. No compensation for drift.
Parameters
----------
R_initialOrientation: ndarray(3,3)
Rotation matrix describing the initial orientation of the sensor,
except a mis-orienation with respect to gravity
omega : ndarray(N,3)
Angular velocity, in [rad/s]
initialPosition : ndarray(3,)
initial Position, in [m]
accMeasured : ndarray(N,3)
Linear acceleration, in [m/s^2]
rate : float
sampling rate, in [Hz]
Returns
-------
q : ndarray(N,3)
Orientation, expressed as a quaternion vector
pos : ndarray(N,3)
Position in space [m]
vel : ndarray(N,3)
Velocity in space [m/s]
Example
-------
>>> q1, pos1 = analytical(R_initialOrientation, omega, initialPosition, acc, rate)
'''
# Transform recordings to angVel/acceleration in space --------------
# Orientation of \vec{g} with the sensor in the "R_initialOrientation"
g = constants.g
g0 = np.linalg.inv(R_initialOrientation).dot(np.r_[0,0,g])
# for the remaining deviation, assume the shortest rotation to there
q0 = vector.q_shortest_rotation(accMeasured[0], g0)
q_initial = rotmat.convert(R_initialOrientation, to='quat')
# combine the two, to form a reference orientation. Note that the sequence
# is very important!
q_ref = quat.q_mult(q_initial, q0)
# Calculate orientation q by "integrating" omega -----------------
q = quat.calc_quat(omega, q_ref, rate, 'bf')
# Acceleration, velocity, and position ----------------------------
# From q and the measured acceleration, get the \frac{d^2x}{dt^2}
g_v = np.r_[0, 0, g]
accReSensor = accMeasured - vector.rotate_vector(g_v, quat.q_inv(q))
accReSpace = vector.rotate_vector(accReSensor, q)
# Make the first position the reference position
q = quat.q_mult(q, quat.q_inv(q[0]))
# compensate for drift
#drift = np.mean(accReSpace, 0)
#accReSpace -= drift*0.7
# Position and Velocity through integration, assuming 0-velocity at t=0
vel = np.nan*np.ones_like(accReSpace)
pos = np.nan*np.ones_like(accReSpace)
for ii in range(accReSpace.shape[1]):
vel[:,ii] = cumtrapz(accReSpace[:,ii], dx=1./rate, initial=0)
pos[:,ii] = cumtrapz(vel[:,ii], dx=1./rate, initial=initialPosition[ii])
return (q, pos, vel)
def orientation(quats, out_file=None, title_text=None, deltaT=100):
'''Calculates the orienation of an arrow-patch used to visualize a quaternion.
Uses "_update_func" for the display.
Parameters
----------
quats : array [(N,3) or (N,4)]
Quaterions describing the orientation.
out_file : string
Path- and file-name of the animated out-file (".mp4"). [Default=None]
title_text : string
Name of title of animation [Default=None]
deltaT : int
interval between frames [msec]. Smaller numbers make faster
animations.
Example
-------
To visualize a rotation about the (vertical) z-axis:
>>> # Set the parameters
>>> omega = np.r_[0, 10, 10] # [deg/s]
>>> duration = 2
>>> rate = 100
>>> q0 = [1, 0, 0, 0]
>>> out_file = 'demo_patch.mp4'
>>> title_text = 'Rotation Demo'
>>>
>>> # Calculate the orientation
>>> num_rep = duration*rate
>>> omegas = np.tile(omega, [num_rep, 1])
>>> q = skin.quat.calc_quat(omegas, q0, rate, 'sf')
>>>
>>> orientation(q, out_file, 'Well done!', deltaT=1000./rate)
Note
----
Seems to be slow. So unless you need a movie, better use "Orientation_OGL".
'''
# Initialize the 3D-figure
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# Define the arrow-shape and the top/bottom colors
delta = 0.01 # "Thickness" of arrow
corners = [[0, 0, 0.6], [0.2, -0.2, 0], [0, 0, 0]]
colors = ['r', 'b']
# Calculate the arrow corners
corner_array = np.column_stack(corners)
corner_arrays = []
corner_arrays.append(corner_array + np.r_[0., 0., delta])
corner_arrays.append(corner_array - np.r_[0., 0., delta])
# Calculate the new orientations, given the quaternion orientation
all_corners = []
for quat in quats:
all_corners.append([
vector.rotate_vector(corner_arrays[0], quat),
vector.rotate_vector(corner_arrays[1], quat)
])
# Animate the whole thing, using 'update_func'
num_frames = len(quats)
ani = animation.FuncAnimation(fig,
_update_func,
num_frames,
fargs=[all_corners, colors, ax, title_text],
interval=deltaT)
# If requested, save the animation to a file
if out_file is not None:
try:
ani.save(out_file)
print('Animation saved to {0}'.format(out_file))
except ValueError:
print('Sorry, no animation saved!')
print(
'You probably have to install "ffmpeg", and add it to your PATH.'
)
plt.show()
return
