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_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)