def generate_quaternion_rotations(
rotation_axis,
rotating_vector,
start=0,
end=360,
num=360 * 3,
endpoint=True,
):
"""
Generate quaternion rotations of a vector around and axis.
Rotates a `vector` around an `axis` for a series of angles.
Rotation is performed using `Quaternion rotation <https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation>`_;
namely, `pyquaterion.rotate <http://kieranwynn.github.io/pyquaternion/#rotation>`_.
If you use this function you should cite
`PyQuaternion package <http://kieranwynn.github.io/pyquaternion/>`_.
Parameters
----------
rotation_axis : tuple, list or array-like
The XYZ coordinates of the rotation axis. This is the axis
around which the vector will be rotated.
rotation_vector : tuple, list or array-like
The XYZ coordinates of the vector to rotate.
start : int
The starting rotation angle in degrees in the series.
Defaults to ``0``.
end : int
The final rotation angle in degrees in the series.
Defaults to ``360``.
num : int
The number of rotation steps in the angle rotation series.
This is provided by ``np.linspace(start, end, num=num)``.
Returns
-------
list of 2 unit tuples
Where the list indexes follow the progression along
``np.linspace(start, end, num=num)`` and tuple elements
are the rotation quatertion used to rotate ``rotation_vector``
and the resulting rotated vector in its unitary form.
""" # noqa: E501
vec_u = Q(vector=rotating_vector).unit
rot_u = Q(vector=rotation_axis).unit
Q_rotated_tuples = []
for angle in np.linspace(start, end, num=num, endpoint=endpoint):
qq = Q(axis=rot_u.vector, degrees=angle)
Q_rotated_tuples.append((qq, qq.rotate(vec_u)))
return Q_rotated_tuples
def interpolate(self, b):
current_model_state = self.get_model_state(model_name="piano2")
quat = Q(current_model_state.pose.orientation.x, current_model_state.pose.orientation.y,\
current_model_state.pose.orientation.z, current_model_state.pose.orientation.w)
a = SE3(current_model_state.pose.position.x, current_model_state.pose.position.y,\
current_model_state.pose.position.z, quat)
x_inc = b.X - a.X
y_inc = b.Y - a.Y
z_inc = b.Z - a.Z
mag = sqrt(x_inc * x_inc + y_inc * y_inc + z_inc * z_inc)
x_inc = x_inc / mag * inc
y_inc = y_inc / mag * inc
z_inc = z_inc / mag * inc
x = a.X
y = a.Y
z = a.Z
q = a.q
cur = a
while SE3.euclid_dist(cur, b) > inc\
or fabs(Q.sym_distance(q, b.q)) > steering_inc:
self.set_position(cur)
self.set_steering_angle(cur.q)
if SE3.euclid_dist(cur, b) > inc:
x += x_inc
y += y_inc
z += z_inc
if Q.sym_distance(q, b.q) > steering_inc:
q = Q.slerp(q, b.q, amount=steering_inc)
cur = SE3(x, y, z, q)
sleep(0.05)
self.set_state(b)
def rotmat2quat_new(self, RR):
"""function to convert a given rotation matrix to a quaternion """
trace = np.trace(RR)
q = np.zeros(4)
if RR[0][0] > trace and RR[0][0] > RR[1][1] and RR[0][0] > RR[2][2]:
q[0] = np.sqrt((1 + (2 * RR[0][0]) - trace) / 4)
q[1] = (1 / (4 * q[0])) * (RR[0][1] + RR[1][0])
q[2] = (1 / (4 * q[0])) * (RR[0][2] + RR[2][0])
q[3] = (1 / (4 * q[0])) * (RR[1][2] - RR[2][1])
elif RR[1][1] >= trace and RR[1][1] >= RR[0][0] and RR[1][1] >= RR[2][
2]:
q[1] = np.sqrt((1 + (2 * RR[1][1]) - trace) / 4)
q[0] = (1 / (4 * q[1])) * (RR[0][1] + RR[1][0])
q[2] = (1 / (4 * q[1])) * (RR[1][2] + RR[2][1])
q[3] = (1 / (4 * q[1])) * (RR[2][0] - RR[0][2])
elif RR[2][2] >= trace and RR[2][2] >= RR[0][0] and RR[2][2] >= RR[1][
1]:
q[2] = np.sqrt((1 + (2 * RR[2][2]) - trace) / 4)
q[0] = (1 / (4 * q[2])) * (RR[0][2] + RR[2][0])
q[1] = (1 / (4 * q[2])) * (RR[1][2] + RR[2][1])
q[3] = (1 / (4 * q[2])) * (RR[0][1] - RR[1][0])
else:
q[3] = np.sqrt((1 + trace) / 4)
q[0] = (1 / (4 * q[3])) * (RR[1][2] - RR[2][1])
q[1] = (1 / (4 * q[3])) * (RR[2][0] - RR[0][2])
q[2] = (1 / (4 * q[3])) * (RR[0][1] - RR[1][0])
if q[3] < 0:
q = -q
q = q / np.linalg.norm(q)
return Q(q[3], q[0], q[1], q[2])
def rotmat2quat(self, RR):
"""function to convert a given rotation matrix to a quaternion """
trace = np.trace(RR)
if trace >= 0:
S = 2.0 * np.sqrt(trace + 1)
qw = 0.25 * S
qx = (RR[2][1] - RR[1][2]) / S
qy = (RR[0][2] - RR[2][0]) / S
qz = (RR[1][0] - RR[0][1]) / S
elif RR[0][0] > RR[1][1] and RR[0][0] > RR[2][2]:
S = 2.0 * np.sqrt(1 + RR[0][0] - RR[1][1] - RR[2][2])
qw = (RR[2][1] - RR[1][2]) / S
qx = 0.25 * S
qy = (RR[1][0] + RR[0][1]) / S
qz = (RR[0][2] + RR[2][0]) / S
elif RR[1][1] > RR[2][2]:
S = 2.0 * np.sqrt(1 + RR[1][1] - RR[0][0] - RR[2][2])
qw = (RR[0][2] - RR[2][0]) / S
qx = (RR[1][0] + RR[0][1]) / S
qy = 0.25 * S
qz = (RR[2][1] + RR[1][2]) / S
else:
S = 2.0 * np.sqrt(1 + RR[2][2] - RR[0][0] - RR[1][1])
qw = (RR[1][0] - RR[0][1]) / S
qx = (RR[0][2] + RR[2][0]) / S
qy = (RR[2][1] + RR[1][2]) / S
qz = 0.25 * S
if qw < 0:
qw = -qw
qx = -qx
qy = -qy
qz = -qz
return Q(qw, qx, qy, qz)
def sort_by_minimum_Qdistances(rotation_tuples, reference_vector):
"""
Sort a list of quaternion rotation tuples.
By its distance to a ``reference_vector``. Designed to receive
the output of :py:func:`generate_quaternion_rotations`.
If you use this function you should cite
`PyQuaternion package <http://kieranwynn.github.io/pyquaternion/>`_.
Parameters
----------
rotation_tuples : list of tuples
List of tuples containing Rotation quaternions and resulting
rotated vector. In other words, the output from
:py:func:`generate_quaternion_rotations`.
reference_vector : tuple, list or array-like
The XYZ 3D coordinates of the reference vector. The quaternion
distance between vectors in ``rotation_tuples`` and this
vector will be computed.
Returns
-------
list
The list sorted according to the creterion.
"""
ref_u = Q(vector=reference_vector).unit
minimum = sorted(
rotation_tuples,
key=lambda x: math.degrees(Q.distance(x[1], ref_u)) % 360,
)
return minimum
def on_disk():
t = r()
ab_weight, cd_weight = math.cos(t), math.sin(t)
t1 = r()
a, b = ab_weight * math.cos(t1), ab_weight * math.sin(t1)
t2 = r()
c, d = cd_weight * math.cos(t2), cd_weight * math.sin(t2)
return Q(a, b, c, d)
def odomcallback(self, data):
""""callback that takes the state estimates from open_vins, all these values are measurements to the UKF
position velocity are of CoG in world frame, orientation"""
px = data.pose.pose.position.x
py = data.pose.pose.position.y
pz = data.pose.pose.position.z
self.X = np.array([[px], [py], [pz]])
vx = data.twist.twist.linear.x
vy = data.twist.twist.linear.y
vz = data.twist.twist.linear.z
self.V = np.array([[vx], [vy], [vz]])
q = Q(data.pose.pose.orientation.w, data.pose.pose.orientation.x,
data.pose.pose.orientation.y, data.pose.pose.orientation.z)
self.R = q.rotation_matrix
self.fast_propagation = True
def get_random_state(ground=False):
minX = 0
maxX = 8
minY = 0
maxY = 10
minZ = 0.317
maxZ = 3
while True:
x = random.uniform(minX, maxX)
y = random.uniform(minY, maxY)
z = random.uniform(minZ, maxZ)
q = Q.random()
if ground:
z = 0.317
q = tf.transformations.quaternion_from_euler(
0, 0, random.uniform(0, 2 * pi))
q = Q(q.item(0), q.item(1), q.item(2), q.item(3))
state = SE3(x, y, z, q)
T, R = state.get_transition_rotation()
if not pqp_client(T, R).result:
# no collision
return state
def repack(data):
x = data[0]
y = data[1]
z = data[2]
q = Q(data[3], data[4], data[5], data[6])
return SE3(x, y, z, Q(q))
def draw(self, environment, color, thickness=1):
l = self.lengthV
ln = np.sqrt(np.sum(l**2))
# Form the rotation axis by getting the cross product between the z-axis vector and the normalized length vector
rotationAxis = np.cross(np.array([0, 0, 1]), l / ln)
# get the magnitude of rotationAxis ( sin(theta) )
# normalize rotationAxis
rotationMag = np.sqrt(np.sum(rotationAxis**2))
angle = np.math.asin(rotationMag)
# Create a quaternion
if l[2] < 0:
angle = np.pi / 2 - angle
if rotationMag == 0:
angle = 0
rotationAxis_ = np.array([0, 0, 1])
else:
rotationAxis_ = rotationAxis / rotationMag
rotationQ = Q(axis=rotationAxis_, angle=angle)
# Draw two circles centered around the point and rotate them by the rotation quaternion
points = [[], []]
for i in range(20):
p = rotationQ.rotate(
np.array([
np.sin(i / 20 * 2 * 3.1415) * self.r,
np.cos(i / 20 * 2 * 3.1415) * self.r, 0
]))
points[0].append(p + self.start)
p = rotationQ.rotate(
np.array([
np.sin(i / 20 * 2 * 3.1415) * self.r,
np.cos(i / 20 * 2 * 3.1415) * self.r, ln
]))
points[1].append(p + self.start)
environment.drawMesh(points, color)
tpoints = environment.transformPoint(np.array([self.start, self.end]))
if (abs(tpoints[1][0] - tpoints[0][0]) < 0.01):
offset = np.array([self.r, 0]) * environment.zoom
elif (abs(tpoints[1][1] - tpoints[0][1]) < 0.01):
offset = np.array([0, self.r]) * environment.zoom
else:
slope = (tpoints[1][1] - tpoints[0][1]) / (tpoints[1][0] -
tpoints[0][0])
perp = -1 / slope
norm = np.sqrt(1 + 1 / slope**2)
offset = np.array([self.r / norm, perp * self.r / norm
]) * environment.zoom
l1 = np.array([tpoints[0] + offset, tpoints[1] + offset]).astype(int)
l2 = np.array([tpoints[0] - offset, tpoints[1] - offset]).astype(int)
points = [[l1[0], l1[1]], [l2[0], l2[1]]]
environment.drawMeshRaw(points, color)
The above poses are from an initial viewpoint of the camera.
We now convert to the coordinate frame of the simluated world:
The camera is at x=1, y=0, z=1.5, so it's in front of the wall where the square is,
And it's oriented with:
camera z along world -x,
camera x along y
camera y along -z
'''
from bimvee.geometry import combineTwoQuaternions
from pyquaternion import Quaternion as Q
# Use pyquaternion library to give us a quaternion starting with a rotation matrix
cameraToWorldRotMat = np.array([[0, 0, -1], [1, 0, 0], [0, -1, 0]],
dtype=np.float64)
cameraToWorldQ = Q(matrix=cameraToWorldRotMat).q
rotationNew = np.zeros_like(rotation)
for idx in range(401):
rotationNew[idx, :] = combineTwoQuaternions(rotation[idx, :],
cameraToWorldQ)
# Same business for the points
pointNew = np.zeros_like(point)
pointNew[:, 0] = -point[:, 2] + 1
pointNew[:, 1] = point[:, 0]
pointNew[:, 2] = -point[:, 1] + 1.5
dataDict['pose6q']['point'] = pointNew
def all_signs_qp(degree):
all_quats = tuple(Q(x) for x in product((-1, 0, 1), repeat=4))
return product(all_quats, repeat=degree)
def on_ball():
r = lambda: uniform(-1, 1)
q = Q(r(), r(), r(), r())
while not (q.norm <= 1):
q = Q(r(), r(), r(), r())
return q
def gaussian_random_qp(degree, number):
r = lambda: gauss(0, 10)
for _ in range(number):
yield [Q(r(), r(), r(), r()) for _ in range(degree)]
def uniform_random_qp(degree, smallest, biggest, number):
r = lambda: uniform(smallest, biggest)
for _ in range(number):
yield [Q(r(), r(), r(), r()) for _ in range(degree)]