def processData(data):
if isinstance(data, Quaternion):
return data
elif isinstance(data, list) and len(data) == 4:
return Quaternion(data)
else:
return Quaternion()
def generate_shapes(self, nodes):
nodes = [Vector3(aNode) for aNode in nodes]
theta = math.pi / 2
quaternions = {}
for i in range(4):
quaternions[(i, 0, 0)] = Quaternion.from_x_rotation(i * theta)
quaternions[(0, i, 0)] = Quaternion.from_y_rotation(i * theta)
quaternions[(0, 0, i)] = Quaternion.from_z_rotation(i * theta)
shapes = {}
for x_rot in (3, 2, 1, 0):
new_rotation = quaternions[(x_rot, 0, 0)]
rotated_nodes_x = self.shape_constrained([new_rotation * aNode for aNode in nodes], zero_align=True)
for y_rot in (3, 2, 1, 0):
new_rotation = quaternions[(0, y_rot, 0)]
rotated_nodes_y = self.shape_constrained([new_rotation * aNode for aNode in rotated_nodes_x], zero_align=True)
for z_rot in (3, 2, 1, 0):
new_rotation = quaternions[(0, 0, z_rot)]
rotated_nodes = self.shape_constrained([new_rotation * aNode for aNode in rotated_nodes_y], zero_align=True)
for x_shift in (2, 1, 0):
for y_shift in (2, 1, 0):
for z_shift in (-2, -1, 0):
new_shift = Vector3([float(x_shift), float(y_shift), float(z_shift)])
shifted_nodes = [new_shift + aNode for aNode in rotated_nodes]
shape = self.shape_constrained(shifted_nodes)
shape = frozenset([Cube(int(round(aNode.x)), int(round(aNode.y)), int(round(aNode.z))) for aNode in shape])
shapes[shape] = (x_rot, y_rot, z_rot, x_shift, y_shift, z_shift)
return shapes
def __init__(self, *args):
if len(args) == 1:
if type(args[0]) is not HexapodLeg:
raise TypeError("Copy constructor expects a single HexapodLeg argument.");
o = args[0];
self.id = o.id;
self.body = o.body;
self.direction = o.direction;
self.ref = o.ref;
self.world_ref = o.world_ref;
self.segments = [];
prevSeg = self;
for seg in o.segments:
nextSeg = seg.clone(prevSeg);
self.segments.append(nextSeg);
prevSeg = nextSeg;
elif len(args) == 4:
config, body, world_ref, legid = args;
self.id = legid;
self.body = body;
disp = config.getLegDisplacement(self);
self.ref = ReferenceFrame(disp, Quaternion.from_z_rotation(0.), body.ref);
self.world_ref = ReferenceFrame(disp, Quaternion.from_z_rotation(0.), world_ref);
self.segments = body.getLegSegments(config, self);
self.direction = body.getLegDirection(self);
else:
raise TypeError("Unexpected");
async def serial_listener():
try:
irl_rot_off = Quaternion.from_x_rotation(
0
) # supply your own imu offsets here, has to be a Quaternion object
with serial.Serial(SERIAL_PORT, SERIAL_BAUD, timeout=1 / 4) as ser:
with serial.threaded.ReaderThread(
ser, u.SerialReaderFactory) as protocol:
protocol.write_line("nut")
await asyncio.sleep(5)
print('serial_listener: starting tracking thread')
while poser.coro_keep_alive["serial_listener"].is_alive:
gg = u.get_numbers_from_text(protocol.last_read, ",")
if len(gg) >= 4:
w, x, y, z, *_ = gg
my_q = Quaternion([-y, z, -x, w])
my_q = Quaternion(my_q * irl_rot_off).normalised
poser.poses[0].r_w = round(my_q[3], 5)
poser.poses[0].r_x = round(my_q[0], 5)
poser.poses[0].r_y = round(my_q[1], 5)
poser.poses[0].r_z = round(my_q[2], 5)
await asyncio.sleep(
poser.coro_keep_alive["serial_listener"].sleep_delay)
except Exception as e:
print(f'serial_listener: failed {e}')
poser.coro_keep_alive["serial_listener"].is_alive = False
def __init__(self, *args):
if len(args) == 1:
if type(args[0]) is not HexapodLeg:
raise TypeError(
"Copy constructor expects a single HexapodLeg argument.")
o = args[0]
self.id = o.id
self.body = o.body
self.direction = o.direction
self.ref = o.ref
self.world_ref = o.world_ref
self.segments = []
prevSeg = self
for seg in o.segments:
nextSeg = seg.clone(prevSeg)
self.segments.append(nextSeg)
prevSeg = nextSeg
elif len(args) == 4:
config, body, world_ref, legid = args
self.id = legid
self.body = body
disp = config.getLegDisplacement(self)
self.ref = ReferenceFrame(disp, Quaternion.from_z_rotation(0.),
body.ref)
self.world_ref = ReferenceFrame(disp,
Quaternion.from_z_rotation(0.),
world_ref)
self.segments = body.getLegSegments(config, self)
self.direction = body.getLegDirection(self)
else:
raise TypeError("Unexpected")
def test_oo_examples(self):
from pyrr import Quaternion, Matrix44, Vector3
import numpy as np
point = Vector3([1., 2., 3.])
orientation = Quaternion()
translation = Vector3()
scale = Vector3([1., 1., 1.])
# translate along X by 1
translation += [1.0, 0.0, 0.0]
# rotate about Y by pi/2
rotation = Quaternion.from_y_rotation(np.pi / 2.0)
orientation = rotation * orientation
# create a matrix
# start our matrix off using the scale
matrix = Matrix44.from_scale(scale)
# apply our orientation
# we can multiply matricies and quaternions directly!
matrix = matrix * orientation
# apply our translation
translation = Matrix44.from_translation(translation)
matrix = matrix * translation
# transform our point by the matrix
# vectors are transformable by matrices and quaternions directly
point = matrix * point
def __compute_bbox_coords(self, mesh, view):
upright = Quaternion.from_y_rotation(np.pi / 2)
rotation = Quaternion(mesh['rotation'])
center = mesh['translation']
world_corners = {
'top_left':
Vector3([center[0], center[1], center[2]]) +
(rotation * upright *
Vector3([0, mesh['width'] / 2, mesh['height'] / 2])),
'top_right':
Vector3([center[0], center[1], center[2]]) +
(rotation * upright *
Vector3([0, -mesh['width'] / 2, mesh['height'] / 2])),
'bottom_right':
Vector3([center[0], center[1], center[2]]) +
(rotation * upright *
Vector3([0, -mesh['width'] / 2, -mesh['height'] / 2])),
'bottom_left':
Vector3([center[0], center[1], center[2]]) +
(rotation * upright *
Vector3([0, mesh['width'] / 2, -mesh['height'] / 2]))
}
hidden_corners = 0
left = right = top = bottom = None
for key, value in world_corners.items():
img_coords = self.__project_to_img_frame(value, view)
if (img_coords[0] < 10 or img_coords[0] > (self.width - 10)
or img_coords[1] < 10 or img_coords[1] >
(self.height - 10)):
hidden_corners += 1
if left is None or (img_coords[0] < left['x']):
left = {'x': img_coords[0], 'y': img_coords[1]}
if top is None or (img_coords[1] < top['y']):
top = {'x': img_coords[0], 'y': img_coords[1]}
if bottom is None or (img_coords[1] > bottom['y']):
bottom = {'x': img_coords[0], 'y': img_coords[1]}
if right is None or (img_coords[0] > right['x']):
right = {'x': img_coords[0], 'y': img_coords[1]}
image_corners = {
'min': [int(left['x']), int(top['y'])],
'max': [int(right['x']), int(bottom['y'])]
}
if hidden_corners > 3:
return {}
elif hidden_corners > 0:
for key, img_coords in image_corners.items():
for i in range(0, 2):
if img_coords[i] < 0:
img_coords[i] = 0
elif img_coords[i] > (self.width
if i == 0 else self.height):
img_coords[i] = self.width if i == 0 else self.height
image_corners[key] = img_coords
return image_corners
def test_euler_to_quaternion_1(self):
v = np.radians(Vector3([0, 0, 0]))
self.assertQuaternionAreEqual(euler_angles_to_quaternion(v), (Quaternion([0, 0, 0, 1])))
v = np.radians([0, 0, 180])
self.assertQuaternionAreEqual(euler_angles_to_quaternion(v), (Quaternion([0, 0, 1, 0])))
v = np.radians([0, 180, 0])
self.assertQuaternionAreEqual(euler_angles_to_quaternion(v), (Quaternion([0, 1, 0, 0])))
v = np.radians([180, 0, 0])
self.assertQuaternionAreEqual(euler_angles_to_quaternion(v), (Quaternion([1, 0, 0, 0])))
def __rotate(self, axis, deg, local):
if local:
row = axis.tolist().index(1)
localBasis = self.orientation.matrix33[row, :]
rot = Quaternion.from_axis_rotation(localBasis,
np.deg2rad(deg),
dtype=np.float32)
else:
rot = Quaternion.from_axis_rotation(axis,
np.deg2rad(deg),
dtype=np.float32)
self.orientation *= rot
def from_to_quaternion(v1, v2, is_unit=True, epsilon=1e-5):
# https://stackoverflow.com/a/11741520/782170
if not is_unit:
v1 = unit_vector(v1)
v2 = unit_vector(v2)
k_cos_theta = np.dot(v1, v2)
k = np.sqrt(np.dot(v1, v1) * np.dot(v2, v2))
if abs(k_cos_theta / k + 1) < epsilon:
# 180 degree rotation around any orthogonal vector
return Quaternion(0, unit_vector(perpendicular_vector(v1)))
q = Quaternion(k_cos_theta + k, *np.cross(v1, v2))
return q.normalize
def get_grav(self, q=None, magnitude=None, stationary=False, accel=None):
if stationary or q is None or ((accel is not None) and
(not np.any(accel))):
g = np.asarray([self._acc_x, self._acc_y, self._acc_z])
self.grav_magnitude = sum(abs(np.asarray(g)))
return g
elif q is not None:
if not isinstance(q, Quaternion):
q = Quaternion.from_euler(*q)
if magnitude is None:
magnitude = self.grav_magnitude
# http://www.varesano.net/blog/fabio/simple-gravity-compensation-9-dom-imus
g = np.asarray([0.0, 0.0, 0.0])
# get expected direction of gravity
g[0] = q.x * magnitude
g[1] = q.y * magnitude
g[2] = q.z * magnitude
return g
elif accel is not None:
g = np.asarray([self._acc_x, self._acc_y, self._acc_z])
g -= accel
self.grav_magnitude = sum(abs(np.asarray(g)))
return g
def rotate_vertical(self, d):
rotation = Quaternion.from_x_rotation(
d * float(self._rotate_vertically) * np.pi / 180)
#self._camera_front[1] = min(1, max(-1, (rotation * self._camera_front)[1]))
self._camera_front[1] = min(
2.5, max(-2.5, self._camera_front[1] + 0.005 * -d))
self.build_look_at()
def draw():
"""Put the main drawing code in here."""
def inc(x, amount=1):
return (x + amount) % 4
def wlc(x):
return (x >> 4) | x
if chaem.stat == chaem.states.stop:
x, z = int((chaem.pos.z + 1) / 2), int((chaem.pos.x + 1) / 2)
spot = room[x][z]
dire = inc(int(round(chaem.heading * 2 / pi)), 1) # Get this into room coordinates (0-3)
for x in [3, 0, 1, 2]: # Check each side, starting with the right.
if not wlc(spot) & (2 ** (inc(dire, x))):
dire = inc(dire, x)
break
chaem.movdir = quaternion.from_y_rotation((dire - 1) * pi / 2) * vector([0., 0., -1.])
chaem.mochae(timedelta)
GL.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT)
GL.glUniformMatrix4fv(GL.glGetUniformLocation(shaderp, 'modelmatrix'), 1, False, modelmtx)
GL.glUniformMatrix4fv(GL.glGetUniformLocation(shaderp, 'viewmatrix'), 1, False, chaem.lookat())
GL.glBindVertexArray(architincture)
GL.glEnable(GL.GL_TEXTURE_2D)
GL.glBindTexture(GL.GL_TEXTURE_2D, terrain)
GL.glDrawArrays(GL.GL_QUADS, 0, len(vs))
GL.glBindTexture(GL.GL_TEXTURE_2D, 0)
GL.glBindVertexArray(0)
def euler_angles_to_quaternion(eulers, dtype=None):
"""Creates a quaternion from a set of Euler angles.
Rotation order for euler angles is YZX.
See: http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
"""
dtype = dtype or eulers.dtype
heading_half = eulers[1] / 2.
attitude_half = eulers[2] / 2.
bank_half = eulers[0] / 2.
c1 = np.math.cos(heading_half)
s1 = np.math.sin(heading_half)
c2 = np.math.cos(attitude_half)
s2 = np.math.sin(attitude_half)
c3 = np.math.cos(bank_half)
s3 = np.math.sin(bank_half)
c1c2 = c1 * c2
s1s2 = s1 * s2
return Quaternion(
[
c1c2 * s3 + s1s2 * c3,
s1 * c2 * c3 + c1 * s2 * s3,
c1 * s2 * c3 - s1 * c2 * s3,
c1c2 * c3 - s1s2 * s3,
],
dtype=dtype
)
def test_oo_examples(self):
from pyrr import Quaternion, Matrix44, Vector3
import numpy as np
point = Vector3([1.,2.,3.])
orientation = Quaternion()
translation = Vector3()
scale = Vector3([1.,1.,1.])
# translate along X by 1
translation += [1.0, 0.0, 0.0]
# rotate about Y by pi/2
rotation = Quaternion.from_y_rotation(np.pi / 2.0)
orientation = rotation * orientation
# create a matrix
# start our matrix off using the scale
matrix = Matrix44.from_scale(scale)
# apply our orientation
# we can multiply matricies and quaternions directly!
matrix = matrix * orientation
# apply our translation
translation = Matrix44.from_translation(translation)
matrix = matrix * translation
# transform our point by the matrix
# vectors are transformable by matrices and quaternions directly
point = matrix * point
def convert_rvec_to_quaternion(self, rvec):
'''Convert rvec (which is log quaternion) to quaternion'''
theta = np.sqrt(rvec[0] * rvec[0] + rvec[1] * rvec[1] + rvec[2] * rvec[2]) # in radians
raxis = [rvec[0] / theta, rvec[1] / theta, rvec[2] / theta]
# pyrr's Quaternion (order is XYZW), https://pyrr.readthedocs.io/en/latest/oo_api_quaternion.html
return Quaternion.from_axis_rotation(raxis, theta)
def mouseMoveEvent(self, event):
pos = event.pos()
# compute point on sphere under pointer
(w, h) = self.viewport
t = (2*self.old_pos.x() - w) / float(w)
u = -(2*self.old_pos.y() - h) / float(h)
# compute inverse of view transform ignoring rotation
m = Matrix44.from_translation(Vector3([0, 0, -self.zoom])) * self.projTransform
m = matrix44.inverse(m)
rayOri = m * Vector3([t, u, -1])
rayEnd = m * Vector3([t, u, 1])
rayDir = rayEnd - rayOri
self.picked = intersectRayUnitSphere(rayOri, rayDir)
# rotate on left-drag
if event.buttons() & QtCore.Qt.LeftButton > 0:
# the rotation vector is the displacement vector rotated by 90 degrees
dx = pos.x() - self.old_pos.x()
dy = pos.y() - self.old_pos.y()
if dx == 0 and dy == 0:
return
v = Vector3([dy, dx, 0])
# update the current orientation
self.layers.multiplyOrientation(Quaternion.from_axis_rotation(
-v.normalised,
-v.length * 0.002,
))
elif event.buttons() & QtCore.Qt.RightButton > 0:
dz = pos.y() - self.old_pos.y()
self.zoom = max(0, self.zoom + dz / 100.0)
self.old_pos = pos
self.update()
def yaw(self, value):
"""Rotate using up direction
"""
rotation = Quaternion.from_axis_rotation(self._up, value * self._speed)
self._forward = normalize(
quaternion.apply_to_vector(rotation, self._forward))
return self
def run_get_i2c_imu():
try:
nonlocal uvw
coms = get_coms_in_range()
con, pro = get_i2c_imu(coms[0])
pro.start()
acc = (float(0), ) * 3
while True:
orient1 = pro.protocol.imu.get_instant_orientation()
if np.isnan(acc).any():
acc = (float(0), ) * 3
if not np.isnan(orient1).any():
qxyz = [
orient1 * Quaternion.from_axis(v) * ~orient1
for v in uvw.transpose()
]
xyz = np.asarray([np.asarray(x.xyz)
for x in qxyz]).transpose()
obj.mlab_source.set(x=xyz[0], y=xyz[1], z=xyz[2])
time.sleep(0)
yield
except:
e = traceback.format_exc()
print(e)
print("ded")
def roll(self, value):
"""Rotate using the forward direction
"""
rotation = Quaternion.from_axis_rotation(self._forward,
value * self._speed)
self._up = normalize(quaternion.apply_to_vector(rotation, self._up))
return self
async def serial_listener3(self):
"""Get orientation data from serial."""
irl_rot_off = Quaternion.from_x_rotation(
np.pi /
3) # imu on this controller is rotated 90 degrees irl for me
my_off = Quaternion()
if not check_serial_dict(self.serialPaths, 'cont_r'):
print(
'failed to init serial_listener3: bad serial dict for key \'cont_r\''
)
self.coro_keep_alive["serial_listener3"][0] = False
return
with serial.Serial(self.serialPaths["cont_r"], 115200,
timeout=1 / 5) as ser2:
with serial.threaded.ReaderThread(
ser2, u.SerialReaderFactory) as protocol:
protocol.write_line("nut")
await asyncio.sleep(5)
while self.coro_keep_alive["serial_listener3"][0]:
try:
gg = u.get_numbers_from_text(protocol.last_read, ',')
if len(gg) > 0:
w, x, y, z = gg
my_q = Quaternion([-y, z, -x, w])
if self._serialResetYaw:
my_off = Quaternion([0, z, 0,
w]).inverse.normalised
my_q = my_off * my_q * irl_rot_off
self.pose_controller_r.r_w = round(my_q[3], 5)
self.pose_controller_r.r_x = round(my_q[0], 5)
self.pose_controller_r.r_y = round(my_q[1], 5)
self.pose_controller_r.r_z = round(my_q[2], 5)
await asyncio.sleep(
self.coro_keep_alive["serial_listener3"][1])
except Exception as e:
print(f"{self.serial_listener.__name__}: {e}")
break
self.coro_keep_alive["serial_listener3"][0] = False
def orient(self, x, y):
""" Orient the current camera with the current x, y
The function will use two quaternions for computing the final rotation. By using
the current up and side vectors of the camera. The angle used for the rotations
are x and y passed by parameters. The function will normalize the new axis vectors
for the camera by the current rotation.
Remark: there is no translation like in Orbit functionality.
"""
side = normalize(np.cross(self._up, self._forward))
rotationYaw = Quaternion.from_axis_rotation(
self._up, -x * self._speed * self._sensitivity)
rotationPitch = Quaternion.from_axis_rotation(
side, y * self._speed * self._sensitivity)
rotation = rotationYaw * rotationPitch
self._forward = normalize(
quaternion.apply_to_vector(rotation, self._forward))
return self
def __init__(self, config):
# Prepare the root reference frame.
self.ref = ReferenceFrame(Vector3([0.,0.,0.]), Quaternion.from_z_rotation(0.));
self.body = HexapodBody(self.ref, config);
self.legs = [HexapodLeg(config, self.body, self.ref, legid) for legid in config.getLegs()];
self.mp = HexapodMotionPlanner(config, self.body, self.legs);
self.stepmp = HexapodStepMotionPlanner(config, self, self.mp, self.legs, config.getLegPhases()[2]);
self.stepmp.start();
def buildBoneMatrices(tree, parentmat):
trans = tree['transform']
translate = Matrix44.from_translation(trans['position'])
rotate = Quaternion(trans['rotation']).matrix44
mat = mats[tree['bone']] = rotate * translate * parentmat
for x in tree['children']:
buildBoneMatrices(x, mat)
def frame(self):
turning = False
if keys[b's'[0]]:
self.ori = self.ori * Quaternion.from_x_rotation(-self.rot_speed)
turning = True
if keys[b'w'[0]]:
self.ori = self.ori * Quaternion.from_x_rotation(+self.rot_speed)
turning = True
if keys[b'a'[0]]:
self.ori = self.ori * Quaternion.from_y_rotation(+self.rot_speed)
turning = True
if keys[b'd'[0]]:
self.ori = self.ori * Quaternion.from_y_rotation(-self.rot_speed)
turning = True
if keys[b'q'[0]]:
self.ori = self.ori * Quaternion.from_z_rotation(-self.rot_speed)
turning = True
if keys[b'e'[0]]:
self.ori = self.ori * Quaternion.from_z_rotation(+self.rot_speed)
turning = True
if self.rot_speed < Player.rot_speed and turning:
self.rot_speed += Player.rot_acc
elif self.rot_speed > 0:
self.rot_speed -= Player.rot_acc
self.dir = self.ori * Vector3((0., 0., 1.))
self.pos = self.pos + Player.speed * self.dir
if self.pos.length > 35.:
self.ori = self.ori * Quaternion.from_y_rotation(3.14159265)
def test_euler_equivalence(self):
eulers = euler.create_from_x_rotation(np.pi / 2.)
m = Matrix33.from_x_rotation(np.pi / 2.)
q = Quaternion.from_x_rotation(np.pi / 2.)
qm = Matrix33.from_quaternion(q)
em = Matrix33.from_eulers(eulers)
self.assertTrue(np.allclose(qm, m))
self.assertTrue(np.allclose(qm, em))
self.assertTrue(np.allclose(m, em))
def pitch(self, value):
""" Rotate using cross product between up and forward vectors (side).
"""
side = normalize(np.cross(self._up, self._forward))
rotation = Quaternion.from_axis_rotation(side, value * self._speed)
self._forward = normalize(
quaternion.apply_to_vector(rotation, self._forward))
self._up = normalize(np.cross(self._forward, side))
return self
def computeForwardKinematics(self):
s_pos, s_rot, joint_axis = self.prev.computeForwardKinematics();
p_pos, p_rot = s_pos[-1], s_rot[-1];
c_rot = p_rot * Quaternion.from_axis_rotation(self.rot_ax, self.start_angle + self.angle);
c_pos = p_pos + c_rot * self.disp;
ja = p_rot * Vector3(self.rot_ax);
return (s_pos + [c_pos]), (s_rot + [c_rot]), (joint_axis + [ja]);
def test_euler_equivalence(self):
eulers = euler.create_from_x_rotation(np.pi / 2.)
m = Matrix33.from_x_rotation(np.pi / 2.)
q = Quaternion.from_x_rotation(np.pi / 2.)
qm = Matrix33.from_quaternion(q)
em = Matrix33.from_eulers(eulers)
self.assertTrue(np.allclose(qm, m))
self.assertTrue(np.allclose(qm, em))
self.assertTrue(np.allclose(m, em))
def test_conversions(self):
from pyrr import Quaternion, Matrix33, Matrix44, Vector3, Vector4
v3 = Vector3([1., 0., 0.])
v4 = Vector4.from_vector3(v3, w=1.0)
v3, w = Vector3.from_vector4(v4)
m44 = Matrix44()
q = Quaternion(m44)
m33 = Matrix33(q)
m33 = Matrix44().matrix33
m44 = Matrix33().matrix44
q = Matrix44().quaternion
q = Matrix33().quaternion
m33 = Quaternion().matrix33
m44 = Quaternion().matrix44
def __init__(self):
self.location = Vector3()
self.scale = Vector3([1, 1, 1])
self.rotation = Rotator([0.0, 0.0, 0.0])
self.quaternion = Quaternion([0, 0, 0, 1])
self.initial_matrix = Matrix44.identity()
self.transform_matrix = Matrix44.identity()
# Flag indicate whether the transformation is modified or not
self.is_changed = False
def computeForwardKinematics(self):
s_pos, s_rot, joint_axis = self.prev.computeForwardKinematics()
p_pos, p_rot = s_pos[-1], s_rot[-1]
c_rot = p_rot * Quaternion.from_axis_rotation(
self.rot_ax, self.start_angle + self.angle)
c_pos = p_pos + c_rot * self.disp
ja = p_rot * Vector3(self.rot_ax)
return (s_pos + [c_pos]), (s_rot + [c_rot]), (joint_axis + [ja])
def test_quaternion_matrix_conversion(self):
# https://au.mathworks.com/help/robotics/ref/quat2rotm.html?requestedDomain=www.mathworks.com
q = Quaternion([0.7071, 0., 0., 0.7071])
m33 = q.matrix33
expected = np.array([
[1., 0., 0.],
[0., -0., -1.],
[0., 1., -0.],
])
self.assertTrue(np.allclose(m33, expected))
# issue #42
q = Quaternion([0.80087974, 0.03166748, 0.59114721, -0.09018753])
m33 = q.matrix33
q2 = Quaternion.from_matrix(m33)
print(q, q2)
self.assertTrue(np.allclose(q, q2))
q3 = Quaternion.from_matrix(m33.T)
self.assertFalse(np.allclose(q2, q3))
def setData(self, data):
if isinstance(data, Quaternion):
self._data = data
elif isinstance(data, list) and len(data) == 4:
# here serialized data will be handled
# when node desirializes itself, it creates all pins
# and then sets data to them. Here, data will be set fo the first time after deserialization
self._data = Quaternion(data)
else:
self._data = self.defaultValue()
PinWidgetBase.setData(self, self._data)
def test_m44_q_equivalence(self):
"""Test for equivalance of matrix and quaternion rotations.
Create a matrix and quaternion, rotate each by the same values
then convert matrix<->quaternion and check the results are the same.
"""
m = Matrix44.from_x_rotation(np.pi / 2.)
mq = Quaternion.from_matrix(m)
q = Quaternion.from_x_rotation(np.pi / 2.)
qm = Matrix44.from_quaternion(q)
self.assertTrue(np.allclose(np.dot([1., 0., 0., 1.], m), [1., 0., 0., 1.]))
self.assertTrue(np.allclose(np.dot([1., 0., 0., 1.], qm), [1., 0., 0., 1.]))
self.assertTrue(np.allclose(q * Vector4([1., 0., 0., 1.]), [1., 0., 0., 1.]))
self.assertTrue(np.allclose(mq * Vector4([1., 0., 0., 1.]), [1., 0., 0., 1.]))
np.testing.assert_almost_equal(np.array(q), np.array(mq), decimal=5)
np.testing.assert_almost_equal(np.array(m), np.array(qm), decimal=5)
def test_quaternion_matrix_conversion(self):
# https://au.mathworks.com/help/robotics/ref/quat2rotm.html?requestedDomain=www.mathworks.com
q = Quaternion([0.7071, 0., 0., 0.7071])
m33 = q.matrix33
expected = np.array([
[1., 0., 0.],
[0.,-0.,-1.],
[0., 1.,-0.],
])
self.assertTrue(np.allclose(m33, expected))
# issue #42
q = Quaternion([0.80087974, 0.03166748, 0.59114721,-0.09018753])
m33 = q.matrix33
q2 = Quaternion.from_matrix(m33)
print(q, q2)
self.assertTrue(np.allclose(q, q2))
q3 = Quaternion.from_matrix(m33.T)
self.assertFalse(np.allclose(q2, q3))
def rotate_z(self, angle: float, local_space: bool = True) -> None:
""" Rotate object around its z-axis.
Args:
angle: The rotation angle in degrees.
local_space: If True rotate in local coordinate system. Otherwise in world space.
"""
rotation_quaternion = Quaternion.from_z_rotation(angle)
if local_space:
self.local_quaternion *= rotation_quaternion
else:
self.world_quaternion *= rotation_quaternion
def __init__(self, name: str = "New Node"):
""" Node element of scene graph (tree structure).
Args:
name: Name for string representation.
"""
self.children = list()
self._parent = None
self.name = name
self._local_position = Vector3()
self._world_position = Vector3()
self._scale = Vector3([1., 1., 1.])
self._local_quaternion = Quaternion()
self._world_quaternion = Quaternion()
self._world_matrix = Matrix44.identity()
self._local_matrix = Matrix44.identity()
self.__matrix_needs_update = True
async def nut(self):
my_qq = np.array([1, 0, 0, 0], dtype=np.float64)
my_off = Quaternion()
my_off2 = Quaternion.from_x_rotation(np.pi/2)*Quaternion.from_y_rotation(np.pi/2)
mig = ahrs.filters.Madgwick(frequency=100.0)
gg = []
while self.coro_keep_alive['nut'].is_alive:
try:
if self.last_read:
gg = u.get_numbers_from_text(self.last_read.decode().strip('\r'), ',')
self.last_read = b''
if len(gg) == 9 and len(self.poses) > 3:
# my_qq = mig.updateIMU(my_qq, gg[3:6], gg[6:])
my_qq = mig.updateMARG(my_qq, gg[3:6], gg[6:], gg[:3])
# print (my_qq)
temp = Quaternion(my_off*Quaternion([*my_qq[1:], my_qq[0]])*my_off2).normalised
self.poses[3].r_w = temp[3]
self.poses[3].r_y = temp[0]
self.poses[3].r_z = temp[1]
self.poses[3].r_x = temp[2]
if self._serialResetYaw:
my_off = (Quaternion([*my_qq[1:], my_qq[0]])*my_off2).inverse.normalised
mig = ahrs.filters.Madgwick(frequency=100.0)
except Exception as e:
print (f'nut: dead, reason: {e}')
# break
await asyncio.sleep(self.coro_keep_alive['nut'].sleep_delay)
def mochae(self, timedelta: float) -> None:
"""Do moving, but look slowly."""
comp = self.dir ^ self.movdir # Comparison reference, for wisity checking.
rent = arctan2(self.ure | comp, self.movdir | self.dir) # Absolute angle of movdir?
if abs(rent) <= abs(self.rotspe * timedelta):
self.dir = self.movdir.copy() # Snap to alignment, don't risk rounding errors.
else:
r = Quaternion.from_axis_rotation(comp if comp.length else self.ure, self.rotspe * timedelta)
self.dir = r * self.dir # move over a little.
if self.stat == self.states.forw:
tent, rest = self.movdir * self.latspe * timedelta, self.orgp + self.movdir*2 - self.pos
if tent.length > rest.length: # Snap to grid.
self.pos = ((self.pos + rest) * 2).round() / 2
self.stat = self.states.stop
else:
self.pos += tent
def __init__(self, base_trans=None, base_rotate=None, parent=None):
if base_trans is None:
base_trans = Vector3([0.0, 0.0, 0.0])
if base_rotate is None:
base_rotate = Quaternion.from_eulers([0.0, 0.0, 0.0])
if type(base_trans) is not Vector3:
raise TypeError("Oops, trans is not a vector.")
if type(base_rotate) is not Quaternion:
raise TypeError("Oops, rotate is not a vector.")
if parent is not None and type(parent) is not ReferenceFrame:
raise TypeError("parent is not a ReferenceFrame.")
self.parent = parent
self.base_trans = base_trans
self.base_rotate = base_rotate
self.setTranslation([0.0, 0.0, 0.0])
self.setRotation([0.0, 0.0, 0.0])
def __init__(self, params, hexa, mp, leg, phase_group, phase_group_max):
self.radius = params["radius"];
self.mp = mp;
self.leg = leg;
self.hexa = hexa;
self.step_type = 0;
self.started = False;
self.frames = params["frames"] * 2;
self.phase_group = phase_group;
self.phase_group_c = round(1/(1-phase_group_max));
self.liftoff_frame = round(self.frames * phase_group_max);
self.step_pattern = None;
self.step_height = params["height"];
self.fr = 0;
self.direction = leg.getDirection();
self.step_ee_pos = None;
# Set the leg center position:
lx,ly,lz = self.leg.getWorldPositionAnchor().xyz;
cx, cy = params["center"];
cx = float(cx * self.direction);
cy = float(cy);
#self.center = Vector3([float(lx + cx), float(ly + cy), 0.]);
self.center = self.leg.getWorldPositionAnchor();
# Calculate the direction for rotation:
# Assuming pure clockwise rotation, what angle would this particular leg
# have to move at to turn in place?
# As it happens, this is quite easy:
displ = Quaternion.from_z_rotation(-math.pi/2) * leg.ref.getTranslationBase();
rotx = displ.x;
roty = displ.y;
rotr = (rotx**2 + roty**2)**0.5
self.rotation_vec = Vector3([rotx/rotr, roty/rotr, 0]);
def rotate_right(self):
rotation = Quaternion.from_y_rotation(- 2 * float (self._rotate_horizontally) * np.pi / 180)
self._camera_front = rotation * self._camera_front
self.build_look_at()
def __init__(self, parent_ref, config):
irot, itrans = config.getInitialPose();
self.ref = ReferenceFrame(Vector3(itrans), Quaternion.from_eulers(irot), parent_ref);
self.legs = [];
def pitch(self, value: float) -> None:
"""Sets the myx rotation."""
self._pitch = clip(value, -pi * 0.4999, pi * 0.4999)
self.dir = (Quaternion.from_axis_rotation(self.ure, self._yaw) *
Quaternion.from_axis_rotation(self.xre, self._pitch) * self.zre)
def setRotation(self, rotate):
self.rotate_raw = rotate
self.rotate = self.base_rotate * Quaternion.from_eulers(rotate)
def yaw(self, value: float) -> None:
"""Sets the yref rotation."""
self._yaw = value % (2 * pi)
self.dir = (Quaternion.from_axis_rotation(self.ure, self._yaw) *
Quaternion.from_axis_rotation(self.xre, self._pitch) * self.zre)
def pitya(self, pit: float, ya: float) -> None:
"""Sets the pitch and yaw simultaneously, saves a second round of vector normalisation?"""
self._pitch = clip(pit, -pi * 0.4999, pi * 0.4999)
self._yaw = ya % (2 * pi)
self.dir = (Quaternion.from_axis_rotation(self.ure, self._yaw) *
Quaternion.from_axis_rotation(self.xre, self._pitch) * self.zre)
