def toQuaternion(mg):
mg.Normalize()
v1, v2, v3 = mg.v1, mg.v2, mg.v3
mat = np.array([[v1.x, v1.y, v1.z], [v2.x, v2.y, v2.z], [v3.x, v3.y,
v3.z]])
q = quaternions.mat2quat(mat)
return [q[1], q[2], -q[3], q[0]]
def toQuaternion(mg):
mg.Normalize()
v1, v2, v3 = mg.v1, mg.v2, mg.v3
mat = np.array([
[v1.x, v1.y, v1.z],
[v2.x, v2.y, v2.z],
[v3.x, v3.y, v3.z]
])
q = quaternions.mat2quat(mat)
return [q[1], q[2], -q[3], q[0]]
def rotation_mat2vec(R):
""" Rotation vector from rotation matrix `R`
Parameters
----------
R : (3,3) array-like
Rotation matrix
Returns
-------
vec : (3,) array
Rotation vector, where norm of `vec` is the angle ``theta``, and the
axis of rotation is given by ``vec / theta``
"""
ax, angle = quat2axangle(mat2quat(R))
return ax * angle
listener = tf2_ros.TransformListener(tfBuffer)
# This is where the magic happens, a query is passed to the listener for the
# /base to /head transform by means of the lookupTransform fn. The arguments are
# from "this frame" to "this frame" at "this specific time"
# (if you pass "rospy.Time(0), the fn will give you the latest available transform
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
try:
transformation = tfBuffer.lookup_transform('base', 'head', rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rate.sleep()
continue
rospy.loginfo("Translation: \n" + str(transformation.transform.translation))
rospy.loginfo("Quaternion: \n" + str(transformation.transform.rotation))
q = transformation.transform.rotation
quat = (q.x, q.y, q.z, q.w)
rot_mat = quaternions.quat2mat(quat) rospy.loginfo("Rotation matrix \n" + str(rot_mat))
H = np.eye(4)
trans = transformation.transform.translation
H[:3,:3] = rot_mat
H[:3,3] = [trans.x, trans.y, trans.z]
rospy.loginfo("Homogenous matrix: \n" + str(H))
new_quat = quaternions.mat2quat(rot_mat)
rospy.loginfo("New quaternion: " + str(new_quat))
rate.sleep()
